diff --git a/.readthedocs.yaml b/.readthedocs.yaml
index 6207066..e3a7ebf 100644
--- a/.readthedocs.yaml
+++ b/.readthedocs.yaml
@@ -24,3 +24,8 @@
 
 sphinx:
   configuration: docs/conf.py
+
+# Auxiliary formats to export to (in addition to the default HTML output).
+formats:
+  - pdf
+
diff --git a/Makefile b/Makefile
index 3ac31a5..4b96be1 100644
--- a/Makefile
+++ b/Makefile
@@ -74,37 +74,85 @@
 				${INC_DRV_DIRS_TO_CHECK}		\
 				${INC_ARM_DIRS_TO_CHECK}
 
-
 ################################################################################
 # Process build options
 ################################################################################
 
 # Verbose flag
 ifeq (${V},0)
-        Q:=@
-        ECHO:=@echo
-        CHECKCODE_ARGS	+=	--no-summary --terse
+	Q:=@
+	ECHO:=@echo
+	CHECKCODE_ARGS	+=	--no-summary --terse
 else
-        Q:=
-        ECHO:=$(ECHO_QUIET)
+	Q:=
+	ECHO:=$(ECHO_QUIET)
 endif
 
 ifneq ($(findstring s,$(filter-out --%,$(MAKEFLAGS))),)
-        Q:=@
-        ECHO:=$(ECHO_QUIET)
+	Q:=@
+	ECHO:=$(ECHO_QUIET)
 endif
 
 export Q ECHO
 
-# The cert_create tool cannot generate certificates individually, so we use the
-# target 'certificates' to create them all
-ifneq (${GENERATE_COT},0)
-        FIP_DEPS += certificates
-        FWU_FIP_DEPS += fwu_certificates
+################################################################################
+# Toolchain
+################################################################################
+
+HOSTCC			:=	gcc
+export HOSTCC
+
+CC			:=	${CROSS_COMPILE}gcc
+CPP			:=	${CROSS_COMPILE}cpp
+AS			:=	${CROSS_COMPILE}gcc
+AR			:=	${CROSS_COMPILE}ar
+LINKER			:=	${CROSS_COMPILE}ld
+OC			:=	${CROSS_COMPILE}objcopy
+OD			:=	${CROSS_COMPILE}objdump
+NM			:=	${CROSS_COMPILE}nm
+PP			:=	${CROSS_COMPILE}gcc -E
+DTC			:=	dtc
+
+# Use ${LD}.bfd instead if it exists (as absolute path or together with $PATH).
+ifneq ($(strip $(wildcard ${LD}.bfd) \
+	$(foreach dir,$(subst :, ,${PATH}),$(wildcard ${dir}/${LINKER}.bfd))),)
+LINKER			:=	${LINKER}.bfd
 endif
 
+################################################################################
+# Auxiliary tools (fiptool, cert_create, etc)
+################################################################################
+
+# Variables for use with Certificate Generation Tool
+CRTTOOLPATH		?=	tools/cert_create
+CRTTOOL			?=	${CRTTOOLPATH}/cert_create${BIN_EXT}
+
+# Variables for use with Firmware Encryption Tool
+ENCTOOLPATH		?=	tools/encrypt_fw
+ENCTOOL			?=	${ENCTOOLPATH}/encrypt_fw${BIN_EXT}
+
+# Variables for use with Firmware Image Package
+FIPTOOLPATH		?=	tools/fiptool
+FIPTOOL			?=	${FIPTOOLPATH}/fiptool${BIN_EXT}
+
+# Variables for use with sptool
+SPTOOLPATH		?=	tools/sptool
+SPTOOL			?=	${SPTOOLPATH}/sptool.py
+SP_MK_GEN		?=	${SPTOOLPATH}/sp_mk_generator.py
+
+# Variables for use with ROMLIB
+ROMLIBPATH		?=	lib/romlib
+
+# Variable for use with Python
+PYTHON			?=	python3
+
+# Variables for use with documentation build using Sphinx tool
+DOCS_PATH		?=	docs
+
+################################################################################
 # Process BRANCH_PROTECTION value and set
 # Pointer Authentication and Branch Target Identification flags
+################################################################################
 ifeq (${BRANCH_PROTECTION},0)
 	# Default value turns off all types of branch protection
 	BP_OPTION := none
@@ -129,160 +177,106 @@
 	ENABLE_BTI := 1
 else
         $(error Unknown BRANCH_PROTECTION value ${BRANCH_PROTECTION})
-endif
+endif #(BRANCH_PROTECTION)
 
+################################################################################
+# RME dependent flags configuration
+################################################################################
 # FEAT_RME
 ifeq (${ENABLE_RME},1)
-# RME doesn't support PIE
-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)
-endif
-# RME requires el2 context to be saved for now.
-CTX_INCLUDE_EL2_REGS := 1
-CTX_INCLUDE_AARCH32_REGS := 0
-ARM_ARCH_MAJOR := 8
-ARM_ARCH_MINOR := 5
-ENABLE_FEAT_ECV = 1
-ENABLE_FEAT_FGT = 1
-CTX_INCLUDE_PAUTH_REGS := 1
-# RME enables CSV2_2 extension by default.
-ENABLE_FEAT_CSV2_2 = 1
+	# RME doesn't support PIE
+	ifneq (${ENABLE_PIE},0)
+                $(error ENABLE_RME does not support PIE)
+	endif
 
-endif
+	# RME doesn't support BRBE
+	ifneq (${ENABLE_BRBE_FOR_NS},0)
+                $(error ENABLE_RME does not support BRBE.)
+	endif
 
-# USE_SPINLOCK_CAS requires AArch64 build
-ifeq (${USE_SPINLOCK_CAS},1)
-ifneq (${ARCH},aarch64)
-        $(error USE_SPINLOCK_CAS requires AArch64)
-endif
-endif
+	# RME requires AARCH64
+	ifneq (${ARCH},aarch64)
+                $(error ENABLE_RME requires AArch64)
+	endif
 
-# USE_DEBUGFS experimental feature recommended only in debug builds
-ifeq (${USE_DEBUGFS},1)
-ifeq (${DEBUG},1)
-        $(warning DEBUGFS experimental feature is enabled.)
-else
-        $(warning DEBUGFS experimental, recommended in DEBUG builds ONLY)
-endif
-endif
+	# RME requires el2 context to be saved for now.
+	CTX_INCLUDE_EL2_REGS := 1
+	CTX_INCLUDE_AARCH32_REGS := 0
+	ARM_ARCH_MAJOR := 8
+	ARM_ARCH_MINOR := 5
+	ENABLE_FEAT_ECV = 1
+	ENABLE_FEAT_FGT = 1
+	CTX_INCLUDE_PAUTH_REGS := 1
 
-ifneq (${DECRYPTION_SUPPORT},none)
-ENC_ARGS += -f ${FW_ENC_STATUS}
-ENC_ARGS += -k ${ENC_KEY}
-ENC_ARGS += -n ${ENC_NONCE}
-FIP_DEPS += enctool
-FWU_FIP_DEPS += enctool
-endif
+	# RME enables CSV2_2 extension by default.
+	ENABLE_FEAT_CSV2_2 = 1
+endif #(FEAT_RME)
 
 ################################################################################
-# Toolchain
+# Compiler Configuration based on ARCH_MAJOR and ARCH_MINOR flags
 ################################################################################
-
-HOSTCC			:=	gcc
-export HOSTCC
-
-CC			:=	${CROSS_COMPILE}gcc
-CPP			:=	${CROSS_COMPILE}cpp
-AS			:=	${CROSS_COMPILE}gcc
-AR			:=	${CROSS_COMPILE}ar
-LINKER			:=	${CROSS_COMPILE}ld
-OC			:=	${CROSS_COMPILE}objcopy
-OD			:=	${CROSS_COMPILE}objdump
-NM			:=	${CROSS_COMPILE}nm
-PP			:=	${CROSS_COMPILE}gcc -E
-DTC			:=	dtc
-
-# Use ${LD}.bfd instead if it exists (as absolute path or together with $PATH).
-ifneq ($(strip $(wildcard ${LD}.bfd) \
-	$(foreach dir,$(subst :, ,${PATH}),$(wildcard ${dir}/${LINKER}.bfd))),)
-LINKER			:=	${LINKER}.bfd
-endif
-
 ifeq (${ARM_ARCH_MAJOR},7)
-target32-directive	= 	-target arm-none-eabi
-# Will set march32-directive from platform configuration
+	target32-directive	= 	-target arm-none-eabi
+# Will set march-directive from platform configuration
 else
-target32-directive	= 	-target armv8a-none-eabi
+	target32-directive	= 	-target armv8a-none-eabi
+endif #(ARM_ARCH_MAJOR)
 
-# Set the compiler's target architecture profile based on
-# ARM_ARCH_MAJOR ARM_ARCH_MINOR options
-ifeq (${ARM_ARCH_MINOR},0)
-march32-directive	= 	-march=armv${ARM_ARCH_MAJOR}-a
-march64-directive	= 	-march=armv${ARM_ARCH_MAJOR}-a
-else
-march32-directive	= 	-march=armv${ARM_ARCH_MAJOR}.${ARM_ARCH_MINOR}-a
-march64-directive	= 	-march=armv${ARM_ARCH_MAJOR}.${ARM_ARCH_MINOR}-a
-endif
-endif
+################################################################################
+# Get Architecture Feature Modifiers
+################################################################################
+arch-features		=	${ARM_ARCH_FEATURE}
+
+####################################################
+# Enable required options for Memory Stack Tagging.
+####################################################
 
 # Memory tagging is supported in architecture Armv8.5-A AArch64 and onwards
 ifeq ($(ARCH), aarch64)
-# Check if revision is greater than or equal to 8.5
-ifeq "8.5" "$(word 1, $(sort 8.5 $(ARM_ARCH_MAJOR).$(ARM_ARCH_MINOR)))"
-mem_tag_arch_support	= 	yes
-endif
-endif
-
-# Get architecture feature modifiers
-arch-features		=	${ARM_ARCH_FEATURE}
+	# Check if revision is greater than or equal to 8.5
+	ifeq "8.5" "$(word 1, $(sort 8.5 $(ARM_ARCH_MAJOR).$(ARM_ARCH_MINOR)))"
+		mem_tag_arch_support	= 	yes
+	endif
+endif #(ARCH=aarch64)
 
-# Enable required options for memory stack tagging.
 # Currently, these options are enabled only for clang and armclang compiler.
 ifeq (${SUPPORT_STACK_MEMTAG},yes)
-ifdef mem_tag_arch_support
-# Check for armclang and clang compilers
-ifneq ( ,$(filter $(notdir $(CC)),armclang clang))
-# Add "memtag" architecture feature modifier if not specified
-ifeq ( ,$(findstring memtag,$(arch-features)))
-arch-features       	:=       $(arch-features)+memtag
-endif	# memtag
-ifeq ($(notdir $(CC)),armclang)
-TF_CFLAGS		+=	-mmemtag-stack
-else ifeq ($(notdir $(CC)),clang)
-TF_CFLAGS		+=	-fsanitize=memtag
-endif	# armclang
-endif	# armclang clang
-else
-$(error "Error: stack memory tagging is not supported for architecture \
-	${ARCH},armv${ARM_ARCH_MAJOR}.${ARM_ARCH_MINOR}-a")
-endif	# mem_tag_arch_support
-endif	# SUPPORT_STACK_MEMTAG
+	ifdef mem_tag_arch_support
+		# Check for armclang and clang compilers
+		ifneq ( ,$(filter $(notdir $(CC)),armclang clang))
+		# Add "memtag" architecture feature modifier if not specified
+			ifeq ( ,$(findstring memtag,$(arch-features)))
+				arch-features	:=	$(arch-features)+memtag
+			endif	# memtag
+			ifeq ($(notdir $(CC)),armclang)
+				TF_CFLAGS	+=	-mmemtag-stack
+			else ifeq ($(notdir $(CC)),clang)
+				TF_CFLAGS	+=	-fsanitize=memtag
+			endif	# armclang
+		endif
+	else
+                $(error "Error: stack memory tagging is not supported for  \
+                 architecture ${ARCH},armv${ARM_ARCH_MAJOR}.${ARM_ARCH_MINOR}-a")
+	endif #(mem_tag_arch_support)
+endif #(SUPPORT_STACK_MEMTAG)
 
 # Set the compiler's architecture feature modifiers
 ifneq ($(arch-features), none)
-# Strip "none+" from arch-features
-arch-features		:=	$(subst none+,,$(arch-features))
-ifeq ($(ARCH), aarch32)
-march32-directive	:=	$(march32-directive)+$(arch-features)
-else
-march64-directive	:=	$(march64-directive)+$(arch-features)
-endif
+	# Strip "none+" from arch-features
+	arch-features	:=	$(subst none+,,$(arch-features))
+	march-directive	:=	$(march-directive)+$(arch-features)
 # Print features
-$(info Arm Architecture Features specified: $(subst +, ,$(arch-features)))
-endif	# arch-features
-
-# Determine if FEAT_RNG is supported
-ENABLE_FEAT_RNG		=	$(if $(findstring rng,${arch-features}),1,0)
-
-# Determine if FEAT_SB is supported
-ENABLE_FEAT_SB		=	$(if $(findstring sb,${arch-features}),1,0)
+        $(info Arm Architecture Features specified: $(subst +, ,$(arch-features)))
+endif #(arch-features)
 
 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)
+		TF_CFLAGS_aarch32	:=	-target arm-arm-none-eabi
+		TF_CFLAGS_aarch64	:=	-target aarch64-arm-none-eabi
 		LD			:=	$(LINKER)
 	else
-		TF_CFLAGS_aarch32	=	$(target32-directive) $(march32-directive)
-		TF_CFLAGS_aarch64	:=	-target aarch64-elf $(march64-directive)
+		TF_CFLAGS_aarch32	=	$(target32-directive)
+		TF_CFLAGS_aarch64	:=	-target aarch64-elf
 		LD			:=	$(shell $(CC) --print-prog-name ld.lld)
 
 		AR			:=	$(shell $(CC) --print-prog-name llvm-ar)
@@ -294,62 +288,60 @@
 	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)
-ifeq ($(ENABLE_LTO),1)
-	# Enable LTO only for aarch64
-	ifeq (${ARCH},aarch64)
-		LTO_CFLAGS	=	-flto
-		# Use gcc as a wrapper for the ld, recommended for LTO
-		LINKER		:=	${CROSS_COMPILE}gcc
+	ifeq ($(ENABLE_LTO),1)
+		# Enable LTO only for aarch64
+		ifeq (${ARCH},aarch64)
+			LTO_CFLAGS	=	-flto
+			# Use gcc as a wrapper for the ld, recommended for LTO
+			LINKER		:=	${CROSS_COMPILE}gcc
+		endif
 	endif
-endif
-LD			=	$(LINKER)
+	LD			=	$(LINKER)
 else
-TF_CFLAGS_aarch32	=	$(march32-directive)
-TF_CFLAGS_aarch64	=	$(march64-directive)
-LD			=	$(LINKER)
-endif
+	LD			=	$(LINKER)
+endif #(clang)
 
 # Process Debug flag
 $(eval $(call add_define,DEBUG))
 ifneq (${DEBUG}, 0)
-        BUILD_TYPE	:=	debug
-        TF_CFLAGS	+=	-g -gdwarf-4
-        ASFLAGS		+=	-g -Wa,-gdwarf-4
+	BUILD_TYPE	:=	debug
+	TF_CFLAGS	+=	-g -gdwarf-4
+	ASFLAGS		+=	-g -Wa,-gdwarf-4
 
-        # Use LOG_LEVEL_INFO by default for debug builds
-        LOG_LEVEL	:=	40
+	# Use LOG_LEVEL_INFO by default for debug builds
+	LOG_LEVEL	:=	40
 else
-        BUILD_TYPE	:=	release
-        # Use LOG_LEVEL_NOTICE by default for release builds
-        LOG_LEVEL	:=	20
-endif
+	BUILD_TYPE	:=	release
+	# Use LOG_LEVEL_NOTICE by default for release builds
+	LOG_LEVEL	:=	20
+endif #(Debug)
 
 # Default build string (git branch and commit)
 ifeq (${BUILD_STRING},)
-        BUILD_STRING  :=  $(shell git describe --always --dirty --tags 2> /dev/null)
+	BUILD_STRING  :=  $(shell git describe --always --dirty --tags 2> /dev/null)
 endif
 VERSION_STRING    :=  v${VERSION}(${BUILD_TYPE}):${BUILD_STRING}
 
 ifeq (${AARCH32_INSTRUCTION_SET},A32)
-TF_CFLAGS_aarch32	+=	-marm
+	TF_CFLAGS_aarch32	+=	-marm
 else ifeq (${AARCH32_INSTRUCTION_SET},T32)
-TF_CFLAGS_aarch32	+=	-mthumb
+	TF_CFLAGS_aarch32	+=	-mthumb
 else
-$(error Error: Unknown AArch32 instruction set ${AARCH32_INSTRUCTION_SET})
-endif
+        $(error Error: Unknown AArch32 instruction set ${AARCH32_INSTRUCTION_SET})
+endif #(AARCH32_INSTRUCTION_SET)
 
 TF_CFLAGS_aarch32	+=	-mno-unaligned-access
 TF_CFLAGS_aarch64	+=	-mgeneral-regs-only -mstrict-align
 
 ifneq (${BP_OPTION},none)
-TF_CFLAGS_aarch64	+=	-mbranch-protection=${BP_OPTION}
-endif
+	TF_CFLAGS_aarch64	+=	-mbranch-protection=${BP_OPTION}
+endif #(BP_OPTION)
 
-ASFLAGS_aarch32		=	$(march32-directive)
-ASFLAGS_aarch64		=	$(march64-directive)
+ASFLAGS		+=	$(march-directive)
 
+##############################################################################
+# WARNINGS Configuration
+###############################################################################
 # General warnings
 WARNINGS		:=	-Wall -Wmissing-include-dirs -Wunused	\
 				-Wdisabled-optimization -Wvla -Wshadow	\
@@ -395,18 +387,18 @@
 # outside of the contributor's control. Don't fail the build on them so warnings
 # can be seen and hopefully addressed
 ifdef W
-ifneq (${W},0)
-E	 ?= 0
-endif
+	ifneq (${W},0)
+		E	 ?= 0
+	endif
 endif
 
 ifeq (${W},1)
-WARNINGS += $(WARNING1)
+	WARNINGS += $(WARNING1)
 else ifeq (${W},2)
-WARNINGS += $(WARNING1) $(WARNING2)
+	WARNINGS += $(WARNING1) $(WARNING2)
 else ifeq (${W},3)
-WARNINGS += $(WARNING1) $(WARNING2) $(WARNING3)
-endif
+	WARNINGS += $(WARNING1) $(WARNING2) $(WARNING3)
+endif #(W)
 
 # Compiler specific warnings
 ifeq ($(findstring clang,$(notdir $(CC))),)
@@ -422,30 +414,32 @@
 # using clang
 WARNINGS	+=		-Wshift-overflow -Wshift-sign-overflow \
 				-Wlogical-op-parentheses
-endif
+endif #(Clang Warning)
 
 ifneq (${E},0)
-ERRORS := -Werror
-endif
+	ERRORS := -Werror
+endif #(E)
 
+################################################################################
+# Compiler and Linker Directives
+################################################################################
 CPPFLAGS		=	${DEFINES} ${INCLUDES} ${MBEDTLS_INC} -nostdinc	\
 				$(ERRORS) $(WARNINGS)
-ASFLAGS			+=	$(CPPFLAGS) $(ASFLAGS_$(ARCH))			\
+ASFLAGS			+=	$(CPPFLAGS)                 			\
 				-ffreestanding -Wa,--fatal-warnings
 TF_CFLAGS		+=	$(CPPFLAGS) $(TF_CFLAGS_$(ARCH))		\
 				-ffunction-sections -fdata-sections		\
 				-ffreestanding -fno-builtin -fno-common		\
 				-Os -std=gnu99
 
-$(eval $(call add_define,SVE_VECTOR_LEN))
-
 ifeq (${SANITIZE_UB},on)
-TF_CFLAGS		+=	-fsanitize=undefined -fno-sanitize-recover
-endif
+	TF_CFLAGS	+=	-fsanitize=undefined -fno-sanitize-recover
+endif #(${SANITIZE_UB},on)
+
 ifeq (${SANITIZE_UB},trap)
-TF_CFLAGS		+=	-fsanitize=undefined -fno-sanitize-recover	\
+	TF_CFLAGS	+=	-fsanitize=undefined -fno-sanitize-recover	\
 				-fsanitize-undefined-trap-on-error
-endif
+endif #(${SANITIZE_UB},trap)
 
 GCC_V_OUTPUT		:=	$(shell $(CC) -v 2>&1)
 
@@ -453,51 +447,53 @@
 
 # LD = armlink
 ifneq ($(findstring armlink,$(notdir $(LD))),)
-TF_LDFLAGS		+=	--diag_error=warning --lto_level=O1
-TF_LDFLAGS		+=	--remove --info=unused,unusedsymbols
-TF_LDFLAGS		+=	$(TF_LDFLAGS_$(ARCH))
+	TF_LDFLAGS		+=	--diag_error=warning --lto_level=O1
+	TF_LDFLAGS		+=	--remove --info=unused,unusedsymbols
+	TF_LDFLAGS		+=	$(TF_LDFLAGS_$(ARCH))
 
 # LD = gcc (used when GCC LTO is enabled)
 else ifneq ($(findstring gcc,$(notdir $(LD))),)
-# Pass ld options with Wl or Xlinker switches
-TF_LDFLAGS		+=	-Wl,--fatal-warnings -O1
-TF_LDFLAGS		+=	-Wl,--gc-sections
+	# Pass ld options with Wl or Xlinker switches
+	TF_LDFLAGS		+=	-Wl,--fatal-warnings -O1
+	TF_LDFLAGS		+=	-Wl,--gc-sections
 
-TF_LDFLAGS		+=	-Wl,-z,common-page-size=4096 # Configure page size constants
-TF_LDFLAGS		+=	-Wl,-z,max-page-size=4096
+	TF_LDFLAGS		+=	-Wl,-z,common-page-size=4096 #Configure page size constants
+	TF_LDFLAGS		+=	-Wl,-z,max-page-size=4096
 
-ifeq ($(ENABLE_LTO),1)
-	ifeq (${ARCH},aarch64)
-		TF_LDFLAGS	+=	-flto -fuse-linker-plugin
-	endif
-endif
+	ifeq ($(ENABLE_LTO),1)
+		ifeq (${ARCH},aarch64)
+			TF_LDFLAGS	+=	-flto -fuse-linker-plugin
+		endif
+	endif #(ENABLE_LTO)
+
 # GCC automatically adds fix-cortex-a53-843419 flag when used to link
 # which breaks some builds, so disable if errata fix is not explicitly enabled
-ifneq (${ERRATA_A53_843419},1)
-	TF_LDFLAGS	+= 	-mno-fix-cortex-a53-843419
-endif
-TF_LDFLAGS		+= 	-nostdlib
-TF_LDFLAGS		+=	$(subst --,-Xlinker --,$(TF_LDFLAGS_$(ARCH)))
+	ifneq (${ERRATA_A53_843419},1)
+		TF_LDFLAGS	+= 	-mno-fix-cortex-a53-843419
+	endif
+	TF_LDFLAGS		+= 	-nostdlib
+	TF_LDFLAGS		+=	$(subst --,-Xlinker --,$(TF_LDFLAGS_$(ARCH)))
 
 # LD = gcc-ld (ld) or llvm-ld (ld.lld) or other
 else
 # With ld.bfd version 2.39 and newer new warnings are added. Skip those since we
 # are not loaded by a elf loader.
-TF_LDFLAGS		+=	$(call ld_option, --no-warn-rwx-segments)
-TF_LDFLAGS		+=	-O1
-TF_LDFLAGS		+=	--gc-sections
+	TF_LDFLAGS		+=	$(call ld_option, --no-warn-rwx-segments)
+	TF_LDFLAGS		+=	-O1
+	TF_LDFLAGS		+=	--gc-sections
 
-TF_LDFLAGS		+=	-z common-page-size=4096 # Configure page size constants
-TF_LDFLAGS		+=	-z max-page-size=4096
+	TF_LDFLAGS		+=	-z common-page-size=4096 # Configure page size constants
+	TF_LDFLAGS		+=	-z max-page-size=4096
 
 # ld.lld doesn't recognize the errata flags,
 # therefore don't add those in that case.
 # ld.lld reports section type mismatch warnings,
 # therefore don't add --fatal-warnings to it.
-ifeq ($(findstring ld.lld,$(notdir $(LD))),)
-TF_LDFLAGS		+=	$(TF_LDFLAGS_$(ARCH)) --fatal-warnings
-endif
-endif
+	ifeq ($(findstring ld.lld,$(notdir $(LD))),)
+		TF_LDFLAGS	+=	$(TF_LDFLAGS_$(ARCH)) --fatal-warnings
+	endif
+
+endif #(LD = armlink)
 
 DTC_FLAGS		+=	-I dts -O dtb
 DTC_CPPFLAGS		+=	-P -nostdinc -Iinclude -Ifdts -undef \
@@ -515,6 +511,7 @@
 				drivers/console/multi_console.c		\
 				lib/${ARCH}/cache_helpers.S		\
 				lib/${ARCH}/misc_helpers.S		\
+				lib/extensions/pmuv3/${ARCH}/pmuv3.c	\
 				plat/common/plat_bl_common.c		\
 				plat/common/plat_log_common.c		\
 				plat/common/${ARCH}/plat_common.c	\
@@ -525,15 +522,15 @@
 ifeq (${ENABLE_PAUTH}, 1)
 # arm/common/aarch64/arm_pauth.c contains a sample platform hook to complete the
 # Pauth support. As it's not secure, it must be reimplemented for real platforms
-BL_COMMON_SOURCES	+=	lib/extensions/pauth/pauth_helpers.S
+	BL_COMMON_SOURCES	+=	lib/extensions/pauth/pauth_helpers.S
 endif
 
 ifeq ($(notdir $(CC)),armclang)
-BL_COMMON_SOURCES	+=	lib/${ARCH}/armclang_printf.S
+	BL_COMMON_SOURCES	+=	lib/${ARCH}/armclang_printf.S
 endif
 
 ifeq (${SANITIZE_UB},on)
-BL_COMMON_SOURCES	+=	plat/common/ubsan.c
+	BL_COMMON_SOURCES	+=	plat/common/ubsan.c
 endif
 
 INCLUDES		+=	-Iinclude				\
@@ -548,7 +545,6 @@
 ################################################################################
 # Generic definitions
 ################################################################################
-
 include ${MAKE_HELPERS_DIRECTORY}plat_helpers.mk
 
 ifeq (${BUILD_BASE},)
@@ -561,96 +557,97 @@
 # Platforms providing their own TBB makefile may override this value
 INCLUDE_TBBR_MK		:=	1
 
-
 ################################################################################
 # Include SPD Makefile if one has been specified
 ################################################################################
 
 ifneq (${SPD},none)
-    ifeq (${ARCH},aarch32)
-        $(error "Error: SPD is incompatible with AArch32.")
-    endif
+	ifeq (${ARCH},aarch32)
+                $(error "Error: SPD is incompatible with AArch32.")
+	endif
 
-    ifdef EL3_PAYLOAD_BASE
-        $(warning "SPD and EL3_PAYLOAD_BASE are incompatible build options.")
-        $(warning "The SPD and its BL32 companion will be present but ignored.")
-    endif
+	ifdef EL3_PAYLOAD_BASE
+                $(warning "SPD and EL3_PAYLOAD_BASE are incompatible build options.")
+                $(warning "The SPD and its BL32 companion will be present but \
+                ignored.")
+	endif
 
-    ifeq (${SPD},spmd)
-        # SPMD is located in std_svc directory
-        SPD_DIR := std_svc
+	ifeq (${SPD},spmd)
+	# SPMD is located in std_svc directory
+		SPD_DIR := std_svc
 
-        ifeq ($(SPMD_SPM_AT_SEL2),1)
-            CTX_INCLUDE_EL2_REGS := 1
-	    ifeq ($(SPMC_AT_EL3),1)
-                $(error SPM cannot be enabled in both S-EL2 and EL3.)
-            endif
-        endif
+		ifeq ($(SPMD_SPM_AT_SEL2),1)
+			CTX_INCLUDE_EL2_REGS := 1
+			ifeq ($(SPMC_AT_EL3),1)
+                                $(error SPM cannot be enabled in both S-EL2 and EL3.)
+			endif
+		endif
 
-        ifeq ($(findstring optee_sp,$(ARM_SPMC_MANIFEST_DTS)),optee_sp)
-            DTC_CPPFLAGS	+=	-DOPTEE_SP_FW_CONFIG
-        endif
+		ifeq ($(findstring optee_sp,$(ARM_SPMC_MANIFEST_DTS)),optee_sp)
+			DTC_CPPFLAGS	+=	-DOPTEE_SP_FW_CONFIG
+		endif
 
-        ifeq ($(TS_SP_FW_CONFIG),1)
-            DTC_CPPFLAGS	+=	-DTS_SP_FW_CONFIG
-        endif
+		ifeq ($(TS_SP_FW_CONFIG),1)
+		DTC_CPPFLAGS	+=	-DTS_SP_FW_CONFIG
+		endif
 
-        ifneq ($(ARM_BL2_SP_LIST_DTS),)
-            DTC_CPPFLAGS += -DARM_BL2_SP_LIST_DTS=$(ARM_BL2_SP_LIST_DTS)
-        endif
+		ifneq ($(ARM_BL2_SP_LIST_DTS),)
+		DTC_CPPFLAGS += -DARM_BL2_SP_LIST_DTS=$(ARM_BL2_SP_LIST_DTS)
+		endif
 
-        ifneq ($(SP_LAYOUT_FILE),)
-            BL2_ENABLE_SP_LOAD := 1
-        endif
-    else
-        # All other SPDs in spd directory
-        SPD_DIR := spd
-    endif
+		ifneq ($(SP_LAYOUT_FILE),)
+		BL2_ENABLE_SP_LOAD := 1
+		endif
+	else
+		# All other SPDs in spd directory
+		SPD_DIR := spd
+	endif #(SPD)
 
-    # We expect to locate an spd.mk under the specified SPD directory
-    SPD_MAKE	:=	$(wildcard services/${SPD_DIR}/${SPD}/${SPD}.mk)
+	# We expect to locate an spd.mk under the specified SPD directory
+	SPD_MAKE	:=	$(wildcard services/${SPD_DIR}/${SPD}/${SPD}.mk)
 
-    ifeq (${SPD_MAKE},)
-        $(error Error: No services/${SPD_DIR}/${SPD}/${SPD}.mk located)
-    endif
-    $(info Including ${SPD_MAKE})
-    include ${SPD_MAKE}
+	ifeq (${SPD_MAKE},)
+                $(error Error: No services/${SPD_DIR}/${SPD}/${SPD}.mk located)
+	endif
+        $(info Including ${SPD_MAKE})
+        include ${SPD_MAKE}
 
-    # If there's BL32 companion for the chosen SPD, we expect that the SPD's
-    # Makefile would set NEED_BL32 to "yes". In this case, the build system
-    # supports two mutually exclusive options:
-    # * BL32 is built from source: then BL32_SOURCES must contain the list
-    #   of source files to build BL32
-    # * BL32 is a prebuilt binary: then BL32 must point to the image file
-    #   that will be included in the FIP
-    # If both BL32_SOURCES and BL32 are defined, the binary takes precedence
-    # over the sources.
-endif
+	# If there's BL32 companion for the chosen SPD, we expect that the SPD's
+	# Makefile would set NEED_BL32 to "yes". In this case, the build system
+	# supports two mutually exclusive options:
+	# * BL32 is built from source: then BL32_SOURCES must contain the list
+	#   of source files to build BL32
+	# * BL32 is a prebuilt binary: then BL32 must point to the image file
+	#   that will be included in the FIP
+	# If both BL32_SOURCES and BL32 are defined, the binary takes precedence
+	# over the sources.
+endif #(SPD=none)
 
 ifeq (${CTX_INCLUDE_EL2_REGS}, 1)
-ifeq (${SPD},none)
-ifeq (${ENABLE_RME},0)
-    $(error CTX_INCLUDE_EL2_REGS is available only when SPD or RME is enabled)
-endif
-endif
+	ifeq (${SPD},none)
+		ifeq (${ENABLE_RME},0)
+                        $(error CTX_INCLUDE_EL2_REGS is available only when SPD \
+                        or RME is enabled)
+		endif
+	endif
 endif
 
 ################################################################################
 # Include rmmd Makefile if RME is enabled
 ################################################################################
-
 ifneq (${ENABLE_RME},0)
-ifneq (${ARCH},aarch64)
-	$(error ENABLE_RME requires AArch64)
-endif
-ifeq ($(SPMC_AT_EL3),1)
-	$(error SPMC_AT_EL3 and ENABLE_RME cannot both be enabled.)
-endif
-ifneq (${SPD}, none)
-ifneq (${SPD}, spmd)
-       $(error ENABLE_RME is incompatible with SPD=${SPD}. Use SPD=spmd)
-endif
-endif
+	ifneq (${ARCH},aarch64)
+                $(error ENABLE_RME requires AArch64)
+	endif
+	ifeq ($(SPMC_AT_EL3),1)
+                $(error SPMC_AT_EL3 and ENABLE_RME cannot both be enabled.)
+	endif
+
+	ifneq (${SPD}, none)
+		ifneq (${SPD}, spmd)
+                        $(error ENABLE_RME is incompatible with SPD=${SPD}. Use SPD=spmd)
+		endif
+	endif
 include services/std_svc/rmmd/rmmd.mk
 $(warning "RME is an experimental feature")
 endif
@@ -662,6 +659,14 @@
 
 include ${PLAT_MAKEFILE_FULL}
 
+################################################################################
+# Platform specific Makefile might provide us ARCH_MAJOR/MINOR use that to come
+# up with appropriate march values for compiler.
+################################################################################
+include ${MAKE_HELPERS_DIRECTORY}march.mk
+
+TF_CFLAGS   +=	$(march-directive)
+
 # This internal flag is common option which is set to 1 for scenarios
 # when the BL2 is running in EL3 level. This occurs in two scenarios -
 # 4 world system running BL2 at EL3 and two world system without BL1 running
@@ -669,9 +674,10 @@
 
 ifeq (${RESET_TO_BL2},1)
 	BL2_RUNS_AT_EL3	:=	1
-    ifeq (${ENABLE_RME},1)
-        $(error RESET_TO_BL2=1 and ENABLE_RME=1 configuration is not supported at the moment.)
-    endif
+	ifeq (${ENABLE_RME},1)
+                $(error RESET_TO_BL2=1 and ENABLE_RME=1 configuration is not \
+                supported at the moment.)
+	endif
 else ifeq (${ENABLE_RME},1)
 	BL2_RUNS_AT_EL3	:=	1
 else
@@ -690,7 +696,7 @@
 ifneq ($(findstring gcc,$(notdir $(LD))),)
 	TF_LDFLAGS	+=	-no-pie
 endif
-endif
+endif #(PIE_FOUND)
 
 ifneq ($(findstring gcc,$(notdir $(LD))),)
 	PIE_LDFLAGS	+=	-Wl,-pie -Wl,--no-dynamic-linker
@@ -699,13 +705,13 @@
 endif
 
 ifeq ($(ENABLE_PIE),1)
-ifeq ($(RESET_TO_BL2),1)
-ifneq ($(BL2_IN_XIP_MEM),1)
-	BL2_CPPFLAGS	+=	-fpie
-	BL2_CFLAGS	+=	-fpie
-	BL2_LDFLAGS	+=	$(PIE_LDFLAGS)
-endif
-endif
+	ifeq ($(RESET_TO_BL2),1)
+		ifneq ($(BL2_IN_XIP_MEM),1)
+			BL2_CPPFLAGS	+=	-fpie
+			BL2_CFLAGS	+=	-fpie
+			BL2_LDFLAGS	+=	$(PIE_LDFLAGS)
+		endif #(BL2_IN_XIP_MEM)
+	endif #(RESET_TO_BL2)
 	BL31_CPPFLAGS	+=	-fpie
 	BL31_CFLAGS 	+=	-fpie
 	BL31_LDFLAGS	+=	$(PIE_LDFLAGS)
@@ -713,7 +719,7 @@
 	BL32_CPPFLAGS	+=	-fpie
 	BL32_CFLAGS	+=	-fpie
 	BL32_LDFLAGS	+=	$(PIE_LDFLAGS)
-endif
+endif #(ENABLE_PIE)
 
 BL1_CPPFLAGS  += -DREPORT_ERRATA=${DEBUG}
 BL31_CPPFLAGS += -DREPORT_ERRATA=${DEBUG}
@@ -721,17 +727,17 @@
 
 BL1_CPPFLAGS += -DIMAGE_AT_EL3
 ifeq ($(RESET_TO_BL2),1)
-BL2_CPPFLAGS += -DIMAGE_AT_EL3
+	BL2_CPPFLAGS += -DIMAGE_AT_EL3
 else
-BL2_CPPFLAGS += -DIMAGE_AT_EL1
-endif
+	BL2_CPPFLAGS += -DIMAGE_AT_EL1
+endif #(RESET_TO_BL2)
 
 ifeq (${ARCH},aarch64)
-BL2U_CPPFLAGS += -DIMAGE_AT_EL1
-BL31_CPPFLAGS += -DIMAGE_AT_EL3
-BL32_CPPFLAGS += -DIMAGE_AT_EL1
+	BL2U_CPPFLAGS += -DIMAGE_AT_EL1
+	BL31_CPPFLAGS += -DIMAGE_AT_EL3
+	BL32_CPPFLAGS += -DIMAGE_AT_EL1
 else
-BL32_CPPFLAGS += -DIMAGE_AT_EL3
+	BL32_CPPFLAGS += -DIMAGE_AT_EL3
 endif
 
 # Include the CPU specific operations makefile, which provides default
@@ -739,25 +745,23 @@
 # This can be overridden by the platform.
 include lib/cpus/cpu-ops.mk
 
-ifeq (${ARCH},aarch32)
-NEED_BL32 := yes
-
 ################################################################################
 # Build `AARCH32_SP` as BL32 image for AArch32
 ################################################################################
-ifneq (${AARCH32_SP},none)
-# We expect to locate an sp.mk under the specified AARCH32_SP directory
-AARCH32_SP_MAKE	:=	$(wildcard bl32/${AARCH32_SP}/${AARCH32_SP}.mk)
-
-ifeq (${AARCH32_SP_MAKE},)
-  $(error Error: No bl32/${AARCH32_SP}/${AARCH32_SP}.mk located)
-endif
+ifeq (${ARCH},aarch32)
+        NEED_BL32 := yes
 
-$(info Including ${AARCH32_SP_MAKE})
-include ${AARCH32_SP_MAKE}
-endif
+        ifneq (${AARCH32_SP},none)
+        # We expect to locate an sp.mk under the specified AARCH32_SP directory
+		AARCH32_SP_MAKE	:=	$(wildcard bl32/${AARCH32_SP}/${AARCH32_SP}.mk)
 
-endif
+                ifeq (${AARCH32_SP_MAKE},)
+                        $(error Error: No bl32/${AARCH32_SP}/${AARCH32_SP}.mk located)
+                endif
+        $(info Including ${AARCH32_SP_MAKE})
+        include ${AARCH32_SP_MAKE}
+        endif
+endif #(ARCH=aarch32)
 
 ################################################################################
 # Include libc if not overridden
@@ -767,226 +771,272 @@
 endif
 
 ################################################################################
-# Check incompatible options
+# Check incompatible options and dependencies
 ################################################################################
 
-ifdef EL3_PAYLOAD_BASE
-        ifdef PRELOADED_BL33_BASE
-                $(warning "PRELOADED_BL33_BASE and EL3_PAYLOAD_BASE are \
-                incompatible build options. EL3_PAYLOAD_BASE has priority.")
-        endif
-        ifneq (${GENERATE_COT},0)
-                $(error "GENERATE_COT and EL3_PAYLOAD_BASE are incompatible build options.")
+# USE_DEBUGFS experimental feature recommended only in debug builds
+ifeq (${USE_DEBUGFS},1)
+        ifeq (${DEBUG},1)
+                $(warning DEBUGFS experimental feature is enabled.)
+        else
+                $(warning DEBUGFS experimental, recommended in DEBUG builds ONLY)
         endif
-        ifneq (${TRUSTED_BOARD_BOOT},0)
-                $(error "TRUSTED_BOARD_BOOT and EL3_PAYLOAD_BASE are incompatible build options.")
+endif #(USE_DEBUGFS)
+
+# USE_SPINLOCK_CAS requires AArch64 build
+ifeq (${USE_SPINLOCK_CAS},1)
+        ifneq (${ARCH},aarch64)
+               $(error USE_SPINLOCK_CAS requires AArch64)
         endif
+endif #(USE_SPINLOCK_CAS)
+
+# The cert_create tool cannot generate certificates individually, so we use the
+# target 'certificates' to create them all
+ifneq (${GENERATE_COT},0)
+        FIP_DEPS += certificates
+        FWU_FIP_DEPS += fwu_certificates
 endif
 
+ifneq (${DECRYPTION_SUPPORT},none)
+	ENC_ARGS += -f ${FW_ENC_STATUS}
+	ENC_ARGS += -k ${ENC_KEY}
+	ENC_ARGS += -n ${ENC_NONCE}
+	FIP_DEPS += enctool
+	FWU_FIP_DEPS += enctool
+endif #(DECRYPTION_SUPPORT)
+
+ifdef EL3_PAYLOAD_BASE
+	ifdef PRELOADED_BL33_BASE
+                $(warning "PRELOADED_BL33_BASE and EL3_PAYLOAD_BASE are \
+		incompatible build options. EL3_PAYLOAD_BASE has priority.")
+	endif
+	ifneq (${GENERATE_COT},0)
+                $(error "GENERATE_COT and EL3_PAYLOAD_BASE are incompatible \
+                build options.")
+	endif
+	ifneq (${TRUSTED_BOARD_BOOT},0)
+                $(error "TRUSTED_BOARD_BOOT and EL3_PAYLOAD_BASE are \
+                incompatible \ build options.")
+	endif
+endif #(EL3_PAYLOAD_BASE)
+
 ifeq (${NEED_BL33},yes)
-        ifdef EL3_PAYLOAD_BASE
+	ifdef EL3_PAYLOAD_BASE
                 $(warning "BL33 image is not needed when option \
                 BL33_PAYLOAD_BASE is used and won't be added to the FIP file.")
-        endif
-        ifdef PRELOADED_BL33_BASE
+	endif
+	ifdef PRELOADED_BL33_BASE
                 $(warning "BL33 image is not needed when option \
-                PRELOADED_BL33_BASE is used and won't be added to the FIP \
-                file.")
-        endif
-endif
+                PRELOADED_BL33_BASE is used and won't be added to the FIP file.")
+	endif
+endif #(NEED_BL33)
 
 # 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.
 ifeq ($(HW_ASSISTED_COHERENCY)-$(USE_COHERENT_MEM),1-1)
-$(error USE_COHERENT_MEM cannot be enabled with HW_ASSISTED_COHERENCY)
+        $(error USE_COHERENT_MEM cannot be enabled with HW_ASSISTED_COHERENCY)
 endif
 
 #For now, BL2_IN_XIP_MEM is only supported when RESET_TO_BL2 is 1.
 ifeq ($(RESET_TO_BL2)-$(BL2_IN_XIP_MEM),0-1)
-$(error "BL2_IN_XIP_MEM is only supported when RESET_TO_BL2 is enabled")
+        $(error "BL2_IN_XIP_MEM is only supported when RESET_TO_BL2 is enabled")
 endif
 
 # RAS_EXTENSION is deprecated, provide alternate build options
 ifeq ($(RAS_EXTENSION),1)
-    $(error "RAS_EXTENSION is now deprecated, please use ENABLE_FEAT_RAS and RAS_FFH_SUPPORT instead")
+        $(error "RAS_EXTENSION is now deprecated, please use ENABLE_FEAT_RAS \
+        and RAS_FFH_SUPPORT instead")
 endif
+
 # RAS firmware first handling requires that EAs are handled in EL3 first
 ifeq ($(RAS_FFH_SUPPORT),1)
-    ifneq ($(ENABLE_FEAT_RAS),1)
-        $(error For RAS_FFH_SUPPORT, ENABLE_FEAT_RAS must also be 1)
-    endif
-    ifneq ($(HANDLE_EA_EL3_FIRST_NS),1)
-        $(error For RAS_FFH_SUPPORT, HANDLE_EA_EL3_FIRST_NS must also be 1)
-    endif
-endif
+	ifneq ($(ENABLE_FEAT_RAS),1)
+                $(error For RAS_FFH_SUPPORT, ENABLE_FEAT_RAS must also be 1)
+	endif
+	ifneq ($(HANDLE_EA_EL3_FIRST_NS),1)
+                $(error For RAS_FFH_SUPPORT, HANDLE_EA_EL3_FIRST_NS must also be 1)
+	endif
+endif #(RAS_FFH_SUPPORT)
+
 # When FAULT_INJECTION_SUPPORT is used, require that FEAT_RAS is enabled
 ifeq ($(FAULT_INJECTION_SUPPORT),1)
-    ifeq ($(ENABLE_FEAT_RAS),0)
-        $(error For FAULT_INJECTION_SUPPORT, ENABLE_FEAT_RAS must not be 0)
-    endif
-endif
+	ifeq ($(ENABLE_FEAT_RAS),0)
+                $(error For FAULT_INJECTION_SUPPORT, ENABLE_FEAT_RAS must not be 0)
+	endif
+endif #(FAULT_INJECTION_SUPPORT)
 
 # DYN_DISABLE_AUTH can be set only when TRUSTED_BOARD_BOOT=1
 ifeq ($(DYN_DISABLE_AUTH), 1)
-    ifeq (${TRUSTED_BOARD_BOOT}, 0)
-        $(error "TRUSTED_BOARD_BOOT must be enabled for DYN_DISABLE_AUTH to be set.")
-    endif
-endif
+	ifeq (${TRUSTED_BOARD_BOOT}, 0)
+                $(error "TRUSTED_BOARD_BOOT must be enabled for DYN_DISABLE_AUTH \
+                to be set.")
+	endif
+endif #(DYN_DISABLE_AUTH)
 
 ifeq ($(MEASURED_BOOT)-$(TRUSTED_BOARD_BOOT),1-1)
 # Support authentication verification and hash calculation
-    CRYPTO_SUPPORT := 3
+	CRYPTO_SUPPORT := 3
 else ifeq ($(DRTM_SUPPORT)-$(TRUSTED_BOARD_BOOT),1-1)
 # Support authentication verification and hash calculation
-    CRYPTO_SUPPORT := 3
+	CRYPTO_SUPPORT := 3
 else ifneq ($(filter 1,${MEASURED_BOOT} ${DRTM_SUPPORT}),)
 # Support hash calculation only
-    CRYPTO_SUPPORT := 2
+	CRYPTO_SUPPORT := 2
 else ifeq (${TRUSTED_BOARD_BOOT},1)
 # Support authentication verification only
-    CRYPTO_SUPPORT := 1
+	CRYPTO_SUPPORT := 1
 else
-    CRYPTO_SUPPORT := 0
-endif
+	CRYPTO_SUPPORT := 0
+endif #($(MEASURED_BOOT)-$(TRUSTED_BOARD_BOOT))
 
 # SDEI_IN_FCONF is only supported when SDEI_SUPPORT is enabled.
 ifeq ($(SDEI_SUPPORT)-$(SDEI_IN_FCONF),0-1)
-$(error "SDEI_IN_FCONF is only supported when SDEI_SUPPORT is enabled")
+        $(error "SDEI_IN_FCONF is only supported when SDEI_SUPPORT is enabled")
 endif
 
 # If pointer authentication is used in the firmware, make sure that all the
 # registers associated to it are also saved and restored.
 # Not doing it would leak the value of the keys used by EL3 to EL1 and S-EL1.
 ifeq ($(ENABLE_PAUTH),1)
-    ifeq ($(CTX_INCLUDE_PAUTH_REGS),0)
-        $(error Pointer Authentication requires CTX_INCLUDE_PAUTH_REGS=1)
-    endif
-endif
+	ifeq ($(CTX_INCLUDE_PAUTH_REGS),0)
+                $(error Pointer Authentication requires CTX_INCLUDE_PAUTH_REGS=1)
+	endif
+endif #(ENABLE_PAUTH)
 
 ifeq ($(CTX_INCLUDE_PAUTH_REGS),1)
-    ifneq (${ARCH},aarch64)
-        $(error CTX_INCLUDE_PAUTH_REGS requires AArch64)
-    endif
-endif
+	ifneq (${ARCH},aarch64)
+                $(error CTX_INCLUDE_PAUTH_REGS requires AArch64)
+	endif
+endif #(CTX_INCLUDE_PAUTH_REGS)
 
 ifeq ($(CTX_INCLUDE_MTE_REGS),1)
-    ifneq (${ARCH},aarch64)
-        $(error CTX_INCLUDE_MTE_REGS requires AArch64)
-    endif
-endif
+	ifneq (${ARCH},aarch64)
+                $(error CTX_INCLUDE_MTE_REGS requires AArch64)
+	endif
+endif #(CTX_INCLUDE_MTE_REGS)
 
 ifeq ($(PSA_FWU_SUPPORT),1)
-    $(info PSA_FWU_SUPPORT is an experimental feature)
-endif
+        $(info PSA_FWU_SUPPORT is an experimental feature)
+endif #(PSA_FWU_SUPPORT)
 
 ifeq ($(FEATURE_DETECTION),1)
-    $(info FEATURE_DETECTION is an experimental feature)
-endif
+        $(info FEATURE_DETECTION is an experimental feature)
+endif #(FEATURE_DETECTION)
 
 ifneq ($(ENABLE_SME2_FOR_NS), 0)
-    ifeq (${ENABLE_SME_FOR_NS}, 0)
-        $(warning "ENABLE_SME2_FOR_NS requires ENABLE_SME_FOR_NS also to be set")
-        $(warning "Forced ENABLE_SME_FOR_NS=1")
-        override ENABLE_SME_FOR_NS	:= 1
-    endif
-endif
+	ifeq (${ENABLE_SME_FOR_NS}, 0)
+                $(warning "ENABLE_SME2_FOR_NS requires ENABLE_SME_FOR_NS also \
+                to be set")
+                $(warning "Forced ENABLE_SME_FOR_NS=1")
+		override ENABLE_SME_FOR_NS	:= 1
+	endif
+endif #(ENABLE_SME2_FOR_NS)
 
 ifeq (${ARM_XLAT_TABLES_LIB_V1}, 1)
-    ifeq (${ALLOW_RO_XLAT_TABLES}, 1)
-        $(error "ALLOW_RO_XLAT_TABLES requires translation tables library v2")
-    endif
-endif
+	ifeq (${ALLOW_RO_XLAT_TABLES}, 1)
+                $(error "ALLOW_RO_XLAT_TABLES requires translation tables \
+                library v2")
+	endif
+endif #(ARM_XLAT_TABLES_LIB_V1)
 
 ifneq (${DECRYPTION_SUPPORT},none)
-    ifeq (${TRUSTED_BOARD_BOOT}, 0)
-        $(error TRUSTED_BOARD_BOOT must be enabled for DECRYPTION_SUPPORT to be set)
-    endif
-endif
+	ifeq (${TRUSTED_BOARD_BOOT}, 0)
+                $(error TRUSTED_BOARD_BOOT must be enabled for DECRYPTION_SUPPORT \
+                to be set)
+	endif
+endif #(DECRYPTION_SUPPORT)
 
 # Ensure that no Aarch64-only features are enabled in Aarch32 build
 ifeq (${ARCH},aarch32)
 
-    # SME/SVE only supported on AArch64
-    ifneq (${ENABLE_SME_FOR_NS},0)
-        $(error "ENABLE_SME_FOR_NS cannot be used with ARCH=aarch32")
-    endif
+	# SME/SVE only supported on AArch64
+	ifneq (${ENABLE_SME_FOR_NS},0)
+                $(error "ENABLE_SME_FOR_NS cannot be used with ARCH=aarch32")
+	endif
 
-    ifeq (${ENABLE_SVE_FOR_NS},1)
-        # Warning instead of error due to CI dependency on this
-        $(error "ENABLE_SVE_FOR_NS cannot be used with ARCH=aarch32")
-    endif
+	ifeq (${ENABLE_SVE_FOR_NS},1)
+		# 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
+	# BRBE is not supported in AArch32
+	ifeq (${ENABLE_BRBE_FOR_NS},1)
+                $(error "ENABLE_BRBE_FOR_NS cannot be used with ARCH=aarch32")
+	endif
 
-    # FEAT_RNG_TRAP is not supported in AArch32
-    ifeq (${ENABLE_FEAT_RNG_TRAP},1)
-        $(error "ENABLE_FEAT_RNG_TRAP cannot be used with ARCH=aarch32")
-    endif
-endif
+	# FEAT_RNG_TRAP is not supported in AArch32
+	ifeq (${ENABLE_FEAT_RNG_TRAP},1)
+                $(error "ENABLE_FEAT_RNG_TRAP cannot be used with ARCH=aarch32")
+	endif
+endif #(ARCH=aarch32)
 
 # Ensure ENABLE_RME is not used with SME
 ifeq (${ENABLE_RME},1)
-    ifneq (${ENABLE_SME_FOR_NS},0)
-        $(error "ENABLE_SME_FOR_NS cannot be used with ENABLE_RME")
-    endif
+	ifneq (${ENABLE_SME_FOR_NS},0)
+                $(error "ENABLE_SME_FOR_NS cannot be used with ENABLE_RME")
+	endif
 endif
 
 ifneq (${ENABLE_SME_FOR_NS},0)
-    ifeq (${ENABLE_SVE_FOR_NS},0)
-        $(error "ENABLE_SME_FOR_NS requires ENABLE_SVE_FOR_NS")
-    endif
-endif
+	ifeq (${ENABLE_SVE_FOR_NS},0)
+                $(error "ENABLE_SME_FOR_NS requires ENABLE_SVE_FOR_NS")
+	endif
+endif #(ENABLE_SME_FOR_NS)
 
 # Secure SME/SVE requires the non-secure component as well
 ifeq (${ENABLE_SME_FOR_SWD},1)
-    ifeq (${ENABLE_SME_FOR_NS},0)
-        $(error "ENABLE_SME_FOR_SWD requires ENABLE_SME_FOR_NS")
-    endif
-    ifeq (${ENABLE_SVE_FOR_SWD},0)
-        $(error "ENABLE_SME_FOR_SWD requires ENABLE_SVE_FOR_SWD")
-    endif
-endif
+	ifeq (${ENABLE_SME_FOR_NS},0)
+                $(error "ENABLE_SME_FOR_SWD requires ENABLE_SME_FOR_NS")
+	endif
+	ifeq (${ENABLE_SVE_FOR_SWD},0)
+                $(error "ENABLE_SME_FOR_SWD requires ENABLE_SVE_FOR_SWD")
+	endif
+endif #(ENABLE_SME_FOR_SWD)
+
 ifeq (${ENABLE_SVE_FOR_SWD},1)
-    ifeq (${ENABLE_SVE_FOR_NS},0)
-        $(error "ENABLE_SVE_FOR_SWD requires ENABLE_SVE_FOR_NS")
-    endif
-endif
+	ifeq (${ENABLE_SVE_FOR_NS},0)
+                $(error "ENABLE_SVE_FOR_SWD requires ENABLE_SVE_FOR_NS")
+	endif
+endif #(ENABLE_SVE_FOR_SWD)
 
 # SVE and SME cannot be used with CTX_INCLUDE_FPREGS since secure manager does
 # its own context management including FPU registers.
 ifeq (${CTX_INCLUDE_FPREGS},1)
-    ifneq (${ENABLE_SME_FOR_NS},0)
-        $(error "ENABLE_SME_FOR_NS cannot be used with CTX_INCLUDE_FPREGS")
-    endif
+	ifneq (${ENABLE_SME_FOR_NS},0)
+                $(error "ENABLE_SME_FOR_NS cannot be used with CTX_INCLUDE_FPREGS")
+	endif
 
-    ifeq (${ENABLE_SVE_FOR_NS},1)
-        # Warning instead of error due to CI dependency on this
-        $(warning "ENABLE_SVE_FOR_NS cannot be used with CTX_INCLUDE_FPREGS")
-        $(warning "Forced ENABLE_SVE_FOR_NS=0")
-        override ENABLE_SVE_FOR_NS	:= 0
-    endif
-endif
+	ifeq (${ENABLE_SVE_FOR_NS},1)
+		# Warning instead of error due to CI dependency on this
+                $(warning "ENABLE_SVE_FOR_NS cannot be used with CTX_INCLUDE_FPREGS")
+                $(warning "Forced ENABLE_SVE_FOR_NS=0")
+		override ENABLE_SVE_FOR_NS	:= 0
+	endif
+endif #(CTX_INCLUDE_FPREGS)
 
 ifeq ($(DRTM_SUPPORT),1)
-    $(info DRTM_SUPPORT is an experimental feature)
+        $(info DRTM_SUPPORT is an experimental feature)
 endif
 
 ifeq (${ENABLE_RME},1)
-    ifneq (${SEPARATE_CODE_AND_RODATA},1)
-        $(error `ENABLE_RME=1` requires `SEPARATE_CODE_AND_RODATA=1`)
-    endif
+	ifneq (${SEPARATE_CODE_AND_RODATA},1)
+                $(error `ENABLE_RME=1` requires `SEPARATE_CODE_AND_RODATA=1`)
+	endif
 endif
 
+# Determine if FEAT_RNG is supported
+ENABLE_FEAT_RNG		=	$(if $(findstring rng,${arch-features}),1,0)
+
+# Determine if FEAT_SB is supported
+ENABLE_FEAT_SB		=	$(if $(findstring sb,${arch-features}),1,0)
+
 ################################################################################
 # Process platform overrideable behaviour
 ################################################################################
 
 ifdef BL1_SOURCES
-NEED_BL1 := yes
-endif
+	NEED_BL1 := yes
+endif #(BL1_SOURCES)
 
 ifdef BL2_SOURCES
 	NEED_BL2 := yes
@@ -994,64 +1044,64 @@
 	# Using BL2 implies that a BL33 image also needs to be supplied for the FIP and
 	# Certificate generation tools. This flag can be overridden by the platform.
 	ifdef EL3_PAYLOAD_BASE
-                # If booting an EL3 payload there is no need for a BL33 image
-                # in the FIP file.
-                NEED_BL33		:=	no
-        else
-                ifdef PRELOADED_BL33_BASE
-                        # If booting a BL33 preloaded image there is no need of
-                        # another one in the FIP file.
-                        NEED_BL33		:=	no
-                else
-                        NEED_BL33		?=	yes
-                endif
-        endif
-endif
+		# If booting an EL3 payload there is no need for a BL33 image
+		# in the FIP file.
+		NEED_BL33		:=	no
+	else
+		ifdef PRELOADED_BL33_BASE
+			# If booting a BL33 preloaded image there is no need of
+			# another one in the FIP file.
+			NEED_BL33		:=	no
+		else
+			NEED_BL33		?=	yes
+		endif
+	endif
+endif #(BL2_SOURCES)
 
 ifdef BL2U_SOURCES
-NEED_BL2U := yes
-endif
+	NEED_BL2U := yes
+endif #(BL2U_SOURCES)
 
 # If SCP_BL2 is given, we always want FIP to include it.
 ifdef SCP_BL2
-        NEED_SCP_BL2		:=	yes
-endif
+	NEED_SCP_BL2		:=	yes
+endif #(SCP_BL2)
 
 # For AArch32, BL31 is not currently supported.
 ifneq (${ARCH},aarch32)
-    ifdef BL31_SOURCES
-        # When booting an EL3 payload, there is no need to compile the BL31 image nor
-        # put it in the FIP.
-        ifndef EL3_PAYLOAD_BASE
-            NEED_BL31 := yes
-        endif
-    endif
-endif
+	ifdef BL31_SOURCES
+	# When booting an EL3 payload, there is no need to compile the BL31
+	# image nor put it in the FIP.
+		ifndef EL3_PAYLOAD_BASE
+			NEED_BL31 := yes
+		endif
+	endif
+endif #(ARCH=aarch64)
 
 # Process TBB related flags
 ifneq (${GENERATE_COT},0)
-        # Common cert_create options
-        ifneq (${CREATE_KEYS},0)
+	# Common cert_create options
+	ifneq (${CREATE_KEYS},0)
                 $(eval CRT_ARGS += -n)
                 $(eval FWU_CRT_ARGS += -n)
-                ifneq (${SAVE_KEYS},0)
+		ifneq (${SAVE_KEYS},0)
                         $(eval CRT_ARGS += -k)
                         $(eval FWU_CRT_ARGS += -k)
-                endif
-        endif
-        # Include TBBR makefile (unless the platform indicates otherwise)
-        ifeq (${INCLUDE_TBBR_MK},1)
+		endif
+	endif
+	# Include TBBR makefile (unless the platform indicates otherwise)
+	ifeq (${INCLUDE_TBBR_MK},1)
                 include make_helpers/tbbr/tbbr_tools.mk
-        endif
-endif
+	endif
+endif #(GENERATE_COT)
 
 ifneq (${FIP_ALIGN},0)
-FIP_ARGS += --align ${FIP_ALIGN}
-endif
+	FIP_ARGS += --align ${FIP_ALIGN}
+endif #(FIP_ALIGN)
 
 ifdef FDT_SOURCES
-NEED_FDT := yes
-endif
+	NEED_FDT := yes
+endif #(FDT_SOURCES)
 
 ################################################################################
 # Include libraries' Makefile that are used in all BL
@@ -1060,39 +1110,6 @@
 include lib/stack_protector/stack_protector.mk
 
 ################################################################################
-# Auxiliary tools (fiptool, cert_create, etc)
-################################################################################
-
-# Variables for use with Certificate Generation Tool
-CRTTOOLPATH		?=	tools/cert_create
-CRTTOOL			?=	${CRTTOOLPATH}/cert_create${BIN_EXT}
-
-# Variables for use with Firmware Encryption Tool
-ENCTOOLPATH		?=	tools/encrypt_fw
-ENCTOOL			?=	${ENCTOOLPATH}/encrypt_fw${BIN_EXT}
-
-# Variables for use with Firmware Image Package
-FIPTOOLPATH		?=	tools/fiptool
-FIPTOOL			?=	${FIPTOOLPATH}/fiptool${BIN_EXT}
-
-# Variables for use with sptool
-SPTOOLPATH		?=	tools/sptool
-SPTOOL			?=	${SPTOOLPATH}/sptool.py
-SP_MK_GEN		?=	${SPTOOLPATH}/sp_mk_generator.py
-
-# Variables for use with ROMLIB
-ROMLIBPATH		?=	lib/romlib
-
-# Variable for use with Python
-PYTHON			?=	python3
-
-# Variables for use with documentation build using Sphinx tool
-DOCS_PATH		?=	docs
-
-# Defination of SIMICS flag
-SIMICS_BUILD	?=	0
-
-################################################################################
 # Include BL specific makefiles
 ################################################################################
 
@@ -1116,78 +1133,77 @@
 # Build options checks
 ################################################################################
 
+# Boolean_Flags
 $(eval $(call assert_booleans,\
     $(sort \
-        ALLOW_RO_XLAT_TABLES \
-        BL2_ENABLE_SP_LOAD \
-        COLD_BOOT_SINGLE_CPU \
-        CREATE_KEYS \
-        CTX_INCLUDE_AARCH32_REGS \
-        CTX_INCLUDE_FPREGS \
-        CTX_INCLUDE_EL2_REGS \
-        DEBUG \
-        DISABLE_MTPMU \
-        DYN_DISABLE_AUTH \
-        EL3_EXCEPTION_HANDLING \
-        ENABLE_AMU_AUXILIARY_COUNTERS \
-        ENABLE_AMU_FCONF \
-        AMU_RESTRICT_COUNTERS \
-        ENABLE_ASSERTIONS \
-        ENABLE_FEAT_SB \
-        ENABLE_PIE \
-        ENABLE_PMF \
-        ENABLE_PSCI_STAT \
-        ENABLE_RUNTIME_INSTRUMENTATION \
-        ENABLE_SME_FOR_SWD \
-        ENABLE_SVE_FOR_SWD \
-        ERROR_DEPRECATED \
-        FAULT_INJECTION_SUPPORT \
-        GENERATE_COT \
-        GICV2_G0_FOR_EL3 \
-        HANDLE_EA_EL3_FIRST_NS \
-        HW_ASSISTED_COHERENCY \
-        MEASURED_BOOT \
-        DRTM_SUPPORT \
-        NS_TIMER_SWITCH \
-        OVERRIDE_LIBC \
-        PL011_GENERIC_UART \
-        PLAT_RSS_NOT_SUPPORTED \
-        PROGRAMMABLE_RESET_ADDRESS \
-        PSCI_EXTENDED_STATE_ID \
-        PSCI_OS_INIT_MODE \
-        RESET_TO_BL31 \
-        SAVE_KEYS \
-        SEPARATE_CODE_AND_RODATA \
-        SEPARATE_BL2_NOLOAD_REGION \
-        SEPARATE_NOBITS_REGION \
-        SPIN_ON_BL1_EXIT \
-        SPM_MM \
-        SPMC_AT_EL3 \
-        SPMD_SPM_AT_SEL2 \
-        TRUSTED_BOARD_BOOT \
-        USE_COHERENT_MEM \
-        USE_DEBUGFS \
-        ARM_IO_IN_DTB \
-        SDEI_IN_FCONF \
-        SEC_INT_DESC_IN_FCONF \
-        USE_ROMLIB \
-        USE_TBBR_DEFS \
-        WARMBOOT_ENABLE_DCACHE_EARLY \
-        RESET_TO_BL2 \
-        BL2_IN_XIP_MEM \
-        BL2_INV_DCACHE \
-        USE_SPINLOCK_CAS \
-        ENCRYPT_BL31 \
-        ENCRYPT_BL32 \
-        ERRATA_SPECULATIVE_AT \
-        RAS_TRAP_NS_ERR_REC_ACCESS \
-        COT_DESC_IN_DTB \
-        USE_SP804_TIMER \
-        PSA_FWU_SUPPORT \
-        ENABLE_MPMM \
-        ENABLE_MPMM_FCONF \
-        SIMICS_BUILD \
-        FEATURE_DETECTION \
+	ALLOW_RO_XLAT_TABLES \
+	BL2_ENABLE_SP_LOAD \
+	COLD_BOOT_SINGLE_CPU \
+	CREATE_KEYS \
+	CTX_INCLUDE_AARCH32_REGS \
+	CTX_INCLUDE_FPREGS \
+	CTX_INCLUDE_EL2_REGS \
+	DEBUG \
+	DYN_DISABLE_AUTH \
+	EL3_EXCEPTION_HANDLING \
+	ENABLE_AMU_AUXILIARY_COUNTERS \
+	ENABLE_AMU_FCONF \
+	AMU_RESTRICT_COUNTERS \
+	ENABLE_ASSERTIONS \
+	ENABLE_FEAT_SB \
+	ENABLE_PIE \
+	ENABLE_PMF \
+	ENABLE_PSCI_STAT \
+	ENABLE_RUNTIME_INSTRUMENTATION \
+	ENABLE_SME_FOR_SWD \
+	ENABLE_SVE_FOR_SWD \
+	ERROR_DEPRECATED \
+	FAULT_INJECTION_SUPPORT \
+	GENERATE_COT \
+	GICV2_G0_FOR_EL3 \
+	HANDLE_EA_EL3_FIRST_NS \
+	HW_ASSISTED_COHERENCY \
+	MEASURED_BOOT \
+	DRTM_SUPPORT \
+	NS_TIMER_SWITCH \
+	OVERRIDE_LIBC \
+	PL011_GENERIC_UART \
+	PLAT_RSS_NOT_SUPPORTED \
+	PROGRAMMABLE_RESET_ADDRESS \
+	PSCI_EXTENDED_STATE_ID \
+	PSCI_OS_INIT_MODE \
+	RESET_TO_BL31 \
+	SAVE_KEYS \
+	SEPARATE_CODE_AND_RODATA \
+	SEPARATE_BL2_NOLOAD_REGION \
+	SEPARATE_NOBITS_REGION \
+	SPIN_ON_BL1_EXIT \
+	SPM_MM \
+	SPMC_AT_EL3 \
+	SPMD_SPM_AT_SEL2 \
+	TRUSTED_BOARD_BOOT \
+	USE_COHERENT_MEM \
+	USE_DEBUGFS \
+	ARM_IO_IN_DTB \
+	SDEI_IN_FCONF \
+	SEC_INT_DESC_IN_FCONF \
+	USE_ROMLIB \
+	USE_TBBR_DEFS \
+	WARMBOOT_ENABLE_DCACHE_EARLY \
+	RESET_TO_BL2 \
+	BL2_IN_XIP_MEM \
+	BL2_INV_DCACHE \
+	USE_SPINLOCK_CAS \
+	ENCRYPT_BL31 \
+	ENCRYPT_BL32 \
+	ERRATA_SPECULATIVE_AT \
+	RAS_TRAP_NS_ERR_REC_ACCESS \
+	COT_DESC_IN_DTB \
+	USE_SP804_TIMER \
+	PSA_FWU_SUPPORT \
+	ENABLE_MPMM \
+	ENABLE_MPMM_FCONF \
+	FEATURE_DETECTION \
 	TRNG_SUPPORT \
 	ERRATA_ABI_SUPPORT \
 	ERRATA_NON_ARM_INTERCONNECT \
@@ -1195,52 +1211,55 @@
 	RAS_FFH_SUPPORT \
 )))
 
+# Numeric_Flags
 $(eval $(call assert_numerics,\
     $(sort \
-        ARM_ARCH_MAJOR \
-        ARM_ARCH_MINOR \
-        BRANCH_PROTECTION \
-        CTX_INCLUDE_PAUTH_REGS \
-        CTX_INCLUDE_MTE_REGS \
-        CTX_INCLUDE_NEVE_REGS \
-        CRYPTO_SUPPORT \
-        ENABLE_BRBE_FOR_NS \
-        ENABLE_TRBE_FOR_NS \
-        ENABLE_BTI \
-        ENABLE_PAUTH \
-        ENABLE_FEAT_AMU \
-        ENABLE_FEAT_AMUv1p1 \
-        ENABLE_FEAT_CSV2_2 \
-        ENABLE_FEAT_RAS	\
-        ENABLE_FEAT_DIT \
-        ENABLE_FEAT_ECV \
-        ENABLE_FEAT_FGT \
-        ENABLE_FEAT_HCX \
-        ENABLE_FEAT_PAN \
-        ENABLE_FEAT_RNG \
-        ENABLE_FEAT_RNG_TRAP \
-        ENABLE_FEAT_SEL2 \
-        ENABLE_FEAT_TCR2 \
-        ENABLE_FEAT_S2PIE \
-        ENABLE_FEAT_S1PIE \
-        ENABLE_FEAT_S2POE \
-        ENABLE_FEAT_S1POE \
-        ENABLE_FEAT_GCS \
-        ENABLE_FEAT_VHE \
-        ENABLE_MPAM_FOR_LOWER_ELS \
-        ENABLE_RME \
-        ENABLE_SPE_FOR_NS \
-        ENABLE_SYS_REG_TRACE_FOR_NS \
-        ENABLE_SME_FOR_NS \
-        ENABLE_SME2_FOR_NS \
-        ENABLE_SVE_FOR_NS \
-        ENABLE_TRF_FOR_NS \
-        FW_ENC_STATUS \
-        NR_OF_FW_BANKS \
-        NR_OF_IMAGES_IN_FW_BANK \
-        TWED_DELAY \
-        ENABLE_FEAT_TWED \
-        SVE_VECTOR_LEN \
+	ARM_ARCH_MAJOR \
+	ARM_ARCH_MINOR \
+	BRANCH_PROTECTION \
+	CTX_INCLUDE_PAUTH_REGS \
+	CTX_INCLUDE_MTE_REGS \
+	CTX_INCLUDE_NEVE_REGS \
+	CRYPTO_SUPPORT \
+	DISABLE_MTPMU \
+	ENABLE_BRBE_FOR_NS \
+	ENABLE_TRBE_FOR_NS \
+	ENABLE_BTI \
+	ENABLE_PAUTH \
+	ENABLE_FEAT_AMU \
+	ENABLE_FEAT_AMUv1p1 \
+	ENABLE_FEAT_CSV2_2 \
+	ENABLE_FEAT_RAS	\
+	ENABLE_FEAT_DIT \
+	ENABLE_FEAT_ECV \
+	ENABLE_FEAT_FGT \
+	ENABLE_FEAT_HCX \
+	ENABLE_FEAT_PAN \
+	ENABLE_FEAT_RNG \
+	ENABLE_FEAT_RNG_TRAP \
+	ENABLE_FEAT_SEL2 \
+	ENABLE_FEAT_TCR2 \
+	ENABLE_FEAT_S2PIE \
+	ENABLE_FEAT_S1PIE \
+	ENABLE_FEAT_S2POE \
+	ENABLE_FEAT_S1POE \
+	ENABLE_FEAT_GCS \
+	ENABLE_FEAT_VHE \
+	ENABLE_FEAT_MTE_PERM \
+	ENABLE_MPAM_FOR_LOWER_ELS \
+	ENABLE_RME \
+	ENABLE_SPE_FOR_NS \
+	ENABLE_SYS_REG_TRACE_FOR_NS \
+	ENABLE_SME_FOR_NS \
+	ENABLE_SME2_FOR_NS \
+	ENABLE_SVE_FOR_NS \
+	ENABLE_TRF_FOR_NS \
+	FW_ENC_STATUS \
+	NR_OF_FW_BANKS \
+	NR_OF_IMAGES_IN_FW_BANK \
+	TWED_DELAY \
+	ENABLE_FEAT_TWED \
+	SVE_VECTOR_LEN \
 	IMPDEF_SYSREG_TRAP \
 )))
 
@@ -1260,162 +1279,163 @@
 
 $(eval $(call add_defines,\
     $(sort \
-        ALLOW_RO_XLAT_TABLES \
-        ARM_ARCH_MAJOR \
-        ARM_ARCH_MINOR \
-        BL2_ENABLE_SP_LOAD \
-        COLD_BOOT_SINGLE_CPU \
-        CTX_INCLUDE_AARCH32_REGS \
-        CTX_INCLUDE_FPREGS \
-        CTX_INCLUDE_PAUTH_REGS \
-        EL3_EXCEPTION_HANDLING \
-        CTX_INCLUDE_MTE_REGS \
-        CTX_INCLUDE_EL2_REGS \
-        CTX_INCLUDE_NEVE_REGS \
-        DECRYPTION_SUPPORT_${DECRYPTION_SUPPORT} \
-        DISABLE_MTPMU \
-        ENABLE_FEAT_AMU \
-        ENABLE_AMU_AUXILIARY_COUNTERS \
-        ENABLE_AMU_FCONF \
-        AMU_RESTRICT_COUNTERS \
-        ENABLE_ASSERTIONS \
-        ENABLE_BTI \
-        ENABLE_MPAM_FOR_LOWER_ELS \
-        ENABLE_PAUTH \
-        ENABLE_PIE \
-        ENABLE_PMF \
-        ENABLE_PSCI_STAT \
-        ENABLE_RME \
-        ENABLE_RUNTIME_INSTRUMENTATION \
-        ENABLE_SME_FOR_NS \
-        ENABLE_SME2_FOR_NS \
-        ENABLE_SME_FOR_SWD \
-        ENABLE_SPE_FOR_NS \
-        ENABLE_SVE_FOR_NS \
-        ENABLE_SVE_FOR_SWD \
-        ENCRYPT_BL31 \
-        ENCRYPT_BL32 \
-        ERROR_DEPRECATED \
-        FAULT_INJECTION_SUPPORT \
-        GICV2_G0_FOR_EL3 \
-        HANDLE_EA_EL3_FIRST_NS \
-        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 \
-        PSCI_OS_INIT_MODE \
-        ENABLE_FEAT_RAS \
-        RAS_FFH_SUPPORT \
-        RESET_TO_BL31 \
-        SEPARATE_CODE_AND_RODATA \
-        SEPARATE_BL2_NOLOAD_REGION \
-        SEPARATE_NOBITS_REGION \
-        RECLAIM_INIT_CODE \
-        SPD_${SPD} \
-        SPIN_ON_BL1_EXIT \
-        SPM_MM \
-        SPMC_AT_EL3 \
-        SPMD_SPM_AT_SEL2 \
-        TRUSTED_BOARD_BOOT \
-        CRYPTO_SUPPORT \
-        TRNG_SUPPORT \
-        ERRATA_ABI_SUPPORT \
+	ALLOW_RO_XLAT_TABLES \
+	ARM_ARCH_MAJOR \
+	ARM_ARCH_MINOR \
+	BL2_ENABLE_SP_LOAD \
+	COLD_BOOT_SINGLE_CPU \
+	CTX_INCLUDE_AARCH32_REGS \
+	CTX_INCLUDE_FPREGS \
+	CTX_INCLUDE_PAUTH_REGS \
+	EL3_EXCEPTION_HANDLING \
+	CTX_INCLUDE_MTE_REGS \
+	CTX_INCLUDE_EL2_REGS \
+	CTX_INCLUDE_NEVE_REGS \
+	DECRYPTION_SUPPORT_${DECRYPTION_SUPPORT} \
+	DISABLE_MTPMU \
+	ENABLE_FEAT_AMU \
+	ENABLE_AMU_AUXILIARY_COUNTERS \
+	ENABLE_AMU_FCONF \
+	AMU_RESTRICT_COUNTERS \
+	ENABLE_ASSERTIONS \
+	ENABLE_BTI \
+	ENABLE_MPAM_FOR_LOWER_ELS \
+	ENABLE_PAUTH \
+	ENABLE_PIE \
+	ENABLE_PMF \
+	ENABLE_PSCI_STAT \
+	ENABLE_RME \
+	ENABLE_RUNTIME_INSTRUMENTATION \
+	ENABLE_SME_FOR_NS \
+	ENABLE_SME2_FOR_NS \
+	ENABLE_SME_FOR_SWD \
+	ENABLE_SPE_FOR_NS \
+	ENABLE_SVE_FOR_NS \
+	ENABLE_SVE_FOR_SWD \
+	ENCRYPT_BL31 \
+	ENCRYPT_BL32 \
+	ERROR_DEPRECATED \
+	FAULT_INJECTION_SUPPORT \
+	GICV2_G0_FOR_EL3 \
+	HANDLE_EA_EL3_FIRST_NS \
+	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 \
+	PSCI_OS_INIT_MODE \
+	ENABLE_FEAT_RAS \
+	RAS_FFH_SUPPORT \
+	RESET_TO_BL31 \
+	SEPARATE_CODE_AND_RODATA \
+	SEPARATE_BL2_NOLOAD_REGION \
+	SEPARATE_NOBITS_REGION \
+	RECLAIM_INIT_CODE \
+	SPD_${SPD} \
+	SPIN_ON_BL1_EXIT \
+	SPM_MM \
+	SPMC_AT_EL3 \
+	SPMD_SPM_AT_SEL2 \
+	TRUSTED_BOARD_BOOT \
+	CRYPTO_SUPPORT \
+	TRNG_SUPPORT \
+	ERRATA_ABI_SUPPORT \
 	ERRATA_NON_ARM_INTERCONNECT \
-        USE_COHERENT_MEM \
-        USE_DEBUGFS \
-        ARM_IO_IN_DTB \
-        SDEI_IN_FCONF \
-        SEC_INT_DESC_IN_FCONF \
-        USE_ROMLIB \
-        USE_TBBR_DEFS \
-        WARMBOOT_ENABLE_DCACHE_EARLY \
-        RESET_TO_BL2 \
-        BL2_RUNS_AT_EL3	\
-        BL2_IN_XIP_MEM \
-        BL2_INV_DCACHE \
-        USE_SPINLOCK_CAS \
-        ERRATA_SPECULATIVE_AT \
-        RAS_TRAP_NS_ERR_REC_ACCESS \
-        COT_DESC_IN_DTB \
-        USE_SP804_TIMER \
-        ENABLE_FEAT_RNG \
-        ENABLE_FEAT_RNG_TRAP \
-        ENABLE_FEAT_SB \
-        ENABLE_FEAT_DIT \
-        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 \
-        ENABLE_FEAT_HCX \
-        ENABLE_MPMM \
-        ENABLE_MPMM_FCONF \
-        ENABLE_FEAT_FGT \
-        ENABLE_FEAT_ECV \
-        SIMICS_BUILD \
-        ENABLE_FEAT_AMUv1p1 \
-        ENABLE_FEAT_SEL2 \
-        ENABLE_FEAT_VHE \
-        ENABLE_FEAT_CSV2_2 \
-        ENABLE_FEAT_PAN \
-        ENABLE_FEAT_TCR2 \
-        ENABLE_FEAT_S2PIE \
-        ENABLE_FEAT_S1PIE \
-        ENABLE_FEAT_S2POE \
-        ENABLE_FEAT_S1POE \
-        ENABLE_FEAT_GCS \
-        FEATURE_DETECTION \
-        TWED_DELAY \
-        ENABLE_FEAT_TWED \
+	USE_COHERENT_MEM \
+	USE_DEBUGFS \
+	ARM_IO_IN_DTB \
+	SDEI_IN_FCONF \
+	SEC_INT_DESC_IN_FCONF \
+	USE_ROMLIB \
+	USE_TBBR_DEFS \
+	WARMBOOT_ENABLE_DCACHE_EARLY \
+	RESET_TO_BL2 \
+	BL2_RUNS_AT_EL3	\
+	BL2_IN_XIP_MEM \
+	BL2_INV_DCACHE \
+	USE_SPINLOCK_CAS \
+	ERRATA_SPECULATIVE_AT \
+	RAS_TRAP_NS_ERR_REC_ACCESS \
+	COT_DESC_IN_DTB \
+	USE_SP804_TIMER \
+	ENABLE_FEAT_RNG \
+	ENABLE_FEAT_RNG_TRAP \
+	ENABLE_FEAT_SB \
+	ENABLE_FEAT_DIT \
+	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 \
+	ENABLE_FEAT_HCX \
+	ENABLE_MPMM \
+	ENABLE_MPMM_FCONF \
+	ENABLE_FEAT_FGT \
+	ENABLE_FEAT_ECV \
+	ENABLE_FEAT_AMUv1p1 \
+	ENABLE_FEAT_SEL2 \
+	ENABLE_FEAT_VHE \
+	ENABLE_FEAT_CSV2_2 \
+	ENABLE_FEAT_PAN \
+	ENABLE_FEAT_TCR2 \
+	ENABLE_FEAT_S2PIE \
+	ENABLE_FEAT_S1PIE \
+	ENABLE_FEAT_S2POE \
+	ENABLE_FEAT_S1POE \
+	ENABLE_FEAT_GCS \
+	ENABLE_FEAT_MTE_PERM \
+	FEATURE_DETECTION \
+	TWED_DELAY \
+	ENABLE_FEAT_TWED \
 	CONDITIONAL_CMO \
 	IMPDEF_SYSREG_TRAP \
+	SVE_VECTOR_LEN \
 )))
 
 ifeq (${SANITIZE_UB},trap)
         $(eval $(call add_define,MONITOR_TRAPS))
-endif
+endif #(SANITIZE_UB)
 
 # Define the EL3_PAYLOAD_BASE flag only if it is provided.
 ifdef EL3_PAYLOAD_BASE
         $(eval $(call add_define,EL3_PAYLOAD_BASE))
 else
-        # Define the PRELOADED_BL33_BASE flag only if it is provided and
-        # EL3_PAYLOAD_BASE is not defined, as it has priority.
-        ifdef PRELOADED_BL33_BASE
+# Define the PRELOADED_BL33_BASE flag only if it is provided and
+# EL3_PAYLOAD_BASE is not defined, as it has priority.
+	ifdef PRELOADED_BL33_BASE
                 $(eval $(call add_define,PRELOADED_BL33_BASE))
-        endif
-endif
+	endif
+endif #(EL3_PAYLOAD_BASE)
 
 # Define the DYN_DISABLE_AUTH flag only if set.
 ifeq (${DYN_DISABLE_AUTH},1)
-$(eval $(call add_define,DYN_DISABLE_AUTH))
+        $(eval $(call add_define,DYN_DISABLE_AUTH))
 endif
 
 ifneq ($(findstring armlink,$(notdir $(LD))),)
-$(eval $(call add_define,USE_ARM_LINK))
+        $(eval $(call add_define,USE_ARM_LINK))
 endif
 
 # Generate and include sp_gen.mk if SPD is spmd and SP_LAYOUT_FILE is defined
 ifeq (${SPD},spmd)
 ifdef SP_LAYOUT_FILE
-        -include $(BUILD_PLAT)/sp_gen.mk
-        FIP_DEPS += sp
-        CRT_DEPS += sp
-        NEED_SP_PKG := yes
+	-include $(BUILD_PLAT)/sp_gen.mk
+	FIP_DEPS += sp
+	CRT_DEPS += sp
+	NEED_SP_PKG := yes
 else
-        ifeq (${SPMD_SPM_AT_SEL2},1)
-            $(error "SPMD with SPM at S-EL2 require SP_LAYOUT_FILE")
-        endif
-endif
-endif
+	ifeq (${SPMD_SPM_AT_SEL2},1)
+                $(error "SPMD with SPM at S-EL2 require SP_LAYOUT_FILE")
+	endif
+endif #(SP_LAYOUT_FILE)
+endif #(SPD)
 
 ################################################################################
 # Build targets
@@ -1436,7 +1456,7 @@
 else
     CPPFLAGS		+= 	-Wno-error=deprecated-declarations -Wno-error=cpp
 endif
-endif # !ERROR_DEPRECATED
+endif #(!ERROR_DEPRECATED)
 
 $(eval $(call MAKE_LIB_DIRS))
 $(eval $(call MAKE_LIB,c))
@@ -1444,11 +1464,11 @@
 # Expand build macros for the different images
 ifeq (${NEED_BL1},yes)
 BL1_SOURCES := $(sort ${BL1_SOURCES})
-
 $(eval $(call MAKE_BL,bl1))
-endif
+endif #(NEED_BL1)
 
 ifeq (${NEED_BL2},yes)
+
 ifeq (${RESET_TO_BL2}, 0)
 FIP_BL2_ARGS := tb-fw
 endif
@@ -1457,11 +1477,12 @@
 
 $(if ${BL2}, $(eval $(call TOOL_ADD_IMG,bl2,--${FIP_BL2_ARGS})),\
 	$(eval $(call MAKE_BL,bl2,${FIP_BL2_ARGS})))
-endif
+
+endif #(NEED_BL2)
 
 ifeq (${NEED_SCP_BL2},yes)
 $(eval $(call TOOL_ADD_IMG,scp_bl2,--scp-fw))
-endif
+endif #(NEED_SCP_BL2)
 
 ifeq (${NEED_BL31},yes)
 BL31_SOURCES += ${SPD_SOURCES}
@@ -1473,8 +1494,8 @@
 else
 $(if ${BL31}, $(eval $(call TOOL_ADD_IMG,bl31,--soc-fw)),\
 	$(eval $(call MAKE_BL,bl31,soc-fw)))
-endif
-endif
+endif #(DECRYPTION_SUPPORT)
+endif #(NEED_BL31)
 
 # If a BL32 image is needed but neither BL32 nor BL32_SOURCES is defined, the
 # build system will call TOOL_ADD_IMG to print a warning message and abort the
@@ -1490,8 +1511,8 @@
 else
 $(if ${BUILD_BL32}, $(eval $(call MAKE_BL,bl32,tos-fw)),\
 	$(eval $(call TOOL_ADD_IMG,bl32,--tos-fw)))
-endif
-endif
+endif #(DECRYPTION_SUPPORT)
+endif #(NEED_BL32)
 
 # If RMM image is needed but RMM is not defined, Test Realm Payload (TRP)
 # needs to be built from RMM_SOURCES.
@@ -1501,23 +1522,23 @@
 BUILD_RMM := $(if $(RMM),,$(if $(RMM_SOURCES),1))
 
 $(if ${BUILD_RMM}, $(eval $(call MAKE_BL,rmm,rmm-fw)),\
-         $(eval $(call TOOL_ADD_IMG,rmm,--rmm-fw)))
-endif
+	 $(eval $(call TOOL_ADD_IMG,rmm,--rmm-fw)))
+endif #(NEED_RMM)
 
 # Add the BL33 image if required by the platform
 ifeq (${NEED_BL33},yes)
 $(eval $(call TOOL_ADD_IMG,bl33,--nt-fw))
-endif
+endif #(NEED_BL33)
 
 ifeq (${NEED_BL2U},yes)
 $(if ${BL2U}, $(eval $(call TOOL_ADD_IMG,bl2u,--ap-fwu-cfg,FWU_)),\
 	$(eval $(call MAKE_BL,bl2u,ap-fwu-cfg,FWU_)))
-endif
+endif #(NEED_BL2U)
 
 # Expand build macros for the different images
 ifeq (${NEED_FDT},yes)
     $(eval $(call MAKE_DTBS,$(BUILD_PLAT)/fdts,$(FDT_SOURCES)))
-endif
+endif #(NEED_FDT)
 
 # Add Secure Partition packages
 ifeq (${NEED_SP_PKG},yes)
@@ -1527,7 +1548,7 @@
 	@${ECHO_BLANK_LINE}
 	@echo "Built SP Images successfully"
 	@${ECHO_BLANK_LINE}
-endif
+endif #(NEED_SP_PKG)
 
 locate-checkpatch:
 ifndef CHECKPATCH
@@ -1536,7 +1557,7 @@
 ifeq (,$(wildcard ${CHECKPATCH}))
 	$(error "The file CHECKPATCH points to cannot be found, use eg: CHECKPATCH=../linux/scripts/checkpatch.pl")
 endif
-endif
+endif #(CHECKPATCH)
 
 clean:
 	@echo "  CLEAN"
@@ -1547,7 +1568,7 @@
 # Clear the MAKEFLAGS as we do not want
 # to pass the gnumake flags to nmake.
 	${Q}set MAKEFLAGS= && ${MSVC_NMAKE} /nologo /f ${FIPTOOLPATH}/Makefile.msvc FIPTOOLPATH=$(subst /,\,$(FIPTOOLPATH)) FIPTOOL=$(subst /,\,$(FIPTOOL)) clean
-endif
+endif #(UNIX_MK)
 	${Q}${MAKE} PLAT=${PLAT} --no-print-directory -C ${CRTTOOLPATH} clean
 	${Q}${MAKE} PLAT=${PLAT} --no-print-directory -C ${ENCTOOLPATH} clean
 	${Q}${MAKE} --no-print-directory -C ${ROMLIBPATH} clean
@@ -1562,7 +1583,7 @@
 # Clear the MAKEFLAGS as we do not want
 # 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
+endif #(UNIX_MK)
 	${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
@@ -1616,7 +1637,7 @@
 	@echo "Built $@ successfully"
 	@echo "Certificates can be found in ${BUILD_PLAT}"
 	@${ECHO_BLANK_LINE}
-endif
+endif #(GENERATE_COT)
 
 ${BUILD_PLAT}/${FIP_NAME}: ${FIP_DEPS} ${FIPTOOL}
 	$(eval ${CHECK_FIP_CMD})
@@ -1633,7 +1654,7 @@
 	@echo "Built $@ successfully"
 	@echo "FWU certificates can be found in ${BUILD_PLAT}"
 	@${ECHO_BLANK_LINE}
-endif
+endif #(GENERATE_COT)
 
 ${BUILD_PLAT}/${FWU_FIP_NAME}: ${FWU_FIP_DEPS} ${FIPTOOL}
 	$(eval ${CHECK_FWU_FIP_CMD})
@@ -1654,7 +1675,7 @@
 # Clear the MAKEFLAGS as we do not want
 # to pass the gnumake flags to nmake.
 	${Q}set MAKEFLAGS= && ${MSVC_NMAKE} /nologo /f ${FIPTOOLPATH}/Makefile.msvc FIPTOOLPATH=$(subst /,\,$(FIPTOOLPATH)) FIPTOOL=$(subst /,\,$(FIPTOOL))
-endif
+endif #(UNIX_MK)
 
 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/bl1.mk b/bl1/bl1.mk
index b1791b1..95fe50e 100644
--- a/bl1/bl1.mk
+++ b/bl1/bl1.mk
@@ -16,10 +16,6 @@
 				plat/common/${ARCH}/platform_up_stack.S \
 				${MBEDTLS_SOURCES}
 
-ifeq (${DISABLE_MTPMU},1)
-BL1_SOURCES		+=	lib/extensions/mtpmu/${ARCH}/mtpmu.S
-endif
-
 ifeq (${ARCH},aarch64)
 BL1_SOURCES		+=	lib/cpus/aarch64/dsu_helpers.S		\
 				lib/el3_runtime/aarch64/context.S
diff --git a/bl2/bl2.mk b/bl2/bl2.mk
index 19b955f..1663c52 100644
--- a/bl2/bl2.mk
+++ b/bl2/bl2.mk
@@ -43,10 +43,6 @@
 				bl2/${ARCH}/bl2_run_next_image.S        \
 				lib/cpus/${ARCH}/cpu_helpers.S
 
-ifeq (${DISABLE_MTPMU},1)
-BL2_SOURCES		+=	lib/extensions/mtpmu/${ARCH}/mtpmu.S
-endif
-
 ifeq (${ARCH},aarch64)
 BL2_SOURCES		+=	lib/cpus/aarch64/dsu_helpers.S
 endif
diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S
index 8bcf94e..7336b91 100644
--- a/bl31/aarch64/runtime_exceptions.S
+++ b/bl31/aarch64/runtime_exceptions.S
@@ -152,92 +152,6 @@
 	b	handle_lower_el_sync_ea
 	.endm
 
-
-	/* ---------------------------------------------------------------------
-	 * This macro handles FIQ or IRQ interrupts i.e. EL3, S-EL1 and NS
-	 * interrupts.
-	 * ---------------------------------------------------------------------
-	 */
-	.macro	handle_interrupt_exception label
-
-	/*
-	 * Save general purpose and ARMv8.3-PAuth registers (if enabled).
-	 * Also save PMCR_EL0 and  set the PSTATE to a known state.
-	 */
-	bl	prepare_el3_entry
-
-#if ENABLE_PAUTH
-	/* Load and program APIAKey firmware key */
-	bl	pauth_load_bl31_apiakey
-#endif
-
-	/* Save the EL3 system registers needed to return from this exception */
-	mrs	x0, spsr_el3
-	mrs	x1, elr_el3
-	stp	x0, x1, [sp, #CTX_EL3STATE_OFFSET + CTX_SPSR_EL3]
-
-	/* Switch to the runtime stack i.e. SP_EL0 */
-	ldr	x2, [sp, #CTX_EL3STATE_OFFSET + CTX_RUNTIME_SP]
-	mov	x20, sp
-	msr	spsel, #MODE_SP_EL0
-	mov	sp, x2
-
-	/*
-	 * Find out whether this is a valid interrupt type.
-	 * If the interrupt controller reports a spurious interrupt then return
-	 * to where we came from.
-	 */
-	bl	plat_ic_get_pending_interrupt_type
-	cmp	x0, #INTR_TYPE_INVAL
-	b.eq	interrupt_exit_\label
-
-	/*
-	 * Get the registered handler for this interrupt type.
-	 * A NULL return value could be 'cause of the following conditions:
-	 *
-	 * a. An interrupt of a type was routed correctly but a handler for its
-	 *    type was not registered.
-	 *
-	 * b. An interrupt of a type was not routed correctly so a handler for
-	 *    its type was not registered.
-	 *
-	 * c. An interrupt of a type was routed correctly to EL3, but was
-	 *    deasserted before its pending state could be read. Another
-	 *    interrupt of a different type pended at the same time and its
-	 *    type was reported as pending instead. However, a handler for this
-	 *    type was not registered.
-	 *
-	 * a. and b. can only happen due to a programming error. The
-	 * occurrence of c. could be beyond the control of Trusted Firmware.
-	 * It makes sense to return from this exception instead of reporting an
-	 * error.
-	 */
-	bl	get_interrupt_type_handler
-	cbz	x0, interrupt_exit_\label
-	mov	x21, x0
-
-	mov	x0, #INTR_ID_UNAVAILABLE
-
-	/* Set the current security state in the 'flags' parameter */
-	mrs	x2, scr_el3
-	ubfx	x1, x2, #0, #1
-
-	/* Restore the reference to the 'handle' i.e. SP_EL3 */
-	mov	x2, x20
-
-	/* x3 will point to a cookie (not used now) */
-	mov	x3, xzr
-
-	/* Call the interrupt type handler */
-	blr	x21
-
-interrupt_exit_\label:
-	/* Return from exception, possibly in a different security state */
-	b	el3_exit
-
-	.endm
-
-
 vector_base runtime_exceptions
 
 	/* ---------------------------------------------------------------------
@@ -342,14 +256,14 @@
 	save_x30
 	apply_at_speculative_wa
 	check_and_unmask_ea
-	handle_interrupt_exception irq_aarch64
+	b	handle_interrupt_exception
 end_vector_entry irq_aarch64
 
 vector_entry fiq_aarch64
 	save_x30
 	apply_at_speculative_wa
 	check_and_unmask_ea
-	handle_interrupt_exception fiq_aarch64
+	b 	handle_interrupt_exception
 end_vector_entry fiq_aarch64
 
 vector_entry serror_aarch64
@@ -385,14 +299,14 @@
 	save_x30
 	apply_at_speculative_wa
 	check_and_unmask_ea
-	handle_interrupt_exception irq_aarch32
+	b	handle_interrupt_exception
 end_vector_entry irq_aarch32
 
 vector_entry fiq_aarch32
 	save_x30
 	apply_at_speculative_wa
 	check_and_unmask_ea
-	handle_interrupt_exception fiq_aarch32
+	b	handle_interrupt_exception
 end_vector_entry fiq_aarch32
 
 vector_entry serror_aarch32
@@ -519,7 +433,8 @@
 	 * flags using the appropriate helper.
 	 */
 2:
-	bfi	x7, x0, #FUNCID_SVE_HINT_SHIFT, #FUNCID_SVE_HINT_MASK
+	and	x16, x0, #(FUNCID_SVE_HINT_MASK << FUNCID_SVE_HINT_SHIFT)
+	orr	x7, x7, x16
 	bic	x0, x0, #(FUNCID_SVE_HINT_MASK << FUNCID_SVE_HINT_SHIFT)
 
 	/* Get the unique owning entity number */
@@ -610,6 +525,90 @@
 endfunc sync_exception_handler
 
 	/* ---------------------------------------------------------------------
+	 * This function handles FIQ or IRQ interrupts i.e. EL3, S-EL1 and NS
+	 * interrupts.
+	 *
+	 * Note that x30 has been explicitly saved and can be used here
+	 * ---------------------------------------------------------------------
+	 */
+func handle_interrupt_exception
+	/*
+	 * Save general purpose and ARMv8.3-PAuth registers (if enabled).
+	 * Also save PMCR_EL0 and  set the PSTATE to a known state.
+	 */
+	bl	prepare_el3_entry
+
+#if ENABLE_PAUTH
+	/* Load and program APIAKey firmware key */
+	bl	pauth_load_bl31_apiakey
+#endif
+
+	/* Save the EL3 system registers needed to return from this exception */
+	mrs	x0, spsr_el3
+	mrs	x1, elr_el3
+	stp	x0, x1, [sp, #CTX_EL3STATE_OFFSET + CTX_SPSR_EL3]
+
+	/* Switch to the runtime stack i.e. SP_EL0 */
+	ldr	x2, [sp, #CTX_EL3STATE_OFFSET + CTX_RUNTIME_SP]
+	mov	x20, sp
+	msr	spsel, #MODE_SP_EL0
+	mov	sp, x2
+
+	/*
+	 * Find out whether this is a valid interrupt type.
+	 * If the interrupt controller reports a spurious interrupt then return
+	 * to where we came from.
+	 */
+	bl	plat_ic_get_pending_interrupt_type
+	cmp	x0, #INTR_TYPE_INVAL
+	b.eq	interrupt_exit
+
+	/*
+	 * Get the registered handler for this interrupt type.
+	 * A NULL return value could be 'cause of the following conditions:
+	 *
+	 * a. An interrupt of a type was routed correctly but a handler for its
+	 *    type was not registered.
+	 *
+	 * b. An interrupt of a type was not routed correctly so a handler for
+	 *    its type was not registered.
+	 *
+	 * c. An interrupt of a type was routed correctly to EL3, but was
+	 *    deasserted before its pending state could be read. Another
+	 *    interrupt of a different type pended at the same time and its
+	 *    type was reported as pending instead. However, a handler for this
+	 *    type was not registered.
+	 *
+	 * a. and b. can only happen due to a programming error. The
+	 * occurrence of c. could be beyond the control of Trusted Firmware.
+	 * It makes sense to return from this exception instead of reporting an
+	 * error.
+	 */
+	bl	get_interrupt_type_handler
+	cbz	x0, interrupt_exit
+	mov	x21, x0
+
+	mov	x0, #INTR_ID_UNAVAILABLE
+
+	/* Set the current security state in the 'flags' parameter */
+	mrs	x2, scr_el3
+	ubfx	x1, x2, #0, #1
+
+	/* Restore the reference to the 'handle' i.e. SP_EL3 */
+	mov	x2, x20
+
+	/* x3 will point to a cookie (not used now) */
+	mov	x3, xzr
+
+	/* Call the interrupt type handler */
+	blr	x21
+
+interrupt_exit:
+	/* Return from exception, possibly in a different security state */
+	b	el3_exit
+endfunc handle_interrupt_exception
+
+	/* ---------------------------------------------------------------------
 	 * The following code handles exceptions caused by BRK instructions.
 	 * Following a BRK instruction, the only real valid cause of action is
 	 * to print some information and panic, as the code that caused it is
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index d7c9a52..0c1d657 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -54,10 +54,6 @@
 				${SPMC_SOURCES}					\
 				${SPM_SOURCES}
 
-ifeq (${DISABLE_MTPMU},1)
-BL31_SOURCES		+=	lib/extensions/mtpmu/aarch64/mtpmu.S
-endif
-
 ifeq (${ENABLE_PMF}, 1)
 BL31_SOURCES		+=	lib/pmf/pmf_main.c
 endif
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index e70eb55..8f1f043 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -112,6 +112,9 @@
  ******************************************************************************/
 void bl31_main(void)
 {
+	/* Init registers that never change for the lifetime of TF-A */
+	cm_manage_extensions_el3();
+
 	NOTICE("BL31: %s\n", version_string);
 	NOTICE("BL31: %s\n", build_message);
 
diff --git a/bl32/sp_min/sp_min.mk b/bl32/sp_min/sp_min.mk
index ec75d88..0b7bc57 100644
--- a/bl32/sp_min/sp_min.mk
+++ b/bl32/sp_min/sp_min.mk
@@ -20,10 +20,6 @@
 				services/std_svc/std_svc_setup.c	\
 				${PSCI_LIB_SOURCES}
 
-ifeq (${DISABLE_MTPMU},1)
-BL32_SOURCES		+=	lib/extensions/mtpmu/aarch32/mtpmu.S
-endif
-
 ifeq (${ENABLE_PMF}, 1)
 BL32_SOURCES		+=	lib/pmf/pmf_main.c
 endif
diff --git a/bl32/tsp/tsp_ffa_main.c b/bl32/tsp/tsp_ffa_main.c
index 268d329..1c8c68f 100644
--- a/bl32/tsp/tsp_ffa_main.c
+++ b/bl32/tsp/tsp_ffa_main.c
@@ -91,7 +91,7 @@
 	smc_args_t ffa_forward_result;
 	ffa_endpoint_id16_t receiver = arg5;
 
-	ffa_forward_result = ffa_msg_send_direct_req(ffa_endpoint_source(arg1),
+	ffa_forward_result = ffa_msg_send_direct_req(tsp_id,
 						     receiver,
 						     FF_A_ECHO_MESSAGE, arg4,
 						     0, 0, 0);
diff --git a/changelog.yaml b/changelog.yaml
index f21aa16..47cfc1e 100644
--- a/changelog.yaml
+++ b/changelog.yaml
@@ -406,6 +406,13 @@
               - title: i.MX 8
                 scope: imx8
 
+              - title: i.MX 9
+                scope: imx9
+
+                subsections:
+                  - title: i.MX93
+                    scope: imx93
+
           - title: Layerscape
             scope: layerscape
 
@@ -1044,6 +1051,9 @@
           - title: TZC-380
             scope: nxp-tzc380
 
+          - title: TRDC
+            scope: imx-trdc
+
       - title: Renesas
         scope: renesas-drivers
 
diff --git a/common/feat_detect.c b/common/feat_detect.c
index 50b74d0..2aa0c5c 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -144,6 +144,14 @@
 	check_feature(ENABLE_FEAT_SB, read_feat_sb_id_field(), "SB", 1, 1);
 	check_feature(ENABLE_FEAT_CSV2_2, read_feat_csv2_id_field(),
 		      "CSV2_2", 2, 3);
+	/*
+	 * Even though the PMUv3 is an OPTIONAL feature, it is always
+	 * implemented and Arm prescribes so. So assume it will be there and do
+	 * away with a flag for it. This is used to check minor PMUv3px
+	 * revisions so that we catch them as they come along
+	 */
+	check_feature(FEAT_STATE_ALWAYS, read_feat_pmuv3_id_field(),
+		      "PMUv3", 1, ID_AA64DFR0_PMUVER_PMUV3P7);
 
 	/* v8.1 features */
 	check_feature(ENABLE_FEAT_PAN, read_feat_pan_id_field(), "PAN", 1, 3);
@@ -184,6 +192,13 @@
 	check_feature(ENABLE_FEAT_TWED, read_feat_twed_id_field(),
 		      "TWED", 1, 1);
 
+	/*
+	 * even though this is a "DISABLE" it does confusingly perform feature
+	 * enablement duties like all other flags here. Check it against the HW
+	 * feature when we intend to diverge from the default behaviour
+	 */
+	check_feature(DISABLE_MTPMU, read_feat_mtpmu_id_field(), "MTPMU", 1, 1);
+
 	/* v8.7 features */
 	check_feature(ENABLE_FEAT_HCX, read_feat_hcx_id_field(), "HCX", 1, 1);
 
@@ -198,6 +213,8 @@
 		      "S2POE", 1, 1);
 	check_feature(ENABLE_FEAT_S1POE, read_feat_s1poe_id_field(),
 		      "S1POE", 1, 1);
+	check_feature(ENABLE_FEAT_MTE_PERM, read_feat_mte_perm_id_field(),
+		      "MTE_PERM", 1, 1);
 
 	/* v9.0 features */
 	check_feature(ENABLE_BRBE_FOR_NS, read_feat_brbe_id_field(),
diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index 663859f..3de09c7 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst
@@ -196,12 +196,14 @@
 
 JTAG DCC console driver
 ^^^^^^^^^^^^^^^^^^^^^^^
-:M: Michal Simek <michal.simek@amd.com>
-:G: `michalsimek`_
-:M: Venkatesh Yadav Abbarapu <venkatesh.abbarapu@amd.com>
-:G: `venkatesh`_
-:F: drivers/arm/dcc/
-:F: include/drivers/arm/dcc.h
+:|M|: Michal Simek <michal.simek@amd.com>
+:|G|: `michalsimek`_
+:|M|: Amit Nagal <amit.nagal@amd.com>
+:|G|: `amit-nagal`_
+:|M|: Akshay Belsare <akshay.belsare@amd.com>
+:|G|: `Akshay-Belsare`_
+:|F|: drivers/arm/dcc/
+:|F|: include/drivers/arm/dcc.h
 
 Power State Coordination Interface (PSCI)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -698,7 +700,10 @@
 :|M|: Nikita Travkin <nikita@trvn.ru>
 :|G|: `TravMurav`_
 :|F|: docs/plat/qti-msm8916.rst
+:|F|: plat/qti/mdm9607/
+:|F|: plat/qti/msm8909/
 :|F|: plat/qti/msm8916/
+:|F|: plat/qti/msm8939/
 
 Raspberry Pi 3 platform port
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -795,8 +800,10 @@
 ^^^^^^^^^^^^^^^^^^^^
 :|M|: Michal Simek <michal.simek@amd.com>
 :|G|: `michalsimek`_
-:|M|: Venkatesh Yadav Abbarapu <venkatesh.abbarapu@amd.com>
-:|G|: `venkatesh`_
+:|M|: Amit Nagal <amit.nagal@amd.com>
+:|G|: `amit-nagal`_
+:|M|: Akshay Belsare <akshay.belsare@amd.com>
+:|G|: `Akshay-Belsare`_
 :|F|: docs/plat/xilinx\*
 :|F|: plat/xilinx/
 
@@ -960,7 +967,6 @@
 .. _TonyXie06: https://github.com/TonyXie06
 .. _TravMurav: https://github.com/TravMurav
 .. _vwadekar: https://github.com/vwadekar
-.. _venkatesh: https://github.com/vabbarap
 .. _Yann-lms: https://github.com/Yann-lms
 .. _manish-pandey-arm: https://github.com/manish-pandey-arm
 .. _mardyk01: https://github.com/mardyk01
@@ -994,3 +1000,5 @@
 .. _jimmy-brisson: https://github.com/theotherjimmy
 .. _ChiaweiW: https://github.com/chiaweiw
 .. _Neal-liu: https://github.com/neal-liu
+.. _amit-nagal: https://github.com/amit-nagal
+.. _Akshay-Belsare: https://github.com/Akshay-Belsare
diff --git a/docs/design/cpu-specific-build-macros.rst b/docs/design/cpu-specific-build-macros.rst
index ecbefdd..df0cfc0 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst
@@ -527,6 +527,10 @@
    IP. This needs to be enabled for revisions r0p0 and r0p1. It has been fixed
    in r0p2.
 
+-  ``ERRATA_V2_2801372``: This applies errata 2801372 workaround to Neoverse-V2
+   CPU, this affects all configurations. This needs to be enabled for revisions
+   r0p0 and r0p1. It has been fixed in r0p2.
+
 For Cortex-A710, the following errata build flags are defined :
 
 -  ``ERRATA_A710_1987031``: This applies errata 1987031 workaround to
diff --git a/docs/design/firmware-design.rst b/docs/design/firmware-design.rst
index 131cca1..879ddda 100644
--- a/docs/design/firmware-design.rst
+++ b/docs/design/firmware-design.rst
@@ -2636,19 +2636,17 @@
   architectural features up to ``ARM_ARCH_MAJOR.ARM_ARCH_MINOR`` are included
   and unconditionally enabled by TF-A build system.
 
-- Passed to compiler via "-march" option to generate binary target : Tell the
-  compiler to emit instructions upto ``ARM_ARCH_MAJOR.ARM_ARCH_MINOR``
+- ``ARM_ARCH_MAJOR`` and ``ARM_ARCH_MINOR`` are passed to a march.mk build utility
+  this will try to come up with an appropriate -march value to be passed to compiler
+  by probing the compiler and checking what's supported by the compiler and what's best
+  that can be used. But if platform provides a ``MARCH_DIRECTIVE`` then it will used
+  directly and compiler probing will be skipped.
 
 The build system requires that the platform provides a valid numeric value based on
 CPU architecture extension, otherwise it defaults to base Armv8.0-A architecture.
 Subsequent Arm Architecture versions also support extensions which were introduced
 in previous versions.
 
-**TO-DO** : Its planned to decouple the two functionalities and introduce a new macro
-for compiler usage. The requirement for this decoupling arises becasue TF-A code
-always provides support for the latest and greatest architecture features but this
-is not the case for the target compiler.
-
 .. seealso:: :ref:`Build Options`
 
 For details on the Architecture Extension and available features, please refer
@@ -2733,12 +2731,12 @@
 the toolchain  target architecture directive.
 
 Platform may choose to not define straight the toolchain target architecture
-directive by defining ``MARCH32_DIRECTIVE``.
+directive by defining ``MARCH_DIRECTIVE``.
 I.e:
 
 .. code:: make
 
-   MARCH32_DIRECTIVE := -mach=armv7-a
+   MARCH_DIRECTIVE := -march=armv7-a
 
 Code Structure
 --------------
diff --git a/docs/design_documents/index.rst b/docs/design_documents/index.rst
index d20fc58..ecc68b2 100644
--- a/docs/design_documents/index.rst
+++ b/docs/design_documents/index.rst
@@ -11,6 +11,7 @@
    drtm_poc
    rss
    psci_osi_mode
+   measured_boot
 
 --------------
 
diff --git a/docs/design_documents/measured_boot.rst b/docs/design_documents/measured_boot.rst
new file mode 100644
index 0000000..8130d7d
--- /dev/null
+++ b/docs/design_documents/measured_boot.rst
@@ -0,0 +1,212 @@
+Measured Boot Design
+====================
+
+This document briefly explains the Measured-Boot design implementation
+in |TF-A|.
+
+Introduction
+------------
+
+Measured Boot is the process of computing and securely recording hashes of code
+and critical data at each stage in the boot chain before the code/data is used.
+
+These measurements can be leveraged by other components in the system to
+implement a complete attestation system. For example, they could be used to
+enforce local attestation policies (such as releasing certain platform keys or
+not), or they could be securely sent to a remote challenger a.k.a. `verifier`
+after boot to attest to the state of the code and critical-data.
+
+Measured Boot does not authenticate the code or critical-data, but simply
+records what code/critical-data was present on the system during boot.
+
+It is assumed that BL1 is implicitly trusted (by virtue of immutability) and
+acts as the root of trust for measurement hence it is not measured.
+
+The Measured Boot implementation in TF-A supports multiple backends to securely
+store measurements mentioned below in the :ref:`Measured Boot Backends` section.
+
+Critical data
+-------------
+
+All firmware images - i.e. BLx images and their corresponding configuration
+files, if any - must be measured. In addition to that, there might be specific
+pieces of data which needs to be measured as well. These are typically different
+on each platform. They are referred to as *critical data*.
+
+Critical data for the platform can be determined using the following criteria:
+
+#. Data that influence boot flow behaviour such as -
+
+   - Configuration parameters that alter the boot flow path.
+   - Parameters that determine which firmware to load from NV-Storage to
+     SRAM/DRAM to pass the boot process successfully.
+
+#. Hardware configurations settings, debug settings and security policies
+   that need to be in a valid state for a device to maintain its security
+   posture during boot and runtime.
+#. Security-sensitive data that is being updated by hardware.
+
+Examples of Critical data:
+
+#. The list of errata workarounds being applied at reset.
+#. State of fuses such as whether an SoC is in secure mode.
+#. NV counters that determine whether firmware is up-to-date and secure.
+
+Measurement slot
+----------------
+
+The measurement slot resides in a Trusted Module and can be either a secure
+register or memory.
+The measurement slot is used to provide a method to cryptographically record
+(measure) images and critical data on a platform.
+The measurement slot update calculation, called an **extend** operation, is
+a one-way hash of all the previous measurements and the new measurement. It
+is the only way to change the slot value, thus no measurements can ever be
+removed or overwritten.
+
+.. _Measured Boot Backends:
+
+Measured Boot Backends
+----------------------
+
+The Measured Boot implementation in TF-A supports:
+
+#. Event Log
+
+   The TCG Event Log holds a record of measurements made into the Measurement
+   Slot aka PCR (Platform Configuration Register).
+
+   The `TCG EFI Protocol Specification`_ provides details on how to measure
+   components. The Arm document
+   `Arm® Server Base Security Guide`_ provides specific guidance for
+   measurements on an SBSA/SBBR server system. By considering these
+   specifications it is decided that -
+
+   #. Use PCR0 for images measurements.
+   #. Use PCR1 for Critical data measurements.
+
+   TCG has specified the architecture for the structure of this log in the
+   `TCG EFI Protocol Specification`_. The specification describes two event
+   log event records—the legacy, fixed size SHA1 structure called TCG_PCR_EVENT
+   and the variable length crypto agile structure called TCG_PCR_EVENT2. Event
+   Log driver implemented in TF-A covers later part.
+
+#. RSS
+
+   It is one of physical backend to extend the measurements. Please refer this
+   document :ref:`Runtime Security Subsystem (RSS)` for more details.
+
+Platform Interface
+------------------
+
+Every image which gets successfully loaded in memory (and authenticated, if
+trusted boot is enabled) then gets measured. In addition to that, platforms
+can measure any relevant piece of critical data at any point during the boot.
+The following diagram outlines the call sequence for Measured Boot platform
+interfaces invoked from generic code:
+
+.. image:: ../resources/diagrams/measured_boot_design.png
+
+These platform interfaces are used by BL1 and BL2 only, and are declared in
+``include/plat/common/platform.h``.
+BL31 does not load and thus does not measure any image.
+
+Responsibilities of these platform interfaces are -
+
+#. **Function : blx_plat_mboot_init()**
+
+   .. code-block:: c
+
+      void bl1_plat_mboot_init(void);
+      void bl2_plat_mboot_init(void);
+
+   Initialise all Measured Boot backends supported by the platform
+   (e.g. Event Log buffer, RSS). As these functions do not return any value,
+   the platform should deal with error management, such as logging the error
+   somewhere, or panicking the system if this is considered a fatal error.
+
+   - On the Arm FVP port -
+
+     - In BL1, this function is used to initialize the Event Log backend
+       driver, and also to write header information in the Event Log
+       buffer.
+     - In BL2, this function is used to initialize the Event Log buffer with
+       the information received from the BL1. It results in panic on
+       error.
+
+#. **Function : plat_mboot_measure_image()**
+
+   .. code-block:: c
+
+      int plat_mboot_measure_image(unsigned int image_id,
+                                   image_info_t *image_data);
+
+   - Measure the image using a hash function of the crypto module.
+
+   - Record the measurement in the corresponding backend -
+
+     - If it is Event Log backend, then record the measurement in TCG Event Log
+       format.
+     - If it is a secure crypto-processor (like RSS), then extend the designated
+       PCR (or slot) with the given measurement.
+   - This function must return 0 on success, a signed integer error code
+     otherwise.
+   - On the Arm FVP port, this function measures the given image and then
+     records that measurement in the Event Log buffer.
+     The passed id is used to retrieve information about on how to measure
+     the image (e.g. PCR number).
+
+#. **Function : blx_plat_mboot_finish()**
+
+   .. code-block:: c
+
+      void bl1_plat_mboot_finish(void);
+      void bl2_plat_mboot_finish(void);
+
+   - Do all teardown operations with respect to initialised Measured Boot backends.
+     This could be -
+
+     - Pass the Event Log details (start address and size) to Normal world or to
+       Secure World using any platform implementation way.
+     - Measure all critical data if any.
+     - As these functions do not return any value, the platform should deal with
+       error management, such as logging the error somewhere, or panicking the
+       system if this is considered a fatal error.
+
+   - On the Arm FVP port -
+
+     - In BL1, this function is used to pass the base address of
+       the Event Log buffer and its size to BL2 via tb_fw_config to extend the
+       Event Log buffer with the measurement of various images loaded by BL2.
+       It results in panic on error.
+     - In BL2, this function is used to pass the Event Log buffer information
+       (base address and size) to non-secure(BL33) and trusted OS(BL32) via
+       nt_fw and tos_fw config respectively.
+       See :ref:`DTB binding for Event Log properties` for a description of the
+       bindings used for Event Log properties.
+
+#. **Function : plat_mboot_measure_critical_data()**
+
+   .. code-block:: c
+
+      int plat_mboot_measure_critical_data(unsigned int critical_data_id,
+                                           const void *base,
+                                           size_t size);
+
+   This interface is not invoked by the generic code and it is up to the
+   platform layer to call it where appropriate.
+
+   This function measures the given critical data structure and records its
+   measurement using the Measured Boot backend driver.
+   This function must return 0 on success, a signed integer error code
+   otherwise.
+
+   In FVP, Non volatile counters get measured and recorded as Critical data
+   using the backend via this interface.
+
+--------------
+
+*Copyright (c) 2023, Arm Limited. All rights reserved.*
+
+.. _Arm® Server Base Security Guide: https://developer.arm.com/documentation/den0086/latest
+.. _TCG EFI Protocol Specification: https://trustedcomputinggroup.org/wp-content/uploads/EFI-Protocol-Specification-rev13-160330final.pdf
diff --git a/docs/design_documents/measured_boot_poc.rst b/docs/design_documents/measured_boot_poc.rst
index 7f9519e..86cf4d1 100644
--- a/docs/design_documents/measured_boot_poc.rst
+++ b/docs/design_documents/measured_boot_poc.rst
@@ -10,6 +10,8 @@
 This section focuses on the `TCG event log`_ backend, which stores measurements
 in secure memory.
 
+See details of :ref:`Measured Boot Design`.
+
 The driver also provides mechanisms to pass the Event Log to normal world if
 needed.
 
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index c8ff7d5..c410a8e 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -45,6 +45,12 @@
    compiling TF-A. Its value must be a numeric, and defaults to 0. See also,
    *Armv8 Architecture Extensions* in :ref:`Firmware Design`.
 
+-  ``ARM_BL2_SP_LIST_DTS``: Path to DTS file snippet to override the hardcoded
+   SP nodes in tb_fw_config.
+
+-  ``ARM_SPMC_MANIFEST_DTS`` : path to an alternate manifest file used as the
+   SPMC Core manifest. Valid when ``SPD=spmd`` is selected.
+
 -  ``BL2``: This is an optional build option which specifies the path to BL2
    image for the ``fip`` target. In this case, the BL2 in the TF-A will not be
    built.
@@ -207,10 +213,10 @@
    of the binary image. If set to 1, then only the ELF image is built.
    0 is the default.
 
--  ``DISABLE_MTPMU``: Boolean option to disable FEAT_MTPMU if implemented
-   (Armv8.6 onwards). Its default value is 0 to keep consistency with platforms
-   that do not implement FEAT_MTPMU. For more information on FEAT_MTPMU,
-   check the latest Arm ARM.
+-  ``DISABLE_MTPMU``: Numeric option to disable ``FEAT_MTPMU`` (Multi Threaded
+   PMU). ``FEAT_MTPMU`` is an optional feature available on Armv8.6 onwards.
+   This flag can take values 0 to 2, to align with the ``FEATURE_DETECTION``
+   mechanism. Default is ``0``.
 
 -  ``DYN_DISABLE_AUTH``: Provides the capability to dynamically disable Trusted
    Board Boot authentication at runtime. This option is meant to be enabled only
@@ -302,6 +308,13 @@
    flag can take the values 0 to 2, to align  with the ``FEATURE_DETECTION``
    mechanism. Default value is ``0``.
 
+-  ``ENABLE_FEAT_MTE_PERM``: Numeric value to enable support for
+   ``FEAT_MTE_PERM``, which introduces Allocation tag access permission to
+   memory region attributes. ``FEAT_MTE_PERM`` is a optional architectural
+   feature available from v8.9 and upwards.  This flag can take the values 0 to
+   2, to align  with the ``FEATURE_DETECTION`` mechanism. Default value is
+   ``0``.
+
 -  ``ENABLE_FEAT_PAN``: Numeric value to enable the ``FEAT_PAN`` (Privileged
    Access Never) extension. ``FEAT_PAN`` adds a bit to PSTATE, generating a
    permission fault for any privileged data access from EL1/EL2 to virtual
@@ -648,12 +661,6 @@
    invert this behavior. Lower addresses will be printed at the top and higher
    addresses at the bottom.
 
--  ``JUNO_AARCH32_EL3_RUNTIME``: This build flag enables you to execute EL3
-   runtime software in AArch32 mode, which is required to run AArch32 on Juno.
-   By default this flag is set to '0'. Enabling this flag builds BL1 and BL2 in
-   AArch64 and facilitates the loading of ``SP_MIN`` and BL33 as AArch32 executable
-   images.
-
 -  ``KEY_ALG``: This build flag enables the user to select the algorithm to be
    used for generating the PKCS keys and subsequent signing of the certificate.
    It accepts 5 values: ``rsa``, ``rsa_1_5``, ``ecdsa``, ``ecdsa-brainpool-regular``
@@ -720,6 +727,13 @@
 
    This option defaults to 0.
 
+-  ``MARCH_DIRECTIVE``: used to pass a -march option from the platform build
+   options to the compiler. An example usage:
+
+   .. code:: make
+
+      MARCH_DIRECTIVE := -march=armv8.5-a
+
 -  ``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.
@@ -733,6 +747,10 @@
    1 (do save and restore). 0 is the default. An SPD may set this to 1 if it
    wants the timer registers to be saved and restored.
 
+-  ``OPTEE_SP_FW_CONFIG``: DTC build flag to include OP-TEE as SP in
+   tb_fw_config device tree. This flag is defined only when
+   ``ARM_SPMC_MANIFEST_DTS`` manifest file name contains pattern optee_sp.
+
 -  ``OVERRIDE_LIBC``: This option allows platforms to override the default libc
    for the BL image. It can be either 0 (include) or 1 (remove). The default
    value is 0.
@@ -942,6 +960,9 @@
       When ``EL3_EXCEPTION_HANDLING`` is ``1``, ``TSP_NS_INTR_ASYNC_PREEMPT``
       must also be set to ``1``.
 
+-  ``TS_SP_FW_CONFIG``: DTC build flag to include Trusted Services (Crypto and
+   internal-trusted-storage) as SP in tb_fw_config device tree.
+
 -  ``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``
diff --git a/docs/plat/arm/arm-build-options.rst b/docs/plat/arm/arm-build-options.rst
index e7e7ee7..3179267 100644
--- a/docs/plat/arm/arm-build-options.rst
+++ b/docs/plat/arm/arm-build-options.rst
@@ -109,19 +109,6 @@
    (```ethosn.bin```). This firmware image will be included in the FIP and
    loaded at runtime.
 
--  ``ARM_SPMC_MANIFEST_DTS`` : path to an alternate manifest file used as the
-   SPMC Core manifest. Valid when ``SPD=spmd`` is selected.
-
--  ``ARM_BL2_SP_LIST_DTS``: Path to DTS file snippet to override the hardcoded
-   SP nodes in tb_fw_config.
-
--  ``OPTEE_SP_FW_CONFIG``: DTC build flag to include OP-TEE as SP in tb_fw_config
-   device tree. This flag is defined only when ``ARM_SPMC_MANIFEST_DTS`` manifest
-   file name contains pattern optee_sp.
-
--  ``TS_SP_FW_CONFIG``: DTC build flag to include Trusted Services (Crypto and
-   internal-trusted-storage) as SP in tb_fw_config device tree.
-
 -  ``ARM_GPT_SUPPORT``: Enable GPT parser to get the entry address and length of
    the various partitions present in the GPT image. This support is available
    only for the BL2 component, and it is disabled by default.
@@ -169,6 +156,21 @@
    require all the CPUs to execute the CPU specific power down sequence to
    complete a warm reboot sequence in which only the CPUs are power cycled.
 
+Arm FVP Build Options
+---------------------
+
+- ``FVP_TRUSTED_SRAM_SIZE``: Size (in kilobytes) of the Trusted SRAM region to
+  utilize when building for the FVP platform. This option defaults to 256.
+
+Arm Juno Build Options
+----------------------
+
+-  ``JUNO_AARCH32_EL3_RUNTIME``: This build flag enables you to execute EL3
+   runtime software in AArch32 mode, which is required to run AArch32 on Juno.
+   By default this flag is set to '0'. Enabling this flag builds BL1 and BL2 in
+   AArch64 and facilitates the loading of ``SP_MIN`` and BL33 as AArch32 executable
+   images.
+
 --------------
 
 .. |FIP in a GPT image| image:: ../../resources/diagrams/FIP_in_a_GPT_image.png
diff --git a/docs/plat/arm/fvp/index.rst b/docs/plat/arm/fvp/index.rst
index 42c0eda..fcfa04a 100644
--- a/docs/plat/arm/fvp/index.rst
+++ b/docs/plat/arm/fvp/index.rst
@@ -51,7 +51,6 @@
 -  ``FVP_Morello``            (Version 0.11/33)
 -  ``FVP_RD_E1_edge``         (Version 11.17/29)
 -  ``FVP_RD_V1``              (Version 11.17/29)
--  ``FVP_TC0`` (Version 11.17/18)
 -  ``FVP_TC1`` (Version 11.17/33)
 -  ``FVP_TC2`` (Version 11.18/28)
 
@@ -631,7 +630,7 @@
 
 --------------
 
-*Copyright (c) 2019-2022, Arm Limited. All rights reserved.*
+*Copyright (c) 2019-2023, Arm Limited. All rights reserved.*
 
 .. _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`_
diff --git a/docs/plat/arm/morello/index.rst b/docs/plat/arm/morello/index.rst
index b18001c..91549c0 100644
--- a/docs/plat/arm/morello/index.rst
+++ b/docs/plat/arm/morello/index.rst
@@ -12,9 +12,19 @@
 Boot Sequence
 -------------
 
-The execution begins from SCP_BL1 which loads the SCP_BL2 and starts its
-execution. SCP_BL2 powers up the AP which starts execution at AP_BL31. The AP
-then continues executing and hands off execution to Non-secure world (UEFI).
+The SCP initializes the RVBAR registers to point to the AP_BL1. Once RVBAR is
+initialized, the primary core is powered on. The primary core boots the AP_BL1.
+It performs minimum initialization necessary to load and authenticate the AP
+firmware image (the FIP image) from the AP QSPI NOR Flash Memory into the
+Trusted SRAM.
+
+AP_BL1 authenticates and loads the AP_BL2 image. AP_BL2 performs additional
+initializations, and then authenticates and loads the AP_BL31 and AP_BL33.
+AP_BL2 then transfers execution control to AP_BL31, which is the EL3 runtime
+firmware. Execution is finally handed off to AP_BL33, which is the non-secure
+world (UEFI).
+
+SCP -> AP_BL1 -> AP_BL2 -> AP_BL31 -> AP_BL33
 
 Build Procedure (TF-A only)
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -30,4 +40,4 @@
 
       make PLAT=morello all
 
-*Copyright (c) 2020, Arm Limited. All rights reserved.*
+*Copyright (c) 2020-2023, Arm Limited. All rights reserved.*
diff --git a/docs/plat/arm/tc/index.rst b/docs/plat/arm/tc/index.rst
index df1847d..c5058f5 100644
--- a/docs/plat/arm/tc/index.rst
+++ b/docs/plat/arm/tc/index.rst
@@ -17,10 +17,9 @@
 (TARGET_PLATFORM=1), TC2 (TARGET_PLATFORM=2) platforms w.r.t to TF-A
 is the CPUs supported as below:
 
--  TC0 has support for Cortex A510, Cortex A710 and Cortex X2.
--  TC1 has support for Cortex A510, Cortex Makalu and Cortex X3.
--  TC2 has support for Hayes and Hunter Arm CPUs.
-
+-  TC0 has support for Cortex A510, Cortex A710 and Cortex X2. (Note TC0 is now deprecated)
+-  TC1 has support for Cortex A510, Cortex A715 and Cortex X3.
+-  TC2 has support for Cortex A520, Cortex A720 and Cortex x4.
 
 Boot Sequence
 -------------
@@ -58,6 +57,6 @@
 
 --------------
 
-*Copyright (c) 2020-2022, Arm Limited. All rights reserved.*
+*Copyright (c) 2020-2023, Arm Limited. All rights reserved.*
 
 .. _Arm Toolchain: https://developer.arm.com/tools-and-software/open-source-software/developer-tools/gnu-toolchain/downloads
diff --git a/docs/plat/imx9.rst b/docs/plat/imx9.rst
new file mode 100644
index 0000000..4adaae3
--- /dev/null
+++ b/docs/plat/imx9.rst
@@ -0,0 +1,60 @@
+NXP i.MX 9 Series
+==================
+
+Building on the market-proven i.MX 6 and i.MX 8 series, i.MX 9 series applications
+processors bring together higher performance applications cores, an independent
+MCU-like real-time domain, Energy Flex architecture, state-of-the-art security
+with EdgeLock® secure enclave and dedicated multi-sensory data processing engines
+(graphics, image, display, audio and voice). The i.MX 9 series, part of the EdgeVerse™
+edge computing platform, integrates hardware neural processing units across many
+members of the series for acceleration of machine learning applications at the edge
+`i.MX9 Applications Processors`_.
+
+Boot Sequence
+-------------
+
+BootROM --> SPL --> BL31 --> BL33(u-boot) --> Linux kernel
+
+How to build
+------------
+
+Build Procedure
+~~~~~~~~~~~~~~~
+
+-  Prepare AARCH64 toolchain.
+
+- Get the ELE FW image from NXP linux SDK package
+
+-  Build SPL and u-boot firstly, and get binary images: u-boot-spl.bin,
+   u-boot.bin and dtb
+
+-  Build TF-A
+
+   Build bl31:
+
+   .. code:: shell
+
+       CROSS_COMPILE=aarch64-linux-gnu- make PLAT=<Target_SoC> bl31
+
+   Target_SoC should be "imx93" for i.MX93 SoC.
+
+Deploy TF-A Images
+~~~~~~~~~~~~~~~~~~
+
+TF-A binary(bl31.bin), u-boot-spl.bin u-boot.bin, ELE FW image are combined
+together to generate a binary file called flash.bin, the imx-mkimage tool is
+used to generate flash.bin, and flash.bin needs to be flashed into SD card
+with certain offset for BOOT ROM.
+
+Reference Documentation
+~~~~~~~~~~~~~~~~~~~~~~~
+
+Details on how to prepare, generate & deploy the boot image be found in following documents:
+
+- i.MX Linux User's Guide
+  `link <https://www.nxp.com/design/software/embedded-software/i-mx-software/embedded-linux-for-i-mx-applications-processors:IMXLINUX>`__
+- i.MX Linux Reference Manual
+  `link <https://www.nxp.com/design/software/embedded-software/i-mx-software/embedded-linux-for-i-mx-applications-processors:IMXLINUX>`__
+
+.. _i.MX9 Applications Processors: https://www.nxp.com/products/processors-and-microcontrollers/arm-processors/i-mx-applications-processors/i-mx-9-processors:IMX9-PROCESSORS
+
diff --git a/docs/plat/index.rst b/docs/plat/index.rst
index 188c986..fe2cc44 100644
--- a/docs/plat/index.rst
+++ b/docs/plat/index.rst
@@ -27,6 +27,7 @@
    warp7
    imx8
    imx8m
+   imx9
    nxp/index
    poplar
    qemu
diff --git a/docs/plat/qti-msm8916.rst b/docs/plat/qti-msm8916.rst
index 09a79b7..3bc121a 100644
--- a/docs/plat/qti-msm8916.rst
+++ b/docs/plat/qti-msm8916.rst
@@ -1,16 +1,27 @@
-Qualcomm Snapdragon 410 (MSM8916/APQ8016)
-=========================================
+Qualcomm MSM8916
+================
+The MSM8916 platform port in TF-A supports multiple similar Qualcomm SoCs:
 
-The `Qualcomm Snapdragon 410`_ is Qualcomm's first 64-bit SoC, released in 2014
-with four ARM Cortex-A53 cores. There are differents variants (MSM8916,
-APQ8016(E), ...) that are all very similar. A popular device based on APQ8016E
-is the `DragonBoard 410c`_ single-board computer, but the SoC is also used in
-various mid-range smartphones/tablets.
++-----------------------+----------------+-------------------+-----------------+
+| System-on-Chip (SoC)  | TF-A Platform  | Application CPU   | Supports        |
++=======================+================+===================+=================+
+| `Snapdragon 410`_     |``PLAT=msm8916``| 4x ARM Cortex-A53 | AArch64/AArch32 |
+| (MSM8x16, APQ8016(E)) |                |                   |                 |
+| (`DragonBoard 410c`_) |                |                   |                 |
++-----------------------+----------------+-------------------+-----------------+
+| `Snapdragon 615`_     |``PLAT=msm8939``| 4x ARM Cortex-A53 | AArch64/AArch32 |
+| (MSM8x39, APQ8039)    |                | 4x ARM Cortex-A53 |                 |
++-----------------------+----------------+-------------------+-----------------+
+| `Snapdragon 210`_     |``PLAT=msm8909``| 4x ARM Cortex-A7  | AArch32 only    |
+| (MSM8x09, APQ8009)    |                |                   |                 |
++-----------------------+----------------+-------------------+-----------------+
+| `Snapdragon X5 Modem`_|``PLAT=mdm9607``| 1x ARM Cortex-A7  | AArch32 only    |
+| (MDM9x07)             |                |                   |                 |
++-----------------------+----------------+-------------------+-----------------+
 
-The TF-A/BL31 port for MSM8916 provides a minimal, community-maintained
-EL3 firmware. It is primarily based on information from the public
-`Snapdragon 410E Technical Reference Manual`_ combined with a lot of
-trial and error to actually make it work.
+It provides a minimal, community-maintained EL3 firmware and PSCI implementation,
+based on information from the public `Snapdragon 410E Technical Reference Manual`_
+combined with a lot of trial and error to actually make it work.
 
 .. note::
 	Unlike the :doc:`QTI SC7180/SC7280 <qti>` ports, this port does **not**
@@ -20,8 +31,7 @@
 
 Functionality
 -------------
-
-The BL31 port is much more minimal compared to the original firmware and
+The TF-A port is much more minimal compared to the original firmware and
 therefore expects the non-secure world (e.g. Linux) to manage more hardware,
 such as the SMMUs and all remote processors (RPM, WCNSS, Venus, Modem).
 Everything except modem is currently functional with a slightly modified version
@@ -41,28 +51,77 @@
 
 Boot Flow
 ---------
-BL31 replaces the original ``tz`` firmware in the boot flow::
+BL31 (AArch64) or BL32/SP_MIN (AArch32) replaces the original ``tz`` firmware
+in the boot flow::
 
 	Boot ROM (PBL) -> SBL -> BL31 (EL3) -> U-Boot (EL2) -> Linux (EL2)
 
-By default, BL31 enters the non-secure world in EL2 AArch64 state at address
-``0x8f600000``. The original hypervisor firmware (``hyp``) is not used, you can
-use KVM or another hypervisor. The entry address is fixed in the BL31 binary
-but can be changed using the ``PRELOADED_BL33_BASE`` make file parameter.
+After initialization the normal world starts at a fixed entry address in EL2/HYP
+mode, configured using ``PRELOADED_BL33_BASE``. At runtime, it is expected that
+the normal world bootloader was already loaded into RAM by a previous firmware
+component (usually SBL) and that it is capable of running in EL2/HYP mode.
 
-Using an AArch64 bootloader (such as `U-Boot for DragonBoard 410c`_) is
-recommended. AArch32 bootloaders (such as the original Little Kernel bootloader
-from Qualcomm) are not directly supported, although it is possible to use an EL2
-shim loader to temporarily switch to AArch32 state.
+`U-Boot for DragonBoard 410c`_ is recommended if possible. The original Little
+Kernel-based bootloader from Qualcomm does not support EL2/HYP, but can be
+booted using an additional shim loader such as `tfalkstub`_.
 
-Installation
-------------
-First, setup the cross compiler for AArch64 and build TF-A for ``msm8916``::
+Build
+-----
+It is possible to build for either AArch64 or AArch32. Some platforms use 32-bit
+CPUs that only support AArch32 (see table above). For all others AArch64 is the
+preferred build option.
+
+AArch64 (BL31)
+^^^^^^^^^^^^^^
+Setup the cross compiler for AArch64 and build BL31 for one of the platforms in
+the table above::
+
+	$ make CROSS_COMPILE=aarch64-none-elf- PLAT=...
+
+The BL31 ELF image is generated in ``build/$PLAT/release/bl31/bl31.elf``.
+
+AArch32 (BL32/SP_MIN)
+^^^^^^^^^^^^^^^^^^^^^
+Setup the cross compiler for AArch32 and build BL32 with SP_MIN for one of the
+platforms in the table above::
 
-	$ make CROSS_COMPILE=aarch64-linux-gnu- PLAT=msm8916
+	$ make CROSS_COMPILE=arm-none-eabi- PLAT=... ARCH=aarch32 AARCH32_SP=sp_min
 
-The BL31 ELF image is generated in ``build/msm8916/release/bl31/bl31.elf``.
-This image must be "signed" before flashing it, even if the board has secure
+The BL32 ELF image is generated in ``build/$PLAT/release/bl32/bl32.elf``.
+
+Build Options
+-------------
+Some options can be changed at build time by adding them to the make command line:
+
+ * ``QTI_UART_NUM``: Number of UART controller to use for debug output and crash
+   reports. This must be the same UART as used by earlier boot firmware since
+   the UART controller does not get fully initialized at the moment. Defaults to
+   the usual debug UART used for the platform (see ``platform.mk``).
+ * ``QTI_RUNTIME_UART``: By default (``0``) the UART is only used for the boot
+   process and critical crashes. If set to ``1`` it is also used for runtime
+   messages. Note that this option can only be used if the UART is reserved in
+   the normal world and the necessary clocks remain enabled.
+
+The memory region used for the different firmware components is not fixed and
+can be changed on the make command line. The default values match the addresses
+used by the original firmware (see ``platform.mk``):
+
+ * ``PRELOADED_BL33_BASE``: The entry address for the normal world. Usually
+   refers to the first bootloader (e.g. U-Boot).
+ * ``BL31_BASE``: Base address for the BL31 firmware component. Must point to
+   a 64K-aligned memory region with at least 128 KiB space that is permanently
+   reserved in the normal world.
+ * ``BL32_BASE``: Base address for the BL32 firmware component.
+
+   * **AArch32:** BL32 is used in place of BL31, so the option is equivalent to
+     ``BL31_BASE``.
+   * **AArch64:** Secure-EL1 Payload. Defaults to using 128 KiB of space
+     directly after BL31. For testing only, the port is primarily intended as
+     a minimal PSCI implementation without a separate secure world.
+
+Installation
+------------
+The ELF image must be "signed" before flashing it, even if the board has secure
 boot disabled. In this case the signature does not provide any security,
 but it provides the firmware with required metadata.
 
@@ -75,6 +134,10 @@
 Then install the resulting ``build/msm8916/release/bl31/bl31-test-signed.mbn``
 to the ``tz`` partition on the device. BL31 should be running after a reboot.
 
+.. note::
+	On AArch32 the ELF image is called ``bl32.elf``.
+	The installation procedure is identical.
+
 .. warning::
 	Do not flash incorrectly signed firmware on devices that have secure
 	boot enabled! Make sure that you have a way to recover the board in case
@@ -82,8 +145,11 @@
 
 Boot Trace
 ----------
-BL31 prints some lines on the debug console UART2, which will usually look like
-this (with ``DEBUG=1``, otherwise only the ``NOTICE`` lines are shown)::
+
+AArch64 (BL31)
+^^^^^^^^^^^^^^
+BL31 prints some lines on the debug console, which will usually look like this
+(with ``DEBUG=1``, otherwise only the ``NOTICE`` lines are shown)::
 
 	...
 	S - DDR Frequency, 400 MHz
@@ -109,8 +175,37 @@
 	Qualcomm-DragonBoard 410C
 	...
 
+AArch32 (BL32/SP_MIN)
+^^^^^^^^^^^^^^^^^^^^^
+BL32/SP_MIN prints some lines on the debug console, which will usually look like
+this (with ``DEBUG=1``, otherwise only the ``NOTICE`` lines are shown)::
+
+	...
+	S - DDR Frequency, 400 MHz
+	NOTICE:  SP_MIN: v2.8(debug):v2.8
+	NOTICE:  SP_MIN: Built : 23:03:31, Mar 31 2023
+	INFO:    SP_MIN: Platform setup start
+	INFO:    ARM GICv2 driver initialized
+	INFO:    SP_MIN: Platform setup done
+	INFO:    SP_MIN: Initializing runtime services
+	INFO:    BL32: cortex_a53: CPU workaround for 819472 was applied
+	INFO:    BL32: cortex_a53: CPU workaround for 824069 was applied
+	INFO:    BL32: cortex_a53: CPU workaround for 826319 was applied
+	INFO:    BL32: cortex_a53: CPU workaround for 827319 was applied
+	INFO:    BL32: cortex_a53: CPU workaround for disable_non_temporal_hint was applied
+	INFO:    SP_MIN: Preparing exit to normal world
+	INFO:    Entry point address = 0x86400000
+	INFO:    SPSR = 0x1da
+	Android Bootloader - UART_DM Initialized!!!
+	[0] welcome to lk
+	...
+
-.. _Qualcomm Snapdragon 410: https://www.qualcomm.com/products/snapdragon-processors-410
+.. _Snapdragon 210: https://www.qualcomm.com/products/snapdragon-processors-210
+.. _Snapdragon 410: https://www.qualcomm.com/products/snapdragon-processors-410
+.. _Snapdragon 615: https://www.qualcomm.com/products/snapdragon-processors-615
+.. _Snapdragon X5 Modem: https://www.qualcomm.com/products/snapdragon-modems-4g-lte-x5
 .. _DragonBoard 410c: https://www.96boards.org/product/dragonboard410c/
 .. _Snapdragon 410E Technical Reference Manual: https://developer.qualcomm.com/download/sd410/snapdragon-410e-technical-reference-manual.pdf
 .. _U-Boot for DragonBoard 410c: https://u-boot.readthedocs.io/en/latest/board/qualcomm/dragonboard410c.html
 .. _qtestsign: https://github.com/msm8916-mainline/qtestsign
+.. _tfalkstub: https://github.com/msm8916-mainline/tfalkstub
diff --git a/docs/porting-guide.rst b/docs/porting-guide.rst
index b557a16..f074021 100644
--- a/docs/porting-guide.rst
+++ b/docs/porting-guide.rst
@@ -1509,43 +1509,6 @@
 the SMCCC function specified in the argument; otherwise returns
 SMC_ARCH_CALL_NOT_SUPPORTED.
 
-Function : plat_mboot_measure_image()
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-::
-
-    Argument : unsigned int, image_info_t *
-    Return   : int
-
-When the MEASURED_BOOT flag is enabled:
-
--  This function measures the given image and records its measurement using
-   the measured boot backend driver.
--  On the Arm FVP port, this function measures the given image using its
-   passed id and information and then records that measurement in the
-   Event Log buffer.
--  This function must return 0 on success, a signed integer error code
-   otherwise.
-
-When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
-
-Function : plat_mboot_measure_critical_data()
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-::
-
-    Argument : unsigned int, const void *, size_t
-    Return   : int
-
-When the MEASURED_BOOT flag is enabled:
-
--  This function measures the given critical data structure and records its
-   measurement using the measured boot backend driver.
--  This function must return 0 on success, a signed integer error code
-   otherwise.
-
-When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
-
 Function : plat_can_cmo()
 ~~~~~~~~~~~~~~~~~~~~~~~~~
 
@@ -1813,42 +1776,6 @@
 The default implementation of this function asserts therefore platforms must
 override it when using the FWU feature.
 
-Function : bl1_plat_mboot_init() [optional]
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-::
-
-    Argument : void
-    Return   : void
-
-When the MEASURED_BOOT flag is enabled:
-
--  This function is used to initialize the backend driver(s) of measured boot.
--  On the Arm FVP port, this function is used to initialize the Event Log
-   backend driver, and also to write header information in the Event Log buffer.
-
-When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
-
-Function : bl1_plat_mboot_finish() [optional]
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-::
-
-    Argument : void
-    Return   : void
-
-When the MEASURED_BOOT flag is enabled:
-
--  This function is used to finalize the measured boot backend driver(s),
-   and also, set the information for the next bootloader component to
-   extend the measurement if needed.
--  On the Arm FVP port, this function is used to pass the base address of
-   the Event Log buffer and its size to BL2 via tb_fw_config to extend the
-   Event Log buffer with the measurement of various images loaded by BL2.
-   It results in panic on error.
-
-When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
-
 Boot Loader Stage 2 (BL2)
 -------------------------
 
@@ -1980,42 +1907,6 @@
 must return 0, otherwise it must return 1. The default implementation
 of this always returns 0.
 
-Function : bl2_plat_mboot_init() [optional]
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-::
-
-    Argument : void
-    Return   : void
-
-When the MEASURED_BOOT flag is enabled:
-
--  This function is used to initialize the backend driver(s) of measured boot.
--  On the Arm FVP port, this function is used to initialize the Event Log
-   backend driver with the Event Log buffer information (base address and
-   size) received from BL1. It results in panic on error.
-
-When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
-
-Function : bl2_plat_mboot_finish() [optional]
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-::
-
-    Argument : void
-    Return   : void
-
-When the MEASURED_BOOT flag is enabled:
-
--  This function is used to finalize the measured boot backend driver(s),
-   and also, set the information for the next bootloader component to extend
-   the measurement if needed.
--  On the Arm FVP port, this function is used to pass the Event Log buffer
-   information (base address and size) to non-secure(BL33) and trusted OS(BL32)
-   via nt_fw and tos_fw config respectively. It results in panic on error.
-
-When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
-
 Boot Loader Stage 2 (BL2) at EL3
 --------------------------------
 
@@ -3664,6 +3555,12 @@
 dynamically allocating memory. This may also have the affect of limiting the
 amount of open resources per driver.
 
+Measured Boot Platform Interface
+--------------------------------
+
+Enabling the MEASURED_BOOT flag adds extra platform requirements. Please refer
+to :ref:`Measured Boot Design` for more details.
+
 --------------
 
 *Copyright (c) 2013-2023, Arm Limited and Contributors. All rights reserved.*
diff --git a/docs/process/code-review-guidelines.rst b/docs/process/code-review-guidelines.rst
index ccdd110..bd42811 100644
--- a/docs/process/code-review-guidelines.rst
+++ b/docs/process/code-review-guidelines.rst
@@ -94,6 +94,14 @@
    directory to have the freedom to change it in any way. This way, your changes
    benefit everyone and will be maintained over time.
 
+-  It is the patch-author's responsibility to respond to review comments within
+   21 days. In the event that the patch-author does not respond within this
+   timeframe, the maintainer is entitled to abandon the patch(es).
+   Patch author(s) may be busy with other priorities, causing a delay in
+   responding to active review comments after posting patch(es). In such a
+   situation, if the author's patch(es) is/are abandoned, they can restore
+   their work for review by resolving comments, merge-conflicts, and revising
+   their original submissions.
 
 Guidelines for all reviewers
 ----------------------------
diff --git a/docs/resources/diagrams/Makefile b/docs/resources/diagrams/Makefile
index c951754..faf9634 100644
--- a/docs/resources/diagrams/Makefile
+++ b/docs/resources/diagrams/Makefile
@@ -79,7 +79,13 @@
 FWU-update_struct_layers		= "background"
 FWU-update_struct_opts			=
 
-all:$(RESET_PNGS) $(INT_PNGS) $(XLAT_PNG) $(RMM_PNG) $(RMM_EL3_MANIFEST_PNG) $(PSA_FWU_PNG)
+MB_DESIGN_DIA				= measured_boot_design.dia
+MB_DESIGN_PNG				= measured_boot_design.png
+
+measured_boot_design_layers		= "background"
+measured_boot_design_opts		=
+
+all:$(RESET_PNGS) $(INT_PNGS) $(XLAT_PNG) $(RMM_PNG) $(RMM_EL3_MANIFEST_PNG) $(PSA_FWU_PNG) $(MB_DESIGN_PNG)
 
 $(RESET_PNGS):$(RESET_DIA)
 	$(call generate_image,$($(patsubst %.png,%_layers,$@)),$@,png,$($(patsubst %.png,%_opts,$@)),$<)
@@ -99,3 +105,6 @@
 
 $(PSA_FWU_PNG):$(PSA_FWU_DIA)
 	$(call generate_image,$($(patsubst %.png,%_layers,$@)),$@,png,$($(patsubst %.png,%_opts,$@)),$<)
+
+$(MB_DESIGN_PNG):$(MB_DESIGN_DIA)
+	$(call generate_image,$($(patsubst %.png,%_layers,$@)),$@,png,$($(patsubst %.png,%_opts,$@)),$<)
diff --git a/docs/resources/diagrams/measured_boot_design.dia b/docs/resources/diagrams/measured_boot_design.dia
new file mode 100644
index 0000000..fdae464
--- /dev/null
+++ b/docs/resources/diagrams/measured_boot_design.dia
Binary files differ
diff --git a/docs/resources/diagrams/measured_boot_design.png b/docs/resources/diagrams/measured_boot_design.png
new file mode 100644
index 0000000..42469be
--- /dev/null
+++ b/docs/resources/diagrams/measured_boot_design.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 d73e74b..762801d 100644
--- a/docs/security_advisories/security-advisory-tfv-9.rst
+++ b/docs/security_advisories/security-advisory-tfv-9.rst
@@ -77,7 +77,7 @@
 +----------------------+
 | Cortex-A715          |
 +----------------------+
-| Cortex-Hunter        |
+| Cortex-A720          |
 +----------------------+
 | Neoverse-N1          |
 +----------------------+
diff --git a/drivers/arm/gic/v3/gic600_multichip.c b/drivers/arm/gic/v3/gic600_multichip.c
index 7f0735d..a4786bb 100644
--- a/drivers/arm/gic/v3/gic600_multichip.c
+++ b/drivers/arm/gic/v3/gic600_multichip.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2023, Arm Limited. All rights reserved.
  * Copyright (c) 2022-2023, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
@@ -217,7 +217,7 @@
 
 			/* SPI IDs range check */
 			if (!(spi_id_min >= GIC600_SPI_ID_MIN) ||
-			    !(spi_id_max < GIC600_SPI_ID_MAX) ||
+			    !(spi_id_max <= GIC600_SPI_ID_MAX) ||
 			    !(spi_id_min <= spi_id_max) ||
 			    !((spi_id_max - spi_id_min + 1) % 32 == 0)) {
 				ERROR("Invalid SPI IDs {%u, %u} passed for "
diff --git a/drivers/arm/gic/v3/gic600_multichip_private.h b/drivers/arm/gic/v3/gic600_multichip_private.h
index f6028ad..fd1cb57 100644
--- a/drivers/arm/gic/v3/gic600_multichip_private.h
+++ b/drivers/arm/gic/v3/gic600_multichip_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, ARM Limited. All rights reserved.
+ * Copyright (c) 2019-2023, ARM Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -39,7 +39,7 @@
 
 /* SPI interrupt id minimum and maximum range */
 #define GIC600_SPI_ID_MIN		32
-#define GIC600_SPI_ID_MAX		960
+#define GIC600_SPI_ID_MAX		991
 
 #define GIC700_SPI_ID_MIN		32
 #define GIC700_SPI_ID_MAX		991
diff --git a/drivers/arm/gic/v3/gicv3_helpers.c b/drivers/arm/gic/v3/gicv3_helpers.c
index 00bd7a1..b27debf 100644
--- a/drivers/arm/gic/v3/gicv3_helpers.c
+++ b/drivers/arm/gic/v3/gicv3_helpers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2023, Arm Limited and Contributors. All rights reserved.
  * Copyright (c) 2023, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
@@ -223,13 +223,16 @@
 		current_prop = &interrupt_props[i];
 
 		unsigned int intr_num = current_prop->intr_num;
-		uintptr_t multichip_gicd_base = gicv3_get_multichip_base(intr_num, gicd_base);
+		uintptr_t multichip_gicd_base;
 
 		/* Skip SGI, (E)PPI and LPI interrupts */
 		if (!IS_SPI(intr_num)) {
 			continue;
 		}
 
+		multichip_gicd_base =
+			gicv3_get_multichip_base(intr_num, gicd_base);
+
 		/* Configure this interrupt as a secure interrupt */
 		gicd_clr_igroupr(multichip_gicd_base, intr_num);
 
diff --git a/drivers/auth/mbedtls/mbedtls_crypto.c b/drivers/auth/mbedtls/mbedtls_crypto.c
index 6d6364f..230cec9 100644
--- a/drivers/auth/mbedtls/mbedtls_crypto.c
+++ b/drivers/auth/mbedtls/mbedtls_crypto.c
@@ -172,17 +172,20 @@
 	int rc;
 
 	/*
-	 * Digest info should be an MBEDTLS_ASN1_SEQUENCE
-	 * and consume all bytes.
+	 * Digest info should be an MBEDTLS_ASN1_SEQUENCE, but padding after
+	 * it is allowed.  This is necessary to support multiple hash
+	 * algorithms.
 	 */
 	p = (unsigned char *)digest_info_ptr;
 	end = p + digest_info_len;
 	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
 				  MBEDTLS_ASN1_SEQUENCE);
-	if (rc != 0 || ((size_t)(end - p) != len)) {
+	if (rc != 0) {
 		return CRYPTO_ERR_HASH;
 	}
 
+	end = p + len;
+
 	/* Get the hash algorithm */
 	rc = mbedtls_asn1_get_alg(&p, end, &hash_oid, &params);
 	if (rc != 0) {
diff --git a/drivers/auth/mbedtls/mbedtls_x509_parser.c b/drivers/auth/mbedtls/mbedtls_x509_parser.c
index fb5b036..8bde5bb 100644
--- a/drivers/auth/mbedtls/mbedtls_x509_parser.c
+++ b/drivers/auth/mbedtls/mbedtls_x509_parser.c
@@ -135,8 +135,38 @@
 		if ((oid != NULL) &&
 		    ((size_t)oid_len == strlen(oid_str)) &&
 		    (strcmp(oid, oid_str) == 0)) {
+			/* Extension must be ASN.1 DER */
+			if (len < 2) {
+				/* too short */
+				return IMG_PARSER_ERR_FORMAT;
+			}
+
+			if ((p[0] & 0x1F) == 0x1F) {
+				/* multi-byte ASN.1 DER tag, not allowed */
+				return IMG_PARSER_ERR_FORMAT;
+			}
+
+			if ((p[0] & 0xDF) == 0) {
+				/* UNIVERSAL 0 tag, not allowed */
+				return IMG_PARSER_ERR_FORMAT;
+			}
+
 			*ext = (void *)p;
 			*ext_len = (unsigned int)len;
+
+			/* Advance past the tag byte */
+			p++;
+
+			if (mbedtls_asn1_get_len(&p, end_ext_data, &len)) {
+				/* not valid DER */
+				return IMG_PARSER_ERR_FORMAT;
+			}
+
+			if (p + len != end_ext_data) {
+				/* junk after ASN.1 object */
+				return IMG_PARSER_ERR_FORMAT;
+			}
+
 			return IMG_PARSER_OK;
 		}
 
diff --git a/drivers/cadence/combo_phy/cdns_combo_phy.c b/drivers/cadence/combo_phy/cdns_combo_phy.c
new file mode 100644
index 0000000..f00d0c1
--- /dev/null
+++ b/drivers/cadence/combo_phy/cdns_combo_phy.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/cadence/cdns_combo_phy.h>
+#include <drivers/cadence/cdns_sdmmc.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+int cdns_sdmmc_write_phy_reg(uint32_t phy_reg_addr, uint32_t phy_reg_addr_value,
+			uint32_t phy_reg_data, uint32_t phy_reg_data_value)
+{
+	uint32_t data = 0U;
+	uint32_t value = 0U;
+
+	/* Get PHY register address, write HRS04*/
+	value = mmio_read_32(phy_reg_addr);
+	value &= ~PHY_REG_ADDR_MASK;
+	value |= phy_reg_addr_value;
+	mmio_write_32(phy_reg_addr, value);
+	data = mmio_read_32(phy_reg_addr);
+	if ((data & PHY_REG_ADDR_MASK) != phy_reg_addr_value) {
+		ERROR("PHY_REG_ADDR is not set properly\n");
+		return -ENXIO;
+	}
+
+	/* Get PHY register data, write HRS05 */
+	value &= ~PHY_REG_DATA_MASK;
+	value |= phy_reg_data_value;
+	mmio_write_32(phy_reg_data, value);
+	data = mmio_read_32(phy_reg_data);
+	if (data != phy_reg_data_value) {
+		ERROR("PHY_REG_DATA is not set properly\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+int cdns_sd_card_detect(void)
+{
+	uint32_t value = 0;
+
+	/* Card detection */
+	do {
+		value = mmio_read_32(SDMMC_CDN(SRS09));
+	/* Wait for card insertion. SRS09.CI = 1 */
+	} while ((value & (1 << SDMMC_CDN_CI)) == 0);
+
+	if ((value & (1 << SDMMC_CDN_CI)) == 0) {
+		ERROR("Card does not detect\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+int cdns_emmc_card_reset(void)
+{
+	uint32_t _status = 0;
+
+	/* Reset embedded card */
+	mmio_write_32(SDMMC_CDN(SRS10), (7 << SDMMC_CDN_BVS) | (1 << SDMMC_CDN_BP) | _status);
+	mdelay(68680); /* ~68680us */
+	mmio_write_32(SDMMC_CDN(SRS10), (7 << SDMMC_CDN_BVS) | (0 << SDMMC_CDN_BP));
+	udelay(340); /* ~340us */
+
+	/* Turn on supply voltage */
+	/* BVS = 7, BP = 1, BP2 only in UHS2 mode */
+	mmio_write_32(SDMMC_CDN(SRS10), (7 << SDMMC_CDN_BVS) | (1 << SDMMC_CDN_BP) | _status);
+
+	return 0;
+}
diff --git a/drivers/cadence/emmc/cdns_sdmmc.c b/drivers/cadence/emmc/cdns_sdmmc.c
new file mode 100644
index 0000000..d2cd4d6
--- /dev/null
+++ b/drivers/cadence/emmc/cdns_sdmmc.c
@@ -0,0 +1,824 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/cadence/cdns_sdmmc.h>
+#include <drivers/delay_timer.h>
+#include <drivers/mmc.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+/* Card busy and present */
+#define CARD_BUSY					1
+#define CARD_NOT_BUSY					0
+
+/* 500 ms delay to read the RINST register */
+#define DELAY_MS_SRS_READ				500
+#define DELAY_RES					10
+
+/* SRS12 error mask */
+#define SRS12_ERR_MASK					0xFFFF8000
+
+/* Check DV dfi_init val=0 */
+#define IO_MASK_END_DATA				0x0
+
+/* Check DV dfi_init val=2; DDR Mode */
+#define IO_MASK_END_DATA_DDR				0x2
+#define IO_MASK_START_DATA				0x0
+#define DATA_SELECT_OE_END_DATA				0x1
+
+#define TIMEOUT						100000
+
+/* General define */
+#define SDHC_REG_MASK					UINT_MAX
+#define SD_HOST_BLOCK_SIZE				0x200
+#define DTCVVAL_DEFAULT_VAL				0xE
+#define CDMMC_DMA_MAX_BUFFER_SIZE			64*1024
+#define CDNSMMC_ADDRESS_MASK				U(0x0f)
+#define CONFIG_CDNS_DESC_COUNT				8
+
+void cdns_init(void);
+int cdns_send_cmd(struct mmc_cmd *cmd);
+int cdns_set_ios(unsigned int clk, unsigned int width);
+int cdns_prepare(int lba, uintptr_t buf, size_t size);
+int cdns_read(int lba, uintptr_t buf, size_t size);
+int cdns_write(int lba, uintptr_t buf, size_t size);
+
+const struct mmc_ops cdns_sdmmc_ops = {
+	.init			= cdns_init,
+	.send_cmd		= cdns_send_cmd,
+	.set_ios		= cdns_set_ios,
+	.prepare		= cdns_prepare,
+	.read			= cdns_read,
+	.write			= cdns_write,
+};
+
+struct cdns_sdmmc_params cdns_params;
+struct cdns_sdmmc_combo_phy sdmmc_combo_phy_reg;
+struct cdns_sdmmc_sdhc sdmmc_sdhc_reg;
+#ifdef CONFIG_DMA_ADDR_T_64BIT
+struct cdns_idmac_desc cdns_desc[CONFIG_CDNS_DESC_COUNT];
+#else
+struct cdns_idmac_desc cdns_desc[CONFIG_CDNS_DESC_COUNT] __aligned(32);
+#endif
+
+bool data_cmd;
+
+int cdns_wait_ics(uint16_t timeout, uint32_t cdn_srs_res)
+{
+	/* Clock for sdmclk and sdclk */
+	uint32_t count = 0;
+	uint32_t data = 0;
+
+	/* Wait status command response ready */
+	do {
+		data = mmio_read_32(cdn_srs_res);
+		count++;
+		if (count >= timeout) {
+			return -ETIMEDOUT;
+		}
+	} while ((data & (1 << SDMMC_CDN_ICS)) == 0);
+
+	return 0;
+}
+
+int cdns_busy(void)
+{
+	unsigned int data;
+
+	data = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS09);
+	return (data & STATUS_DATA_BUSY) ? CARD_BUSY : CARD_NOT_BUSY;
+}
+
+int cdns_vol_reset(void)
+{
+	/* Reset embedded card */
+	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), (7 << SDMMC_CDN_BVS) | (1 << SDMMC_CDN_BP));
+	udelay(250);
+	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), (7 << SDMMC_CDN_BVS) | (0 << SDMMC_CDN_BP));
+	udelay(500);
+
+	/* Turn on supply voltage */
+	/* BVS = 7, BP = 1, BP2 only in UHS2 mode */
+	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), (7 << SDMMC_CDN_BVS) | (1 << SDMMC_CDN_BP));
+	udelay(250);
+	return 0;
+}
+
+void cdns_set_sdmmc_var(struct cdns_sdmmc_combo_phy *combo_phy_reg,
+	struct cdns_sdmmc_sdhc *sdhc_reg)
+{
+	/* Values are taken by the reference of cadence IP documents */
+	combo_phy_reg->cp_clk_wr_delay = 0;
+	combo_phy_reg->cp_clk_wrdqs_delay = 0;
+	combo_phy_reg->cp_data_select_oe_end = 0;
+	combo_phy_reg->cp_dll_bypass_mode = 1;
+	combo_phy_reg->cp_dll_locked_mode = 0;
+	combo_phy_reg->cp_dll_start_point = 0;
+	combo_phy_reg->cp_gate_cfg_always_on = 1;
+	combo_phy_reg->cp_io_mask_always_on = 0;
+	combo_phy_reg->cp_io_mask_end = 0;
+	combo_phy_reg->cp_io_mask_start = 0;
+	combo_phy_reg->cp_rd_del_sel = 52;
+	combo_phy_reg->cp_read_dqs_cmd_delay = 0;
+	combo_phy_reg->cp_read_dqs_delay = 0;
+	combo_phy_reg->cp_sw_half_cycle_shift = 0;
+	combo_phy_reg->cp_sync_method = 1;
+	combo_phy_reg->cp_underrun_suppress = 1;
+	combo_phy_reg->cp_use_ext_lpbk_dqs = 1;
+	combo_phy_reg->cp_use_lpbk_dqs = 1;
+	combo_phy_reg->cp_use_phony_dqs = 1;
+	combo_phy_reg->cp_use_phony_dqs_cmd = 1;
+
+	sdhc_reg->sdhc_extended_rd_mode = 1;
+	sdhc_reg->sdhc_extended_wr_mode = 1;
+	sdhc_reg->sdhc_hcsdclkadj = 0;
+	sdhc_reg->sdhc_idelay_val = 0;
+	sdhc_reg->sdhc_rdcmd_en = 1;
+	sdhc_reg->sdhc_rddata_en = 1;
+	sdhc_reg->sdhc_rw_compensate = 9;
+	sdhc_reg->sdhc_sdcfsh = 0;
+	sdhc_reg->sdhc_sdcfsl = 1;
+	sdhc_reg->sdhc_wrcmd0_dly = 1;
+	sdhc_reg->sdhc_wrcmd0_sdclk_dly = 0;
+	sdhc_reg->sdhc_wrcmd1_dly = 0;
+	sdhc_reg->sdhc_wrcmd1_sdclk_dly = 0;
+	sdhc_reg->sdhc_wrdata0_dly = 1;
+	sdhc_reg->sdhc_wrdata0_sdclk_dly = 0;
+	sdhc_reg->sdhc_wrdata1_dly = 0;
+	sdhc_reg->sdhc_wrdata1_sdclk_dly = 0;
+}
+
+static int cdns_program_phy_reg(struct cdns_sdmmc_combo_phy *combo_phy_reg,
+	struct cdns_sdmmc_sdhc *sdhc_reg)
+{
+	uint32_t value = 0;
+	int ret = 0;
+
+	/* program PHY_DQS_TIMING_REG */
+	value = (CP_USE_EXT_LPBK_DQS(combo_phy_reg->cp_use_ext_lpbk_dqs)) |
+		(CP_USE_LPBK_DQS(combo_phy_reg->cp_use_lpbk_dqs)) |
+		(CP_USE_PHONY_DQS(combo_phy_reg->cp_use_phony_dqs)) |
+		(CP_USE_PHONY_DQS_CMD(combo_phy_reg->cp_use_phony_dqs_cmd));
+	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
+			COMBO_PHY_REG + PHY_DQS_TIMING_REG, MMC_REG_BASE +
+			SDHC_CDNS_HRS05, value);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* program PHY_GATE_LPBK_CTRL_REG */
+	value = (CP_SYNC_METHOD(combo_phy_reg->cp_sync_method)) |
+		(CP_SW_HALF_CYCLE_SHIFT(combo_phy_reg->cp_sw_half_cycle_shift)) |
+		(CP_RD_DEL_SEL(combo_phy_reg->cp_rd_del_sel)) |
+		(CP_UNDERRUN_SUPPRESS(combo_phy_reg->cp_underrun_suppress)) |
+		(CP_GATE_CFG_ALWAYS_ON(combo_phy_reg->cp_gate_cfg_always_on));
+	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
+			COMBO_PHY_REG + PHY_GATE_LPBK_CTRL_REG, MMC_REG_BASE +
+			SDHC_CDNS_HRS05, value);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* program PHY_DLL_MASTER_CTRL_REG */
+	value = (CP_DLL_BYPASS_MODE(combo_phy_reg->cp_dll_bypass_mode))
+			| (CP_DLL_START_POINT(combo_phy_reg->cp_dll_start_point));
+	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
+			COMBO_PHY_REG + PHY_DLL_MASTER_CTRL_REG, MMC_REG_BASE
+			+ SDHC_CDNS_HRS05, value);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* program PHY_DLL_SLAVE_CTRL_REG */
+	value = (CP_READ_DQS_CMD_DELAY(combo_phy_reg->cp_read_dqs_cmd_delay))
+		| (CP_CLK_WRDQS_DELAY(combo_phy_reg->cp_clk_wrdqs_delay))
+		| (CP_CLK_WR_DELAY(combo_phy_reg->cp_clk_wr_delay))
+		| (CP_READ_DQS_DELAY(combo_phy_reg->cp_read_dqs_delay));
+	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
+			COMBO_PHY_REG + PHY_DLL_SLAVE_CTRL_REG, MMC_REG_BASE
+			+ SDHC_CDNS_HRS05, value);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* program PHY_CTRL_REG */
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS04, COMBO_PHY_REG
+			+ PHY_CTRL_REG);
+	value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS05);
+
+	/* phony_dqs_timing=0 */
+	value &= ~(CP_PHONY_DQS_TIMING_MASK << CP_PHONY_DQS_TIMING_SHIFT);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS05, value);
+
+	/* switch off DLL_RESET */
+	do {
+		value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
+		value |= SDHC_PHY_SW_RESET;
+		mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, value);
+		value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
+	/* polling PHY_INIT_COMPLETE */
+	} while ((value & SDHC_PHY_INIT_COMPLETE) != SDHC_PHY_INIT_COMPLETE);
+
+	/* program PHY_DQ_TIMING_REG */
+	combo_phy_reg->cp_io_mask_end = 0U;
+	value = (CP_IO_MASK_ALWAYS_ON(combo_phy_reg->cp_io_mask_always_on))
+		| (CP_IO_MASK_END(combo_phy_reg->cp_io_mask_end))
+		| (CP_IO_MASK_START(combo_phy_reg->cp_io_mask_start))
+		| (CP_DATA_SELECT_OE_END(combo_phy_reg->cp_data_select_oe_end));
+
+	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
+			COMBO_PHY_REG + PHY_DQ_TIMING_REG, MMC_REG_BASE
+			+ SDHC_CDNS_HRS05, value);
+	if (ret != 0) {
+		return ret;
+	}
+	return 0;
+}
+
+int cdns_read(int lba, uintptr_t buf, size_t size)
+{
+	inv_dcache_range(buf, size);
+
+	return 0;
+}
+
+void cdns_init(void)
+{
+	/* Dummy function pointer for cdns_init. */
+}
+
+int cdns_prepare(int dma_start_addr, uintptr_t dma_buff, size_t size)
+{
+	data_cmd = true;
+	struct cdns_idmac_desc *desc;
+	uint32_t desc_cnt, i;
+	uint64_t desc_base;
+
+	assert(((dma_buff & CDNSMMC_ADDRESS_MASK) == 0) &&
+			(cdns_params.desc_size > 0) &&
+			((MMC_REG_BASE & MMC_BLOCK_MASK) == 0) &&
+			((cdns_params.desc_base & MMC_BLOCK_MASK) == 0) &&
+			((cdns_params.desc_size & MMC_BLOCK_MASK) == 0));
+
+	flush_dcache_range(dma_buff, size);
+
+	desc_cnt = (size + (CDMMC_DMA_MAX_BUFFER_SIZE) - 1) / (CDMMC_DMA_MAX_BUFFER_SIZE);
+	assert(desc_cnt * sizeof(struct cdns_idmac_desc) < cdns_params.desc_size);
+
+	if (desc_cnt > CONFIG_CDNS_DESC_COUNT) {
+		ERROR("Requested data transfer length %ld is greater than configured length %d",
+				size, (CONFIG_CDNS_DESC_COUNT * CDMMC_DMA_MAX_BUFFER_SIZE));
+		return -EINVAL;
+	}
+
+	desc = (struct cdns_idmac_desc *)cdns_params.desc_base;
+	desc_base = (uint64_t)desc;
+	i = 0;
+
+	while ((i + 1) < desc_cnt) {
+		desc->attr = ADMA_DESC_ATTR_VALID | ADMA_DESC_TRANSFER_DATA;
+		desc->reserved = 0;
+		desc->len = MAX_64KB_PAGE;
+		desc->addr_lo = (dma_buff & UINT_MAX) + (CDMMC_DMA_MAX_BUFFER_SIZE * i);
+#if CONFIG_DMA_ADDR_T_64BIT == 1
+		desc->addr_hi = (dma_buff >> 32) & 0xffffffff;
+#endif
+		size -= CDMMC_DMA_MAX_BUFFER_SIZE;
+		desc++;
+		i++;
+	}
+
+	desc->attr = ADMA_DESC_ATTR_VALID | ADMA_DESC_TRANSFER_DATA |
+			ADMA_DESC_ATTR_END;
+	desc->reserved = 0;
+	desc->len = size;
+#if CONFIG_DMA_ADDR_T_64BIT == 1
+	desc->addr_lo = (dma_buff & UINT_MAX) + (CDMMC_DMA_MAX_BUFFER_SIZE * i);
+	desc->addr_hi = (dma_buff >> 32) & UINT_MAX;
+#else
+	desc->addr_lo = (dma_buff & UINT_MAX);
+#endif
+
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS22, (uint32_t)desc_base);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS23, (uint32_t)(desc_base >> 32));
+	flush_dcache_range(cdns_params.desc_base,
+				desc_cnt * CDMMC_DMA_MAX_BUFFER_SIZE);
+
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS01,
+			((512 << BLOCK_SIZE) | ((size/512) << BLK_COUNT_CT) | SDMA_BUF));
+	return 0;
+}
+
+static void cdns_host_set_clk(int clk)
+{
+	uint32_t ret = 0;
+	uint32_t sdclkfsval = 0;
+	uint32_t dtcvval = DTCVVAL_DEFAULT_VAL;
+
+	sdclkfsval = (cdns_params.clk_rate / 2000) / clk;
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, 0);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (dtcvval << SDMMC_CDN_DTCV) |
+			(sdclkfsval << SDMMC_CDN_SDCLKFS) | (1 << SDMMC_CDN_ICE));
+
+	ret = cdns_wait_ics(5000, MMC_REG_BASE + SDHC_CDNS_SRS11);
+	if (ret != 0U) {
+		ERROR("Waiting SDMMC_CDN_ICS timeout");
+	}
+
+	/* Enable DLL reset */
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09) &
+			~SDHC_DLL_RESET_MASK);
+	/* Set extended_wr_mode */
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, (mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09)
+			& SDHC_EXTENDED_WR_MODE_MASK) | (1 << EXTENDED_WR_MODE));
+	/* Release DLL reset */
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE
+			+ SDHC_CDNS_HRS09) | 1);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE
+			+ SDHC_CDNS_HRS09) | (3 << RDCMD_EN));
+
+	do {
+		mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
+	} while (~mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09) & (1 << 1));
+
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (dtcvval << SDMMC_CDN_DTCV) |
+	(sdclkfsval << SDMMC_CDN_SDCLKFS) | (1 << SDMMC_CDN_ICE) | (1 << SDMMC_CDN_SDCE));
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS13, UINT_MAX);
+}
+
+int cdns_set_ios(unsigned int clk, unsigned int width)
+{
+
+	switch (width) {
+	case MMC_BUS_WIDTH_1:
+		mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), LEDC_OFF);
+		break;
+	case MMC_BUS_WIDTH_4:
+		mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), DTW_4BIT);
+		break;
+	case MMC_BUS_WIDTH_8:
+		mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), EDTW_8BIT);
+		break;
+	default:
+		assert(0);
+		break;
+	}
+	cdns_host_set_clk(clk);
+
+	return 0;
+}
+
+int cdns_sdmmc_write_sd_host_reg(uint32_t addr, uint32_t data)
+{
+	uint32_t value = 0;
+
+	value = mmio_read_32(addr);
+	value &= ~SDHC_REG_MASK;
+	value |= data;
+	mmio_write_32(addr, value);
+	value = mmio_read_32(addr);
+	if (value != data) {
+		ERROR("SD host address is not set properly\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+int cdns_write(int lba, uintptr_t buf, size_t size)
+{
+	return 0;
+}
+
+static int cdns_init_hrs_io(struct cdns_sdmmc_combo_phy *combo_phy_reg,
+	struct cdns_sdmmc_sdhc *sdhc_reg)
+{
+	uint32_t value = 0;
+	int ret = 0;
+
+	/* program HRS09, register 42 */
+	value = (SDHC_RDDATA_EN(sdhc_reg->sdhc_rddata_en))
+		| (SDHC_RDCMD_EN(sdhc_reg->sdhc_rdcmd_en))
+		| (SDHC_EXTENDED_WR_MODE(sdhc_reg->sdhc_extended_wr_mode))
+		| (SDHC_EXTENDED_RD_MODE(sdhc_reg->sdhc_extended_rd_mode));
+	ret = cdns_sdmmc_write_sd_host_reg(MMC_REG_BASE + SDHC_CDNS_HRS09, value);
+	if (ret != 0) {
+		ERROR("Program HRS09 failed");
+		return ret;
+	}
+
+	/* program HRS10, register 43 */
+	value = (SDHC_HCSDCLKADJ(sdhc_reg->sdhc_hcsdclkadj));
+	ret = cdns_sdmmc_write_sd_host_reg(MMC_REG_BASE + SDHC_CDNS_HRS10, value);
+	if (ret != 0) {
+		ERROR("Program HRS10 failed");
+		return ret;
+	}
+
+	/* program HRS16, register 48 */
+	value = (SDHC_WRDATA1_SDCLK_DLY(sdhc_reg->sdhc_wrdata1_sdclk_dly))
+		| (SDHC_WRDATA0_SDCLK_DLY(sdhc_reg->sdhc_wrdata0_sdclk_dly))
+		| (SDHC_WRCMD1_SDCLK_DLY(sdhc_reg->sdhc_wrcmd1_sdclk_dly))
+		| (SDHC_WRCMD0_SDCLK_DLY(sdhc_reg->sdhc_wrcmd0_sdclk_dly))
+		| (SDHC_WRDATA1_DLY(sdhc_reg->sdhc_wrdata1_dly))
+		| (SDHC_WRDATA0_DLY(sdhc_reg->sdhc_wrdata0_dly))
+		| (SDHC_WRCMD1_DLY(sdhc_reg->sdhc_wrcmd1_dly))
+		| (SDHC_WRCMD0_DLY(sdhc_reg->sdhc_wrcmd0_dly));
+	ret = cdns_sdmmc_write_sd_host_reg(MMC_REG_BASE + SDHC_CDNS_HRS16, value);
+	if (ret != 0) {
+		ERROR("Program HRS16 failed");
+		return ret;
+	}
+
+	/* program HRS07, register 40 */
+	value = (SDHC_RW_COMPENSATE(sdhc_reg->sdhc_rw_compensate))
+		| (SDHC_IDELAY_VAL(sdhc_reg->sdhc_idelay_val));
+	ret = cdns_sdmmc_write_sd_host_reg(MMC_REG_BASE + SDHC_CDNS_HRS07, value);
+	if (ret != 0) {
+		ERROR("Program HRS07 failed");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int cdns_hc_set_clk(struct cdns_sdmmc_params *cdn_sdmmc_dev_mode_params)
+{
+	uint32_t ret = 0;
+	uint32_t dtcvval, sdclkfsval;
+
+	dtcvval = DTC_VAL;
+	sdclkfsval = 0;
+
+	if ((cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_DS) ||
+		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_SDR12) ||
+		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_SDR_BC)) {
+		sdclkfsval = 4;
+	} else if ((cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_HS) ||
+		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_SDR25) ||
+		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_DDR50) ||
+		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_SDR)) {
+		sdclkfsval = 2;
+	} else if ((cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_SDR50) ||
+		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_DDR) ||
+		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_HS400) ||
+		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_HS400es)) {
+		sdclkfsval = 1;
+	} else if ((cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_SDR104) ||
+		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_HS200)) {
+		sdclkfsval = 0;
+	}
+
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, 0);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (dtcvval << SDMMC_CDN_DTCV) |
+		(sdclkfsval << SDMMC_CDN_SDCLKFS) | (1 << SDMMC_CDN_ICE));
+	ret = cdns_wait_ics(5000, MMC_REG_BASE + SDHC_CDNS_SRS11);
+	if (ret != 0U) {
+		ERROR("Waiting SDMMC_CDN_ICS timeout");
+		return ret;
+	}
+
+	/* Enable DLL reset */
+	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_HRS09), mmio_read_32(MMC_REG_BASE
+			+ SDHC_CDNS_HRS09) & ~SDHC_DLL_RESET_MASK);
+	/* Set extended_wr_mode */
+	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_HRS09),
+	(mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09) &	SDHC_EXTENDED_WR_MODE_MASK) |
+			(1 << EXTENDED_WR_MODE));
+	/* Release DLL reset */
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE
+			+ SDHC_CDNS_HRS09) | 1);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE
+			+ SDHC_CDNS_HRS09) | (3 << RDCMD_EN));
+	do {
+		mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
+	} while (~mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09) & (1 << 1));
+
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (dtcvval << SDMMC_CDN_DTCV) |
+		(sdclkfsval << SDMMC_CDN_SDCLKFS) | (1 << SDMMC_CDN_ICE) | (1 << SDMMC_CDN_SDCE));
+
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS13, UINT_MAX);
+	return 0;
+}
+
+int cdns_reset(void)
+{
+	uint32_t data = 0;
+	uint32_t count = 0;
+	uint32_t value = 0;
+
+	value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS11);
+	value &= ~(0xFFFF);
+	value |= 0x0;
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, value);
+	udelay(500);
+
+	/* Software reset */
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS00, 1);
+	/* Wait status command response ready */
+	do {
+		data = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS00);
+		count++;
+		if (count >= 5000) {
+			return -ETIMEDOUT;
+		}
+	/* Wait for HRS00.SWR */
+	} while ((data & 1) == 1);
+
+	/* Step 1, switch on DLL_RESET */
+	value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
+	value &= ~SDHC_PHY_SW_RESET;
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, value);
+
+	return 0;
+}
+
+int cdns_sd_host_init(struct cdns_sdmmc_combo_phy *mmc_combo_phy_reg,
+struct cdns_sdmmc_sdhc *mmc_sdhc_reg)
+{
+	int ret = 0;
+
+	ret = cdns_reset();
+	if (ret != 0) {
+		ERROR("Program phy reg init failed");
+		return ret;
+	}
+
+	ret = cdns_program_phy_reg(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
+	if (ret != 0) {
+		ERROR("Program phy reg init failed");
+		return ret;
+	}
+
+	ret = cdns_init_hrs_io(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
+	if (ret != 0) {
+		ERROR("Program init for HRS reg is failed");
+		return ret;
+	}
+
+	ret = cdns_sd_card_detect();
+	if (ret != 0) {
+		ERROR("SD card does not detect");
+		return ret;
+	}
+
+	ret = cdns_vol_reset();
+	if (ret != 0) {
+		ERROR("eMMC card reset failed");
+		return ret;
+	}
+
+	ret = cdns_hc_set_clk(&cdns_params);
+	if (ret != 0) {
+		ERROR("hc set clk failed");
+		return ret;
+	}
+
+	return 0;
+}
+
+void cdns_srs10_value_toggle(uint8_t write_val, uint8_t prev_val)
+{
+	uint32_t data_op = 0U;
+
+	data_op = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS10);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS10, (data_op & (prev_val << 0)));
+	mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS10);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS10, data_op | (write_val << 0));
+}
+
+void cdns_srs11_srs15_config(uint32_t srs11_val, uint32_t srs15_val)
+{
+	uint32_t data = 0U;
+
+	data = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS11);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (data | srs11_val));
+	data = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS15);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS15, (data | srs15_val));
+}
+
+int cdns_send_cmd(struct mmc_cmd *cmd)
+{
+	uint32_t op = 0, ret = 0;
+	uint8_t write_value = 0, prev_val = 0;
+	uint32_t value;
+	int32_t timeout;
+	uint32_t cmd_indx;
+	uint32_t status = 0, srs15_val = 0, srs11_val = 0;
+	uint32_t status_check = 0;
+
+	assert(cmd);
+	cmd_indx = (cmd->cmd_idx) << COM_IDX;
+
+	if (data_cmd) {
+		switch (cmd->cmd_idx) {
+		case SD_SWITCH:
+			op = DATA_PRESENT;
+			write_value = ADMA2_32 | DT_WIDTH;
+			prev_val = ADMA2_32 | DT_WIDTH;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
+			srs15_val = BIT_AD_64 | HV4E | V18SE;
+			cdns_srs11_srs15_config(srs11_val, srs15_val);
+			break;
+
+		case SD_WRITE_SINGLE_BLOCK:
+		case SD_READ_SINGLE_BLOCK:
+			op = DATA_PRESENT;
+			write_value = ADMA2_32 | HS_EN | DT_WIDTH | LEDC;
+			prev_val = ADMA2_32 | HS_EN | DT_WIDTH;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			srs15_val = PVE | BIT_AD_64 | HV4E | SDR104_MODE | V18SE;
+			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
+			cdns_srs11_srs15_config(srs11_val, srs15_val);
+			mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS00, SAAR);
+			break;
+
+		case SD_WRITE_MULTIPLE_BLOCK:
+		case SD_READ_MULTIPLE_BLOCK:
+			op = DATA_PRESENT | AUTO_CMD_EN | MULTI_BLK_READ;
+			write_value = ADMA2_32 | HS_EN | DT_WIDTH | LEDC;
+			prev_val = ADMA2_32 | HS_EN | DT_WIDTH;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			srs15_val = PVE | BIT_AD_64 | HV4E | SDR104_MODE | V18SE;
+			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
+			cdns_srs11_srs15_config(srs11_val, srs15_val);
+			mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS00, SAAR);
+			break;
+
+		case SD_APP_SEND_SCR:
+			op = DATA_PRESENT;
+			write_value = ADMA2_32 | LEDC;
+			prev_val = LEDC;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			srs15_val = BIT_AD_64 | HV4E | V18SE;
+			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
+			cdns_srs11_srs15_config(srs11_val, srs15_val);
+			break;
+
+		case SD_SEND_IF_COND:
+			op = DATA_PRESENT | CMD_IDX_CHK_ENABLE;
+			write_value = LEDC;
+			prev_val = 0x0;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			srs15_val = HV4E;
+			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
+			cdns_srs11_srs15_config(srs11_val, srs15_val);
+			break;
+
+		default:
+			write_value = LEDC;
+			prev_val = 0x0;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			op = 0;
+			break;
+		}
+	} else {
+		switch (cmd->cmd_idx) {
+		case SD_GO_IDLE_STATE:
+			write_value = LEDC;
+			prev_val = 0x0;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			srs15_val = HV4E;
+			srs11_val = SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
+			cdns_srs11_srs15_config(srs11_val, srs15_val);
+			break;
+
+		case SD_ALL_SEND_CID:
+			write_value = LEDC;
+			prev_val = 0x0;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			srs15_val = HV4E | V18SE;
+			srs11_val = SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
+			cdns_srs11_srs15_config(srs11_val, srs15_val);
+			break;
+
+		case SD_SEND_IF_COND:
+			op = CMD_IDX_CHK_ENABLE;
+			write_value = LEDC;
+			prev_val = 0x0;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			srs15_val = HV4E;
+			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
+			cdns_srs11_srs15_config(srs11_val, srs15_val);
+			break;
+
+		case SD_STOP_TRANSMISSION:
+			op = CMD_STOP_ABORT_CMD;
+			break;
+
+		case SD_SEND_STATUS:
+			break;
+
+		case 1:
+			cmd->cmd_arg = 0;
+			break;
+
+		case SD_SELECT_CARD:
+			op = MULTI_BLK_READ;
+			break;
+
+		case SD_APP_CMD:
+		default:
+			write_value = LEDC;
+			prev_val = 0x0;
+			cdns_srs10_value_toggle(write_value, prev_val);
+			op = 0;
+			break;
+		}
+	}
+
+	switch (cmd->resp_type) {
+	case MMC_RESPONSE_NONE:
+		op |= CMD_READ | MULTI_BLK_READ | DMA_ENABLED | BLK_CNT_EN;
+		break;
+
+	case MMC_RESPONSE_R2:
+		op |= CMD_READ | MULTI_BLK_READ | DMA_ENABLED | BLK_CNT_EN |
+			RES_TYPE_SEL_136 | CMD_CHECK_RESP_CRC;
+		break;
+
+	case MMC_RESPONSE_R3:
+		op |= CMD_READ | MULTI_BLK_READ | DMA_ENABLED | BLK_CNT_EN |
+			RES_TYPE_SEL_48;
+		break;
+
+	case MMC_RESPONSE_R1:
+		if ((cmd->cmd_idx == SD_WRITE_SINGLE_BLOCK) || (cmd->cmd_idx
+			== SD_WRITE_MULTIPLE_BLOCK)) {
+			op |= DMA_ENABLED | BLK_CNT_EN | RES_TYPE_SEL_48
+			| CMD_CHECK_RESP_CRC | CMD_IDX_CHK_ENABLE;
+		} else {
+			op |= DMA_ENABLED | BLK_CNT_EN | CMD_READ | RES_TYPE_SEL_48
+			| CMD_CHECK_RESP_CRC | CMD_IDX_CHK_ENABLE;
+		}
+		break;
+
+	default:
+		op |= DMA_ENABLED | BLK_CNT_EN | CMD_READ | MULTI_BLK_READ |
+			RES_TYPE_SEL_48 | CMD_CHECK_RESP_CRC | CMD_IDX_CHK_ENABLE;
+		break;
+	}
+
+	timeout = TIMEOUT;
+	do {
+		udelay(100);
+		ret = cdns_busy();
+		if (--timeout <= 0) {
+			udelay(50);
+			panic();
+		}
+	} while (ret);
+
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS12, UINT_MAX);
+
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS02, cmd->cmd_arg);
+	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS14, 0x00000000);
+	if (cmd_indx == 1)
+		mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS03, SDHC_CDNS_SRS03_VALUE);
+	else
+		mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS03, op | cmd_indx);
+
+	timeout = TIMEOUT;
+	do {
+		udelay(500);
+		value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS12);
+	} while (((value & (INT_CMD_DONE | ERROR_INT)) == 0) && (timeout-- > 0));
+
+	timeout = TIMEOUT;
+
+	if (data_cmd) {
+		data_cmd = false;
+		do {
+			udelay(250);
+		} while (((value & TRAN_COMP) == 0) && (timeout-- > 0));
+	}
+
+	status_check = value & SRS12_ERR_MASK;
+	if (status_check != 0U) {
+		ERROR("SD host controller send command failed, SRS12 = %x", status);
+		return -1;
+	}
+
+	if ((op & RES_TYPE_SEL_48) || (op & RES_TYPE_SEL_136)) {
+		cmd->resp_data[0] = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS04);
+		if (op & RES_TYPE_SEL_136) {
+			cmd->resp_data[1] = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS05);
+			cmd->resp_data[2] = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS06);
+			cmd->resp_data[3] = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS07);
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/cadence/nand/cdns_nand.c b/drivers/cadence/nand/cdns_nand.c
new file mode 100644
index 0000000..5a66262
--- /dev/null
+++ b/drivers/cadence/nand/cdns_nand.c
@@ -0,0 +1,435 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/cadence/cdns_nand.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <platform_def.h>
+
+/* NAND flash device information struct */
+static cnf_dev_info_t dev_info;
+
+/* Scratch buffers for read and write operations */
+static uint8_t scratch_buff[PLATFORM_MTD_MAX_PAGE_SIZE];
+
+/* Wait for controller to be in idle state */
+static inline void cdns_nand_wait_idle(void)
+{
+	uint32_t reg = 0U;
+
+	do {
+		udelay(CNF_DEF_DELAY_US);
+		reg = mmio_read_32(CNF_CMDREG(CTRL_STATUS));
+	} while (CNF_GET_CTRL_BUSY(reg) != 0U);
+}
+
+/* Wait for given thread to be in ready state */
+static inline void cdns_nand_wait_thread_ready(uint8_t thread_id)
+{
+	uint32_t reg = 0U;
+
+	do {
+		udelay(CNF_DEF_DELAY_US);
+		reg = mmio_read_32(CNF_CMDREG(TRD_STATUS));
+		reg &= (1U << (uint32_t)thread_id);
+	} while (reg != 0U);
+}
+
+/* Check if the last operation/command in selected thread is completed */
+static int cdns_nand_last_opr_status(uint8_t thread_id)
+{
+	uint8_t nthreads = 0U;
+	uint32_t reg = 0U;
+
+	/* Get number of threads */
+	reg = mmio_read_32(CNF_CTRLPARAM(FEATURE));
+	nthreads = CNF_GET_NTHREADS(reg);
+
+	if (thread_id > nthreads) {
+		ERROR("%s: Invalid thread ID\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Select thread */
+	mmio_write_32(CNF_CMDREG(CMD_STAT_PTR), (uint32_t)thread_id);
+
+	uint32_t err_mask = CNF_ECMD | CNF_EECC | CNF_EDEV | CNF_EDQS | CNF_EFAIL |
+				CNF_EBUS | CNF_EDI | CNF_EPAR | CNF_ECTX | CNF_EPRO;
+
+	do {
+		udelay(CNF_DEF_DELAY_US * 2);
+		reg = mmio_read_32(CNF_CMDREG(CMD_STAT));
+	} while ((reg & CNF_CMPLT) == 0U);
+
+	/* last operation is completed, make sure no other error bits are set */
+	if ((reg & err_mask) == 1U) {
+		ERROR("%s, CMD_STATUS:0x%x\n", __func__, reg);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/* Set feature command */
+int cdns_nand_set_feature(uint8_t feat_addr, uint8_t feat_val, uint8_t thread_id)
+{
+	/* Wait for thread to be ready */
+	cdns_nand_wait_thread_ready(thread_id);
+
+	/* Set feature address */
+	mmio_write_32(CNF_CMDREG(CMD_REG1), (uint32_t)feat_addr);
+	/* Set feature volume */
+	mmio_write_32(CNF_CMDREG(CMD_REG2), (uint32_t)feat_val);
+
+	/* Set feature command */
+	uint32_t reg = (CNF_WORK_MODE_PIO << CNF_CMDREG0_CT);
+
+	reg |= (thread_id << CNF_CMDREG0_TRD);
+	reg |= (CNF_DEF_VOL_ID << CNF_CMDREG0_VOL);
+	reg |= (CNF_INT_DIS << CNF_CMDREG0_INTR);
+	reg |= (CNF_CT_SET_FEATURE << CNF_CMDREG0_CMD);
+	mmio_write_32(CNF_CMDREG(CMD_REG0), reg);
+
+	return cdns_nand_last_opr_status(thread_id);
+}
+
+/* Reset command to the selected device */
+int cdns_nand_reset(uint8_t thread_id)
+{
+	/* Operation is executed in selected thread */
+	cdns_nand_wait_thread_ready(thread_id);
+
+	/* Select memory */
+	mmio_write_32(CNF_CMDREG(CMD_REG4), (CNF_DEF_DEVICE << CNF_CMDREG4_MEM));
+
+	/* Issue reset command */
+	uint32_t reg = (CNF_WORK_MODE_PIO << CNF_CMDREG0_CT);
+
+	reg |= (thread_id << CNF_CMDREG0_TRD);
+	reg |= (CNF_DEF_VOL_ID << CNF_CMDREG0_VOL);
+	reg |= (CNF_INT_DIS << CNF_CMDREG0_INTR);
+	reg |= (CNF_CT_RESET_ASYNC << CNF_CMDREG0_CMD);
+	mmio_write_32(CNF_CMDREG(CMD_REG0), reg);
+
+	return cdns_nand_last_opr_status(thread_id);
+}
+
+/* Set operation work mode */
+static void cdns_nand_set_opr_mode(uint8_t opr_mode)
+{
+	/* Wait for controller to be in idle state */
+	cdns_nand_wait_idle();
+
+	/* Reset DLL PHY */
+	uint32_t reg = mmio_read_32(CNF_MINICTRL(DLL_PHY_CTRL));
+
+	reg &= ~(1 << CNF_DLL_PHY_RST_N);
+	mmio_write_32(CNF_MINICTRL(DLL_PHY_CTRL), reg);
+
+	if (opr_mode == CNF_OPR_WORK_MODE_SDR) {
+		/* Combo PHY Control Timing Block register settings */
+		mmio_write_32(CP_CTB(CTRL_REG), CP_CTRL_REG_SDR);
+		mmio_write_32(CP_CTB(TSEL_REG), CP_TSEL_REG_SDR);
+
+		/* Combo PHY DLL register settings */
+		mmio_write_32(CP_DLL(DQ_TIMING_REG), CP_DQ_TIMING_REG_SDR);
+		mmio_write_32(CP_DLL(DQS_TIMING_REG), CP_DQS_TIMING_REG_SDR);
+		mmio_write_32(CP_DLL(GATE_LPBK_CTRL_REG), CP_GATE_LPBK_CTRL_REG_SDR);
+		mmio_write_32(CP_DLL(MASTER_CTRL_REG), CP_DLL_MASTER_CTRL_REG_SDR);
+
+		/* Async mode timing settings */
+		mmio_write_32(CNF_MINICTRL(ASYNC_TOGGLE_TIMINGS),
+								(2 << CNF_ASYNC_TIMINGS_TRH) |
+								(4 << CNF_ASYNC_TIMINGS_TRP) |
+								(2 << CNF_ASYNC_TIMINGS_TWH) |
+								(4 << CNF_ASYNC_TIMINGS_TWP));
+
+		/* Set extended read and write mode */
+		reg |= (1 << CNF_DLL_PHY_EXT_RD_MODE);
+		reg |= (1 << CNF_DLL_PHY_EXT_WR_MODE);
+
+		/* Set operation work mode in common settings */
+		uint32_t data = mmio_read_32(CNF_MINICTRL(CMN_SETTINGS));
+
+		data |= (CNF_OPR_WORK_MODE_SDR << CNF_CMN_SETTINGS_OPR);
+		mmio_write_32(CNF_MINICTRL(CMN_SETTINGS), data);
+
+	} else if (opr_mode == CNF_OPR_WORK_MODE_NVDDR) {
+		; /* ToDo: add DDR mode settings also once available on SIMICS */
+	} else {
+		;
+	}
+
+	reg |= (1 << CNF_DLL_PHY_RST_N);
+	mmio_write_32(CNF_MINICTRL(DLL_PHY_CTRL), reg);
+}
+
+/* Data transfer configuration */
+static void cdns_nand_transfer_config(void)
+{
+	/* Wait for controller to be in idle state */
+	cdns_nand_wait_idle();
+
+	/* Configure data transfer parameters */
+	mmio_write_32(CNF_CTRLCFG(TRANS_CFG0), 1);
+
+	/* ECC is disabled */
+	mmio_write_32(CNF_CTRLCFG(ECC_CFG0), 0);
+
+	/* DMA burst select */
+	mmio_write_32(CNF_CTRLCFG(DMA_SETTINGS),
+					(CNF_DMA_BURST_SIZE_MAX << CNF_DMA_SETTINGS_BURST) |
+					(1 << CNF_DMA_SETTINGS_OTE));
+
+	/* Enable pre-fetching for 1K */
+	mmio_write_32(CNF_CTRLCFG(FIFO_TLEVEL),
+					(CNF_DMA_PREFETCH_SIZE << CNF_FIFO_TLEVEL_POS) |
+					(CNF_DMA_PREFETCH_SIZE << CNF_FIFO_TLEVEL_DMA_SIZE));
+
+	/* Select access type */
+	mmio_write_32(CNF_CTRLCFG(MULTIPLANE_CFG), 0);
+	mmio_write_32(CNF_CTRLCFG(CACHE_CFG), 0);
+}
+
+/* Update the nand flash device info */
+static int cdns_nand_update_dev_info(void)
+{
+	uint32_t reg = 0U;
+
+	/* Read the device type and number of LUNs */
+	reg = mmio_read_32(CNF_CTRLPARAM(DEV_PARAMS0));
+	dev_info.type = CNF_GET_DEV_TYPE(reg);
+	if (dev_info.type == CNF_DT_UNKNOWN) {
+		ERROR("%s: device type unknown\n", __func__);
+		return -ENXIO;
+	}
+	dev_info.nluns = CNF_GET_NLUNS(reg);
+
+	/* Pages per block */
+	reg = mmio_read_32(CNF_CTRLCFG(DEV_LAYOUT));
+	dev_info.npages_per_block = CNF_GET_NPAGES_PER_BLOCK(reg);
+
+	/* Sector size and last sector size */
+	reg = mmio_read_32(CNF_CTRLCFG(TRANS_CFG1));
+	dev_info.sector_size = CNF_GET_SCTR_SIZE(reg);
+	dev_info.last_sector_size = CNF_GET_LAST_SCTR_SIZE(reg);
+
+	/* Page size and spare size */
+	reg = mmio_read_32(CNF_CTRLPARAM(DEV_AREA));
+	dev_info.page_size = CNF_GET_PAGE_SIZE(reg);
+	dev_info.spare_size = CNF_GET_SPARE_SIZE(reg);
+
+	/* Device blocks per LUN */
+	dev_info.nblocks_per_lun = mmio_read_32(CNF_CTRLPARAM(DEV_BLOCKS_PLUN));
+
+	/* Calculate block size and total device size */
+	dev_info.block_size = (dev_info.npages_per_block * dev_info.page_size);
+	dev_info.total_size = (dev_info.block_size * dev_info.nblocks_per_lun *
+							dev_info.nluns);
+
+	VERBOSE("CNF params: page %d, spare %d, block %d, total %lld\n",
+				dev_info.page_size, dev_info.spare_size,
+				dev_info.block_size, dev_info.total_size);
+
+	return 0;
+}
+
+/* NAND Flash Controller/Host initialization */
+int cdns_nand_host_init(void)
+{
+	uint32_t reg = 0U;
+	int ret = 0;
+
+	do {
+		/* Read controller status register for init complete */
+		reg = mmio_read_32(CNF_CMDREG(CTRL_STATUS));
+	} while (CNF_GET_INIT_COMP(reg) == 0);
+
+	ret = cdns_nand_update_dev_info();
+	if (ret != 0) {
+		return ret;
+	}
+
+	INFO("CNF: device discovery process completed and device type %d\n",
+			dev_info.type);
+
+	/* Enable data integrity, enable CRC and parity */
+	reg = mmio_read_32(CNF_DI(CONTROL));
+	reg |= (1 << CNF_DI_PAR_EN);
+	reg |= (1 << CNF_DI_CRC_EN);
+	mmio_write_32(CNF_DI(CONTROL), reg);
+
+	/* Status polling mode, device control and status register */
+	cdns_nand_wait_idle();
+	reg = mmio_read_32(CNF_CTRLCFG(DEV_STAT));
+	reg = reg & ~1;
+	mmio_write_32(CNF_CTRLCFG(DEV_STAT), reg);
+
+	/* Set operation work mode */
+	cdns_nand_set_opr_mode(CNF_OPR_WORK_MODE_SDR);
+
+	/* Set data transfer configuration parameters */
+	cdns_nand_transfer_config();
+
+	return 0;
+}
+
+/* erase: Block erase command */
+int cdns_nand_erase(uint32_t offset, uint32_t size)
+{
+	/* Determine the starting block offset i.e row address */
+	uint32_t row_address = dev_info.npages_per_block * offset;
+
+	/* Wait for thread to be in ready state */
+	cdns_nand_wait_thread_ready(CNF_DEF_TRD);
+
+	/*Set row address */
+	mmio_write_32(CNF_CMDREG(CMD_REG1), row_address);
+
+	/* Operation bank number */
+	mmio_write_32(CNF_CMDREG(CMD_REG4), (CNF_DEF_DEVICE << CNF_CMDREG4_MEM));
+
+	/* Block erase command */
+	uint32_t reg = (CNF_WORK_MODE_PIO << CNF_CMDREG0_CT);
+
+	reg |= (CNF_DEF_TRD << CNF_CMDREG0_TRD);
+	reg |= (CNF_DEF_VOL_ID << CNF_CMDREG0_VOL);
+	reg |= (CNF_INT_DIS << CNF_CMDREG0_INTR);
+	reg |= (CNF_CT_ERASE << CNF_CMDREG0_CMD);
+	reg |= (((size-1) & 0xFF) << CNF_CMDREG0_CMD);
+	mmio_write_32(CNF_CMDREG(CMD_REG0), reg);
+
+	/* Wait for erase operation to complete */
+	return cdns_nand_last_opr_status(CNF_DEF_TRD);
+}
+
+/* io mtd functions */
+int cdns_nand_init_mtd(unsigned long long *size, unsigned int *erase_size)
+{
+	*size = dev_info.total_size;
+	*erase_size = dev_info.block_size;
+
+	return 0;
+}
+
+/* NAND Flash page read */
+static int cdns_nand_read_page(uint32_t block, uint32_t page, uintptr_t buffer)
+{
+	/* Wait for thread to be ready */
+	cdns_nand_wait_thread_ready(CNF_DEF_TRD);
+
+	/* Select device */
+	mmio_write_32(CNF_CMDREG(CMD_REG4),
+					(CNF_DEF_DEVICE << CNF_CMDREG4_MEM));
+
+	/* Set host memory address for DMA transfers */
+	mmio_write_32(CNF_CMDREG(CMD_REG2), (buffer & 0xFFFF));
+	mmio_write_32(CNF_CMDREG(CMD_REG3), ((buffer >> 32) & 0xFFFF));
+
+	/* Set row address */
+	uint32_t row_address = 0U;
+
+	row_address |= ((page & 0x3F) | (block << 6));
+	mmio_write_32(CNF_CMDREG(CMD_REG1), row_address);
+
+	/* Page read command */
+	uint32_t reg = (CNF_WORK_MODE_PIO << CNF_CMDREG0_CT);
+
+	reg |= (CNF_DEF_TRD << CNF_CMDREG0_TRD);
+	reg |= (CNF_DEF_VOL_ID << CNF_CMDREG0_VOL);
+	reg |= (CNF_INT_DIS << CNF_CMDREG0_INTR);
+	reg |= (CNF_DMA_MASTER_SEL << CNF_CMDREG0_DMA);
+	reg |= (CNF_CT_PAGE_READ << CNF_CMDREG0_CMD);
+	reg |= (((CNF_READ_SINGLE_PAGE-1) & 0xFF) << CNF_CMDREG0_CMD);
+	mmio_write_32(CNF_CMDREG(CMD_REG0), reg);
+
+	/* Wait for read operation to complete */
+	if (cdns_nand_last_opr_status(CNF_DEF_TRD)) {
+		ERROR("%s: Page read failed\n", __func__);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int cdns_nand_read(unsigned int offset, uintptr_t buffer, size_t length,
+					size_t *out_length)
+{
+	uint32_t block = offset / dev_info.block_size;
+	uint32_t end_block = (offset + length - 1U) / dev_info.block_size;
+	uint32_t page_start = (offset % dev_info.block_size) / dev_info.page_size;
+	uint32_t start_offset = offset % dev_info.page_size;
+	uint32_t nb_pages = dev_info.block_size / dev_info.page_size;
+	uint32_t bytes_read = 0U;
+	uint32_t page = 0U;
+	int result = 0;
+
+	VERBOSE("CNF: block %u-%u, page_start %u, len %zu, offset %u\n",
+				block, end_block, page_start, length, offset);
+
+	if ((offset >= dev_info.total_size) ||
+		(offset + length-1 >= dev_info.total_size) ||
+		(length == 0U)) {
+		ERROR("CNF: Invalid read parameters\n");
+		return -EINVAL;
+	}
+
+	*out_length = 0UL;
+
+	while (block <= end_block) {
+		for (page = page_start; page < nb_pages; page++) {
+			if ((start_offset != 0U) || (length < dev_info.page_size)) {
+				/* Partial page read */
+				result = cdns_nand_read_page(block, page,
+				(uintptr_t)scratch_buff);
+				if (result != 0) {
+					return result;
+				}
+
+				bytes_read = MIN((size_t)(dev_info.page_size - start_offset),
+								length);
+
+				memcpy((uint8_t *)buffer, scratch_buff + start_offset,
+						bytes_read);
+				start_offset = 0U;
+			} else {
+				/* Full page read */
+				result = cdns_nand_read_page(block, page,
+				(uintptr_t)scratch_buff);
+				if (result != 0) {
+					return result;
+				}
+
+				bytes_read = dev_info.page_size;
+				memcpy((uint8_t *)buffer, scratch_buff, bytes_read);
+			}
+
+			length -= bytes_read;
+			buffer += bytes_read;
+			*out_length += bytes_read;
+
+			/* All the bytes have read */
+			if (length == 0U) {
+				break;
+			}
+
+			udelay(CNF_READ_INT_DELAY_US);
+		} /* for */
+
+		page_start = 0U;
+		block++;
+	} /* while */
+
+	return 0;
+}
diff --git a/drivers/measured_boot/rss/rss_measured_boot.c b/drivers/measured_boot/rss/rss_measured_boot.c
index cf545a7..1b2f177 100644
--- a/drivers/measured_boot/rss/rss_measured_boot.c
+++ b/drivers/measured_boot/rss/rss_measured_boot.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -32,23 +32,10 @@
 #  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)
+void rss_measured_boot_init(struct rss_mboot_metadata *metadata_ptr)
 {
-	/* At this point it is expected that communication channel over MHU
-	 * is already initialised by platform init.
-	 */
-	struct rss_mboot_metadata *metadata_ptr;
-
-	/* Get pointer to platform's struct rss_mboot_metadata structure */
-	plat_metadata_ptr = plat_rss_mboot_get_metadata();
-	assert(plat_metadata_ptr != NULL);
-
-	/* Use a local variable to preserve the value of the global pointer */
-	metadata_ptr = plat_metadata_ptr;
+	assert(metadata_ptr != NULL);
 
 	/* Init the non-const members of the metadata structure */
 	while (metadata_ptr->id != RSS_MBOOT_INVALID_ID) {
@@ -58,13 +45,15 @@
 	}
 }
 
-int rss_mboot_measure_and_record(uintptr_t data_base, uint32_t data_size,
+int rss_mboot_measure_and_record(struct rss_mboot_metadata *metadata_ptr,
+				 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;
+
+	assert(metadata_ptr != NULL);
 
 	/* Get the metadata associated with this image. */
 	while ((metadata_ptr->id != RSS_MBOOT_INVALID_ID) &&
@@ -103,14 +92,16 @@
 	return 0;
 }
 
-int rss_mboot_set_signer_id(unsigned int img_id,
+int rss_mboot_set_signer_id(struct rss_mboot_metadata *metadata_ptr,
+			    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;
 
+	assert(metadata_ptr != NULL);
+
 	/* Get the metadata associated with this image. */
 	while ((metadata_ptr->id != RSS_MBOOT_INVALID_ID) &&
 		(metadata_ptr->id != img_id)) {
diff --git a/drivers/nxp/trdc/imx_trdc.c b/drivers/nxp/trdc/imx_trdc.c
new file mode 100644
index 0000000..ab66585
--- /dev/null
+++ b/drivers/nxp/trdc/imx_trdc.c
@@ -0,0 +1,365 @@
+/*
+ * Copyright 2022-2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/nxp/trdc/imx_trdc.h>
+#include <lib/mmio.h>
+
+
+int trdc_mda_set_cpu(uintptr_t trdc_base, uint32_t mda_inst,
+		     uint32_t mda_reg, uint8_t sa, uint8_t dids,
+		     uint8_t did, uint8_t pe, uint8_t pidm, uint8_t pid)
+{
+	uint32_t val = mmio_read_32(trdc_base + MDAC_W_X(mda_inst, mda_reg));
+	/* invalid: config non-cpu master with cpu config format. */
+	if ((val & MDA_DFMT) != 0U) {
+		return -EINVAL;
+	}
+
+	val = MDA_VLD | MDA_DFMT0_DID(pid) | MDA_DFMT0_PIDM(pidm) | MDA_DFMT0_PE(pe) |
+	      MDA_DFMT0_SA(sa) | MDA_DFMT0_DIDS(dids) | MDA_DFMT0_DID(did);
+
+	mmio_write_32(trdc_base + MDAC_W_X(mda_inst, mda_reg), val);
+
+	return 0;
+}
+
+int trdc_mda_set_noncpu(uintptr_t trdc_base, uint32_t mda_inst,
+			bool did_bypass, uint8_t sa, uint8_t pa,
+			uint8_t did)
+{
+	uint32_t val = mmio_read_32(trdc_base + MDAC_W_X(mda_inst, 0));
+
+	/* invalid: config cpu master with non-cpu config format. */
+	if ((val & MDA_DFMT) == 0U) {
+		return -EINVAL;
+	}
+
+	val = MDA_VLD | MDA_DFMT1_SA(sa) | MDA_DFMT1_PA(pa) | MDA_DFMT1_DID(did) |
+	      MDA_DFMT1_DIDB(did_bypass ? 1U : 0U);
+
+	mmio_write_32(trdc_base + MDAC_W_X(mda_inst, 0), val);
+
+	return 0;
+}
+
+static uintptr_t trdc_get_mbc_base(uintptr_t trdc_reg, uint32_t mbc_x)
+{
+	struct trdc_mgr *trdc_base = (struct trdc_mgr *)trdc_reg;
+	uint32_t mbc_num = MBC_NUM(trdc_base->trdc_hwcfg0);
+
+	if (mbc_x >= mbc_num) {
+		return 0U;
+	}
+
+	return trdc_reg + 0x10000 + 0x2000 * mbc_x;
+}
+
+static uintptr_t trdc_get_mrc_base(uintptr_t trdc_reg, uint32_t mrc_x)
+{
+	struct trdc_mgr *trdc_base = (struct trdc_mgr *)trdc_reg;
+	uint32_t mbc_num = MBC_NUM(trdc_base->trdc_hwcfg0);
+	uint32_t mrc_num = MRC_NUM(trdc_base->trdc_hwcfg0);
+
+	if (mrc_x >= mrc_num) {
+		return 0U;
+	}
+
+	return trdc_reg + 0x10000 + 0x2000 * mbc_num + 0x1000 * mrc_x;
+}
+
+uint32_t trdc_mbc_blk_num(uintptr_t trdc_reg, uint32_t mbc_x, uint32_t mem_x)
+{
+	uint32_t glbcfg;
+	struct mbc_mem_dom *mbc_dom;
+	struct trdc_mbc *mbc_base = (struct trdc_mbc *)trdc_get_mbc_base(trdc_reg, mbc_x);
+
+	if (mbc_base == NULL) {
+		return 0;
+	}
+
+	/* only first dom has the glbcfg */
+	mbc_dom = &mbc_base->mem_dom[0];
+	glbcfg = mmio_read_32((uintptr_t)&mbc_dom->mem_glbcfg[mem_x]);
+
+	return MBC_BLK_NUM(glbcfg);
+}
+
+uint32_t trdc_mrc_rgn_num(uintptr_t trdc_reg, uint32_t mrc_x)
+{
+	uint32_t glbcfg;
+	struct mrc_rgn_dom *mrc_dom;
+	struct trdc_mrc *mrc_base = (struct trdc_mrc *)trdc_get_mrc_base(trdc_reg, mrc_x);
+
+	if (mrc_base == NULL) {
+		return 0;
+	}
+
+	/* only first dom has the glbcfg */
+	mrc_dom = &mrc_base->mrc_dom[0];
+	glbcfg = mmio_read_32((uintptr_t)&mrc_dom->mrc_glbcfg[0]);
+
+	return MBC_BLK_NUM(glbcfg);
+}
+
+int trdc_mbc_set_control(uintptr_t trdc_reg, uint32_t mbc_x,
+			 uint32_t glbac_id, uint32_t glbac_val)
+{
+	struct mbc_mem_dom *mbc_dom;
+	struct trdc_mbc *mbc_base = (struct trdc_mbc *)trdc_get_mbc_base(trdc_reg, mbc_x);
+
+	if (mbc_base == NULL || glbac_id >= GLBAC_NUM) {
+		return -EINVAL;
+	}
+
+	/* only first dom has the glbac */
+	mbc_dom = &mbc_base->mem_dom[0];
+
+	mmio_write_32((uintptr_t)&mbc_dom->memn_glbac[glbac_id], glbac_val);
+
+	return 0;
+}
+
+int trdc_mbc_blk_config(uintptr_t trdc_reg, uint32_t mbc_x,
+			uint32_t dom_x, uint32_t mem_x, uint32_t blk_x,
+			bool sec_access, uint32_t glbac_id)
+{
+	uint32_t *cfg_w;
+	uint32_t index, offset, val;
+	struct mbc_mem_dom *mbc_dom;
+	struct trdc_mbc *mbc_base = (struct trdc_mbc *)trdc_get_mbc_base(trdc_reg, mbc_x);
+
+	if (mbc_base == NULL || glbac_id >= GLBAC_NUM) {
+		return -EINVAL;
+	}
+
+	mbc_dom = &mbc_base->mem_dom[dom_x];
+
+	switch (mem_x) {
+	case 0:
+		cfg_w = &mbc_dom->mem0_blk_cfg_w[blk_x / 8];
+		break;
+	case 1:
+		cfg_w = &mbc_dom->mem1_blk_cfg_w[blk_x / 8];
+		break;
+	case 2:
+		cfg_w = &mbc_dom->mem2_blk_cfg_w[blk_x / 8];
+		break;
+	case 3:
+		cfg_w = &mbc_dom->mem3_blk_cfg_w[blk_x / 8];
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	index = blk_x % 8;
+	offset = index * 4;
+
+	val = mmio_read_32((uintptr_t)cfg_w);
+	val &= ~(0xF << offset);
+
+	/*
+	 * MBC0-3
+	 * Global 0, 0x7777 secure pri/user read/write/execute,
+	 * S400 has already set it. So select MBC0_MEMN_GLBAC0
+	 */
+	if (sec_access) {
+		val |= ((0x0 | (glbac_id & 0x7)) << offset);
+		mmio_write_32((uintptr_t)cfg_w, val);
+	} else {
+		/* nse bit set */
+		val |= ((0x8 | (glbac_id & 0x7)) << offset);
+		mmio_write_32((uintptr_t)cfg_w, val);
+	}
+
+	return 0;
+}
+
+int trdc_mrc_set_control(uintptr_t trdc_reg, uint32_t mrc_x,
+			 uint32_t glbac_id, uint32_t glbac_val)
+{
+	struct mrc_rgn_dom *mrc_dom;
+	struct trdc_mrc *mrc_base = (struct trdc_mrc *)trdc_get_mrc_base(trdc_reg, mrc_x);
+
+	if (mrc_base == NULL || glbac_id >= GLBAC_NUM) {
+		return -EINVAL;
+	}
+
+	/* only first dom has the glbac */
+	mrc_dom = &mrc_base->mrc_dom[0];
+
+	mmio_write_32((uintptr_t)&mrc_dom->memn_glbac[glbac_id], glbac_val);
+
+	return 0;
+}
+
+int trdc_mrc_rgn_config(uintptr_t trdc_reg, uint32_t mrc_x,
+			uint32_t dom_x, uint32_t rgn_id,
+			uint32_t addr_start, uint32_t addr_size,
+			bool sec_access, uint32_t glbac_id)
+{
+	uint32_t *desc_w;
+	uint32_t addr_end;
+	struct mrc_rgn_dom *mrc_dom;
+	struct trdc_mrc *mrc_base = (struct trdc_mrc *)trdc_get_mrc_base(trdc_reg, mrc_x);
+
+	if (mrc_base == NULL || glbac_id >= GLBAC_NUM || rgn_id >= MRC_REG_ALL) {
+		return -EINVAL;
+	}
+
+	mrc_dom = &mrc_base->mrc_dom[dom_x];
+
+	addr_end = addr_start + addr_size - 1;
+	addr_start &= ~0x3fff;
+	addr_end &= ~0x3fff;
+
+	desc_w = &mrc_dom->rgn_desc_words[rgn_id][0];
+
+	if (sec_access) {
+		mmio_write_32((uintptr_t)desc_w, addr_start | (glbac_id & 0x7));
+		mmio_write_32((uintptr_t)(desc_w + 1), addr_end | 0x1);
+	} else {
+		mmio_write_32((uintptr_t)desc_w, addr_start | (glbac_id & 0x7));
+		mmio_write_32((uintptr_t)(desc_w + 1), (addr_end | 0x1 | 0x10));
+	}
+
+	return 0;
+}
+
+bool trdc_mrc_enabled(uintptr_t mrc_base)
+{
+	return (mmio_read_32(mrc_base) & BIT(15));
+}
+
+bool trdc_mbc_enabled(uintptr_t mbc_base)
+{
+	return (mmio_read_32(mbc_base) & BIT(14));
+}
+
+static bool is_trdc_mgr_slot(uintptr_t trdc_base, uint8_t mbc_id,
+			     uint8_t mem_id, uint16_t blk_id)
+{
+	unsigned int i;
+
+	for (i = 0U; i < trdc_mgr_num; i++) {
+		if (trdc_mgr_blks[i].trdc_base == trdc_base) {
+			if (mbc_id == trdc_mgr_blks[i].mbc_id &&
+			    mem_id == trdc_mgr_blks[i].mbc_mem_id &&
+			   (blk_id == trdc_mgr_blks[i].blk_mgr ||
+			    blk_id == trdc_mgr_blks[i].blk_mc)) {
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+/*
+ * config the TRDC MGR & MC's access policy. only the secure privilege
+ * mode SW can access it.
+ */
+void trdc_mgr_mbc_setup(struct trdc_mgr_info *mgr)
+{
+	unsigned int i;
+
+	/*
+	 * If the MBC is global enabled, need to cconfigure the MBCs of
+	 * TRDC MGR & MC correctly.
+	 */
+	if (trdc_mbc_enabled(mgr->trdc_base)) {
+		/* ONLY secure privilige can access */
+		trdc_mbc_set_control(mgr->trdc_base, mgr->mbc_id, 7, 0x6000);
+		for (i = 0U; i < 16U; i++) {
+			trdc_mbc_blk_config(mgr->trdc_base, mgr->mbc_id, i,
+				mgr->mbc_mem_id, mgr->blk_mgr, true, 7);
+
+			trdc_mbc_blk_config(mgr->trdc_base, mgr->mbc_id, i,
+				mgr->mbc_mem_id, mgr->blk_mc, true, 7);
+		}
+	}
+}
+
+/*
+ * Set up the TRDC access policy for all the resources under
+ * the TRDC control.
+ */
+void trdc_setup(struct trdc_config_info *cfg)
+{
+	unsigned int i, j, num;
+	bool is_mgr;
+
+	/* config the MRCs */
+	if (trdc_mrc_enabled(cfg->trdc_base)) {
+		/* set global access policy */
+		for (i = 0U; i < cfg->num_mrc_glbac; i++) {
+			trdc_mrc_set_control(cfg->trdc_base,
+					     cfg->mrc_glbac[i].mbc_mrc_id,
+					     cfg->mrc_glbac[i].glbac_id,
+					     cfg->mrc_glbac[i].glbac_val);
+		}
+
+		/* set each MRC region access policy */
+		for (i = 0U; i < cfg->num_mrc_cfg; i++) {
+			trdc_mrc_rgn_config(cfg->trdc_base, cfg->mrc_cfg[i].mrc_id,
+					    cfg->mrc_cfg[i].dom_id,
+					    cfg->mrc_cfg[i].region_id,
+					    cfg->mrc_cfg[i].region_start,
+					    cfg->mrc_cfg[i].region_size,
+					    cfg->mrc_cfg[i].secure,
+					    cfg->mrc_cfg[i].glbac_id);
+		}
+	}
+
+	/* config the MBCs */
+	if (trdc_mbc_enabled(cfg->trdc_base)) {
+		/* set MBC global access policy */
+		for (i = 0U; i < cfg->num_mbc_glbac; i++) {
+			trdc_mbc_set_control(cfg->trdc_base,
+					     cfg->mbc_glbac[i].mbc_mrc_id,
+					     cfg->mbc_glbac[i].glbac_id,
+					     cfg->mbc_glbac[i].glbac_val);
+		}
+
+		for (i = 0U; i < cfg->num_mbc_cfg; i++) {
+			if (cfg->mbc_cfg[i].blk_id == MBC_BLK_ALL) {
+				num = trdc_mbc_blk_num(cfg->trdc_base,
+						       cfg->mbc_cfg[i].mbc_id,
+						       cfg->mbc_cfg[i].mem_id);
+
+				for (j = 0U; j < num; j++) {
+					/* Skip mgr and mc */
+					is_mgr = is_trdc_mgr_slot(cfg->trdc_base,
+								  cfg->mbc_cfg[i].mbc_id,
+								  cfg->mbc_cfg[i].mem_id, j);
+					if (is_mgr) {
+						continue;
+					}
+
+					trdc_mbc_blk_config(cfg->trdc_base,
+							    cfg->mbc_cfg[i].mbc_id,
+							    cfg->mbc_cfg[i].dom_id,
+							    cfg->mbc_cfg[i].mem_id, j,
+							    cfg->mbc_cfg[i].secure,
+							    cfg->mbc_cfg[i].glbac_id);
+				}
+			} else {
+				trdc_mbc_blk_config(cfg->trdc_base,
+						    cfg->mbc_cfg[i].mbc_id,
+						    cfg->mbc_cfg[i].dom_id,
+						    cfg->mbc_cfg[i].mem_id,
+						    cfg->mbc_cfg[i].blk_id,
+						    cfg->mbc_cfg[i].secure,
+						    cfg->mbc_cfg[i].glbac_id);
+			}
+		}
+	}
+}
diff --git a/drivers/st/uart/aarch32/stm32_console.S b/drivers/st/uart/aarch32/stm32_console.S
index d64a6cd..e063941 100644
--- a/drivers/st/uart/aarch32/stm32_console.S
+++ b/drivers/st/uart/aarch32/stm32_console.S
@@ -236,7 +236,7 @@
 #endif /* ENABLE_ASSERTIONS */
 	/* Skip flush if UART is not enabled */
 	ldr	r1, [r0, #USART_CR1]
-	ands	r1, r1, #USART_CR1_UE
+	tst	r1, #USART_CR1_UE
 	beq	1f
 	/* Check Transmit Data Register Empty */
 txe_loop_3:
diff --git a/fdts/morello-coresight.dtsi b/fdts/morello-coresight.dtsi
new file mode 100644
index 0000000..ed3e9e5
--- /dev/null
+++ b/fdts/morello-coresight.dtsi
@@ -0,0 +1,361 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+
+/ {
+	/*
+	 * Morello TRMs specify the size for these coresight components as 64K.
+	 * The actual size is just 4K though 64K is reserved. Access to the
+	 * unmapped reserved region results in a DECERR response.
+	 */
+	cpu_debug0: cpu-debug@402010000 {
+		compatible = "arm,coresight-cpu-debug", "arm,primecell";
+		cpu = <&cpu0>;
+		reg = <0x4 0x02010000 0x0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+	};
+
+	etm0: etm@402040000 {
+		compatible = "arm,coresight-etm4x", "arm,primecell";
+		cpu = <&cpu0>;
+		reg = <0x4 0x02040000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		out-ports {
+			port {
+				cluster0_etm0_out_port: endpoint {
+					remote-endpoint = <&cluster0_static_funnel_in_port0>;
+				};
+			};
+		};
+	};
+
+	cpu_debug1: cpu-debug@402110000 {
+		compatible = "arm,coresight-cpu-debug", "arm,primecell";
+		cpu = <&cpu1>;
+		reg = <0x4 0x02110000 0x0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+	};
+
+	etm1: etm@402140000 {
+		compatible = "arm,coresight-etm4x", "arm,primecell";
+		cpu = <&cpu1>;
+		reg = <0x4 0x02140000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		out-ports {
+			port {
+				cluster0_etm1_out_port: endpoint {
+					remote-endpoint = <&cluster0_static_funnel_in_port1>;
+				};
+			};
+		};
+	};
+
+	cpu_debug2: cpu-debug@403010000 {
+		compatible = "arm,coresight-cpu-debug", "arm,primecell";
+		cpu = <&cpu2>;
+		reg = <0x4 0x03010000 0x0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+	};
+
+	etm2: etm@403040000 {
+		compatible = "arm,coresight-etm4x", "arm,primecell";
+		cpu = <&cpu2>;
+		reg = <0x4 0x03040000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		out-ports {
+			port {
+				cluster1_etm0_out_port: endpoint {
+					remote-endpoint = <&cluster1_static_funnel_in_port0>;
+				};
+			};
+		};
+	};
+
+	cpu_debug3: cpu-debug@403110000 {
+		compatible = "arm,coresight-cpu-debug", "arm,primecell";
+		cpu = <&cpu3>;
+		reg = <0x4 0x03110000 0x0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+	};
+
+	etm3: etm@403140000 {
+		compatible = "arm,coresight-etm4x", "arm,primecell";
+		cpu = <&cpu3>;
+		reg = <0x4 0x03140000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		out-ports {
+			port {
+				cluster1_etm1_out_port: endpoint {
+					remote-endpoint = <&cluster1_static_funnel_in_port1>;
+				};
+			};
+		};
+	};
+
+	sfunnel0: funnel@0 { /* cluster0 funnel */
+		compatible = "arm,coresight-static-funnel";
+		out-ports {
+			port {
+				cluster0_static_funnel_out_port: endpoint {
+					remote-endpoint = <&etf0_in_port>;
+				};
+			};
+		};
+		in-ports {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			port@0 {
+				reg = <0>;
+				cluster0_static_funnel_in_port0: endpoint {
+					remote-endpoint = <&cluster0_etm0_out_port>;
+				};
+			};
+			port@1 {
+				reg = <1>;
+				cluster0_static_funnel_in_port1: endpoint {
+					remote-endpoint = <&cluster0_etm1_out_port>;
+				};
+			};
+		};
+	};
+
+	sfunnel1: funnel@1 { /* cluster1 funnel */
+		compatible = "arm,coresight-static-funnel";
+		out-ports {
+			port {
+				cluster1_static_funnel_out_port: endpoint {
+					remote-endpoint = <&etf1_in_port>;
+				};
+			};
+		};
+		in-ports {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			port@0 {
+				reg = <0>;
+				cluster1_static_funnel_in_port0: endpoint {
+					remote-endpoint = <&cluster1_etm0_out_port>;
+				};
+			};
+			port@1 {
+				reg = <1>;
+				cluster1_static_funnel_in_port1: endpoint {
+					remote-endpoint = <&cluster1_etm1_out_port>;
+				};
+			};
+		};
+	};
+
+	tpiu@400130000 {
+		compatible = "arm,coresight-tpiu", "arm,primecell";
+		reg = <0x4 0x00130000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		in-ports {
+			port {
+				tpiu_in_port: endpoint {
+					remote-endpoint = <&replicator_out_port0>;
+				};
+			};
+		};
+	};
+
+	main_funnel: funnel@4000a0000 {
+		compatible = "arm,coresight-dynamic-funnel", "arm,primecell";
+		reg = <0x4 0x000a0000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		out-ports {
+			port {
+				main_funnel_out_port: endpoint {
+					remote-endpoint = <&replicator_in_port>;
+				};
+			};
+		};
+		main_funnel_in_ports: in-ports {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			port@0 {
+				reg = <0>;
+				main_funnel_in_port0: endpoint {
+					remote-endpoint = <&cluster_funnel_out_port>;
+				};
+			};
+			port@5 {
+				reg = <5>;
+				main_funnel_in_port5: endpoint {
+					remote-endpoint = <&etf2_out_port>;
+				};
+			};
+		};
+	};
+
+	etr@400120000 {
+		compatible = "arm,coresight-tmc", "arm,primecell";
+		reg = <0x4 0x00120000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		arm,scatter-gather;
+		interrupts = <GIC_SPI 39 IRQ_TYPE_LEVEL_HIGH>;
+		interrupt-names = "etrbufint";
+		in-ports {
+			port {
+				etr_in_port: endpoint {
+					remote-endpoint = <&replicator_out_port1>;
+				};
+			};
+		};
+	};
+
+	replicator@400110000 {
+		compatible = "arm,coresight-dynamic-replicator", "arm,primecell";
+		reg = <0x4 0x00110000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		out-ports {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			/* replicator output ports */
+			port@0 {
+				reg = <0>;
+				replicator_out_port0: endpoint {
+					remote-endpoint = <&tpiu_in_port>;
+				};
+			};
+			port@1 {
+				reg = <1>;
+				replicator_out_port1: endpoint {
+					remote-endpoint = <&etr_in_port>;
+				};
+			};
+		};
+		in-ports {
+			port {
+				replicator_in_port: endpoint {
+					remote-endpoint = <&main_funnel_out_port>;
+				};
+			};
+		};
+	};
+
+	cluster_funnel: funnel@4000b0000 {
+		compatible = "arm,coresight-dynamic-funnel", "arm,primecell";
+		reg = <0x4 0x000b0000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		out-ports {
+			port {
+				cluster_funnel_out_port: endpoint {
+					remote-endpoint = <&main_funnel_in_port0>;
+				};
+			};
+		};
+		in-ports {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			port@0 {
+				reg = <0>;
+				cluster_funnel_in_port0: endpoint {
+					remote-endpoint = <&etf0_out_port>;
+				};
+			};
+			port@1 {
+				reg = <1>;
+				cluster_funnel_in_port1: endpoint {
+					remote-endpoint = <&etf1_out_port>;
+				};
+			};
+		};
+	};
+
+	etf0: etf@400410000 {
+		compatible = "arm,coresight-tmc", "arm,primecell";
+		reg = <0x4 0x00410000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		in-ports {
+			port {
+				etf0_in_port: endpoint {
+					remote-endpoint = <&cluster0_static_funnel_out_port>;
+				};
+			};
+		};
+		out-ports {
+			port {
+				etf0_out_port: endpoint {
+					remote-endpoint = <&cluster_funnel_in_port0>;
+				};
+			};
+		};
+	};
+
+	etf1: etf@400420000 {
+		compatible = "arm,coresight-tmc", "arm,primecell";
+		reg = <0x4 0x00420000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		in-ports {
+			port {
+				etf1_in_port: endpoint {
+					remote-endpoint = <&cluster1_static_funnel_out_port>;
+				};
+			};
+		};
+		out-ports {
+			port {
+				etf1_out_port: endpoint {
+					remote-endpoint = <&cluster_funnel_in_port1>;
+				};
+			};
+		};
+	};
+
+	stm_etf: etf@400010000 {
+		compatible = "arm,coresight-tmc", "arm,primecell";
+		reg = <0x4 0x00010000 0 0x1000>;
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		in-ports {
+			port {
+				etf2_in_port: endpoint {
+					remote-endpoint = <&stm_out_port>;
+				};
+			};
+		};
+		out-ports {
+			port {
+				etf2_out_port: endpoint {
+					remote-endpoint = <&main_funnel_in_port5>;
+				};
+			};
+		};
+	};
+
+	stm@400800000 {
+		compatible = "arm,coresight-stm", "arm,primecell";
+		reg = <4 0x00800000 0 0x1000>,
+		      <0 0x4d000000 0 0x1000000>;
+		reg-names = "stm-base", "stm-stimulus-base";
+		clocks = <&soc_refclk50mhz>;
+		clock-names = "apb_pclk";
+		out-ports {
+			port {
+				stm_out_port: endpoint {
+					remote-endpoint = <&etf2_in_port>;
+				};
+			};
+		};
+	};
+};
diff --git a/fdts/morello-soc.dts b/fdts/morello-soc.dts
index e87b617..9f996bd 100644
--- a/fdts/morello-soc.dts
+++ b/fdts/morello-soc.dts
@@ -6,6 +6,7 @@
 
 /dts-v1/;
 #include "morello.dtsi"
+#include "morello-coresight.dtsi"
 
 / {
 	model = "Arm Morello System Development Platform";
@@ -28,28 +29,28 @@
 	cpus {
 		#address-cells = <2>;
 		#size-cells = <0>;
-		cpu0@0 {
+		cpu0: cpu0@0 {
 			compatible = "arm,armv8";
 			reg = <0x0 0x0>;
 			device_type = "cpu";
 			enable-method = "psci";
 			clocks = <&scmi_dvfs 0>;
 		};
-		cpu1@100 {
+		cpu1: cpu1@100 {
 			compatible = "arm,armv8";
 			reg = <0x0 0x100>;
 			device_type = "cpu";
 			enable-method = "psci";
 			clocks = <&scmi_dvfs 0>;
 		};
-		cpu2@10000 {
+		cpu2: cpu2@10000 {
 			compatible = "arm,armv8";
 			reg = <0x0 0x10000>;
 			device_type = "cpu";
 			enable-method = "psci";
 			clocks = <&scmi_dvfs 1>;
 		};
-		cpu3@10100 {
+		cpu3: cpu3@10100 {
 			compatible = "arm,armv8";
 			reg = <0x0 0x10100>;
 			device_type = "cpu";
@@ -193,10 +194,12 @@
 		interrupts = <GIC_SPI 138 IRQ_TYPE_LEVEL_HIGH>;
 		clocks = <&dpu_aclk>;
 
-		hdmi-transmitter@70 {
+		hdmi_tx: hdmi-transmitter@70 {
 			compatible = "nxp,tda998x";
 			reg = <0x70>;
 			video-ports = <0x234501>;
+			#sound-dai-cells = <0>;
+			audio-ports = <2 0x03>;
 			port {
 				tda998x_0_input: endpoint {
 					remote-endpoint = <&dp_pl0_out0>;
@@ -254,6 +257,52 @@
 			};
 		};
 	};
+
+	iofpga_i2s: xlnx-i2s@1c150000 {
+		#sound-dai-cells = <0>;
+		compatible = "xlnx,i2s-transmitter-1.0";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		reg = <0x0 0x1c150000 0x0 0x10000>;
+		xlnx,dwidth = <0x18>;
+		xlnx,num-channels = <1>;
+	};
+
+	audio_formatter: audio-formatter@1c100000 {
+		compatible = "xlnx,audio-formatter-1.0";
+		reg = <0x0 0x1c000000 0x0 0x10000>;
+		#sound-dai-cells = <0>;
+		interrupt-names = "irq_mm2s";
+		interrupts = <GIC_SPI 137 IRQ_TYPE_LEVEL_HIGH>;
+		clock-names = "s_axi_lite_aclk", "aud_mclk", "m_axis_mm2s_aclk";
+		clocks = <&soc_refclk85mhz>, <&i2s_audclk>, <&soc_refclk85mhz>;
+	};
+
+	sound {
+		compatible = "simple-audio-card";
+		simple-audio-card,format = "i2s";
+		simple-audio-card,bitclock-master = <&audio_master>;
+		simple-audio-card,frame-master = <&audio_master>;
+		audio_master: simple-audio-card,cpu {
+			sound-dai = <&iofpga_i2s>;
+			clocks = <&i2s_audclk>;
+		};
+
+		simple-audio-card,codec {
+			sound-dai = <&hdmi_tx>;
+		};
+
+		simple-audio-card,plat {
+			sound-dai = <&audio_formatter>;
+		};
+	};
+
+	i2s_audclk: i2s_audclk {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <12288000>;
+		clock-output-names = "iofpga:i2s_audclk";
+	};
 };
 
 &gic {
diff --git a/fdts/morello.dtsi b/fdts/morello.dtsi
index 20640c5..7f39d75 100644
--- a/fdts/morello.dtsi
+++ b/fdts/morello.dtsi
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -88,6 +88,13 @@
 		clock-output-names = "apb_pclk";
 	};
 
+	soc_refclk85mhz: refclk85mhz {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <85000000>;
+		clock-output-names = "iofpga:aclk";
+	};
+
 	soc_uartclk:  uartclk {
 		compatible = "fixed-clock";
 		#clock-cells = <0>;
diff --git a/include/arch/aarch32/arch.h b/include/arch/aarch32/arch.h
index c8a6334..da4ee7f 100644
--- a/include/arch/aarch32/arch.h
+++ b/include/arch/aarch32/arch.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2023, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -104,7 +104,11 @@
 /* CSSELR definitions */
 #define LEVEL_SHIFT		U(1)
 
-/* ID_DFR0_EL1 definitions */
+/* ID_DFR0 definitions */
+#define ID_DFR0_PERFMON_SHIFT		U(24)
+#define ID_DFR0_PERFMON_MASK		U(0xf)
+#define ID_DFR0_PERFMON_PMUV3		U(3)
+#define ID_DFR0_PERFMON_PMUV3P5		U(6)
 #define ID_DFR0_COPTRC_SHIFT		U(12)
 #define ID_DFR0_COPTRC_MASK		U(0xf)
 #define ID_DFR0_COPTRC_SUPPORTED	U(1)
@@ -118,6 +122,7 @@
 #define ID_DFR1_MTPMU_SHIFT	U(0)
 #define ID_DFR1_MTPMU_MASK	U(0xf)
 #define ID_DFR1_MTPMU_SUPPORTED	U(1)
+#define ID_DFR1_MTPMU_DISABLED	U(15)
 
 /* ID_MMFR3 definitions */
 #define ID_MMFR3_PAN_SHIFT	U(16)
@@ -192,11 +197,11 @@
 #define SDCR_SPD_LEGACY		U(0x0)
 #define SDCR_SPD_DISABLE	U(0x2)
 #define SDCR_SPD_ENABLE		U(0x3)
-#define SDCR_SCCD_BIT		(U(1) << 23)
-#define SDCR_TTRF_BIT		(U(1) << 19)
 #define SDCR_SPME_BIT		(U(1) << 17)
-#define SDCR_RESET_VAL		U(0x0)
+#define SDCR_TTRF_BIT		(U(1) << 19)
+#define SDCR_SCCD_BIT		(U(1) << 23)
 #define SDCR_MTPME_BIT		(U(1) << 28)
+#define SDCR_RESET_VAL		U(0x0)
 
 /* HSCTLR definitions */
 #define HSCTLR_RES1	((U(1) << 29) | (U(1) << 28) | (U(1) << 23) | \
@@ -464,6 +469,10 @@
 #define PMCR_LP_BIT		(U(1) << 7)
 #define PMCR_LC_BIT		(U(1) << 6)
 #define PMCR_DP_BIT		(U(1) << 5)
+#define PMCR_X_BIT		(U(1) << 4)
+#define PMCR_C_BIT		(U(1) << 2)
+#define PMCR_P_BIT		(U(1) << 1)
+#define PMCR_E_BIT		(U(1) << 0)
 #define	PMCR_RESET_VAL		U(0x0)
 
 /*******************************************************************************
diff --git a/include/arch/aarch32/arch_features.h b/include/arch/aarch32/arch_features.h
index 99e3fd0..f19c4c2 100644
--- a/include/arch/aarch32/arch_features.h
+++ b/include/arch/aarch32/arch_features.h
@@ -162,4 +162,29 @@
 static inline bool is_feat_s1pie_supported(void) { return false; }
 static inline bool is_feat_sxpie_supported(void) { return false; }
 
+static inline unsigned int read_feat_pmuv3_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_dfr0(), ID_DFR0_PERFMON);
+}
+
+static inline unsigned int read_feat_mtpmu_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_dfr1(), ID_DFR1_MTPMU);
+}
+
+static inline bool is_feat_mtpmu_supported(void)
+{
+	if (DISABLE_MTPMU == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (DISABLE_MTPMU == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	unsigned int mtpmu = read_feat_mtpmu_id_field();
+
+	return mtpmu != 0U && mtpmu != ID_DFR1_MTPMU_DISABLED;
+}
+
 #endif /* ARCH_FEATURES_H */
diff --git a/include/arch/aarch32/arch_helpers.h b/include/arch/aarch32/arch_helpers.h
index ca5a44b..3a7c768 100644
--- a/include/arch/aarch32/arch_helpers.h
+++ b/include/arch/aarch32/arch_helpers.h
@@ -221,6 +221,7 @@
 DEFINE_COPROCR_READ_FUNC(id_mmfr3, ID_MMFR3)
 DEFINE_COPROCR_READ_FUNC(id_mmfr4, ID_MMFR4)
 DEFINE_COPROCR_READ_FUNC(id_dfr0, ID_DFR0)
+DEFINE_COPROCR_READ_FUNC(id_dfr1, ID_DFR1)
 DEFINE_COPROCR_READ_FUNC(id_pfr0, ID_PFR0)
 DEFINE_COPROCR_READ_FUNC(id_pfr1, ID_PFR1)
 DEFINE_COPROCR_READ_FUNC(isr, ISR)
@@ -290,7 +291,7 @@
 DEFINE_COPROCR_RW_FUNCS(sdcr, SDCR)
 DEFINE_COPROCR_RW_FUNCS(hdcr, HDCR)
 DEFINE_COPROCR_RW_FUNCS(cnthp_ctl, CNTHP_CTL)
-DEFINE_COPROCR_READ_FUNC(pmcr, PMCR)
+DEFINE_COPROCR_RW_FUNCS(pmcr, PMCR)
 
 /*
  * Address translation
diff --git a/include/arch/aarch32/el3_common_macros.S b/include/arch/aarch32/el3_common_macros.S
index 585a9ae..697eb82 100644
--- a/include/arch/aarch32/el3_common_macros.S
+++ b/include/arch/aarch32/el3_common_macros.S
@@ -277,10 +277,6 @@
 	cps	#MODE32_mon
 	isb
 
-#if DISABLE_MTPMU
-	bl	mtpmu_disable
-#endif
-
 	.if \_warm_boot_mailbox
 		/* -------------------------------------------------------------
 		 * This code will be executed for both warm and cold resets.
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index f3bccc4..b19e8af 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -221,6 +221,12 @@
 #define ID_AA64DFR0_TRACEFILT_MASK	U(0xf)
 #define ID_AA64DFR0_TRACEFILT_SUPPORTED	U(1)
 #define ID_AA64DFR0_TRACEFILT_LENGTH	U(4)
+#define ID_AA64DFR0_PMUVER_LENGTH	U(4)
+#define ID_AA64DFR0_PMUVER_SHIFT	U(8)
+#define ID_AA64DFR0_PMUVER_MASK		U(0xf)
+#define ID_AA64DFR0_PMUVER_PMUV3	U(1)
+#define ID_AA64DFR0_PMUVER_PMUV3P7	U(7)
+#define ID_AA64DFR0_PMUVER_IMP_DEF	U(0xf)
 
 /* ID_AA64DFR0_EL1.PMS definitions (for ARMv8.2+) */
 #define ID_AA64DFR0_PMS_SHIFT		U(32)
@@ -237,6 +243,7 @@
 #define ID_AA64DFR0_MTPMU_SHIFT		U(48)
 #define ID_AA64DFR0_MTPMU_MASK		ULL(0xf)
 #define ID_AA64DFR0_MTPMU_SUPPORTED	ULL(1)
+#define ID_AA64DFR0_MTPMU_DISABLED	ULL(15)
 
 /* ID_AA64DFR0_EL1.BRBE definitions */
 #define ID_AA64DFR0_BRBE_SHIFT		U(52)
@@ -268,6 +275,9 @@
 /* ID_AA64ISAR2_EL1 definitions */
 #define ID_AA64ISAR2_EL1		S3_0_C0_C6_2
 
+/* ID_AA64PFR2_EL1 definitions */
+#define ID_AA64PFR2_EL1			S3_0_C0_C4_2
+
 #define ID_AA64ISAR2_GPA3_SHIFT		U(8)
 #define ID_AA64ISAR2_GPA3_MASK		ULL(0xf)
 
@@ -395,6 +405,16 @@
 #define ID_AA64PFR1_EL1_RNG_TRAP_SUPPORTED	ULL(0x1)
 #define ID_AA64PFR1_EL1_RNG_TRAP_NOT_SUPPORTED	ULL(0x0)
 
+/* ID_AA64PFR2_EL1 definitions */
+#define ID_AA64PFR2_EL1_MTEPERM_SHIFT		U(0)
+#define ID_AA64PFR2_EL1_MTEPERM_MASK		ULL(0xf)
+
+#define ID_AA64PFR2_EL1_MTESTOREONLY_SHIFT	U(4)
+#define ID_AA64PFR2_EL1_MTESTOREONLY_MASK	ULL(0xf)
+
+#define ID_AA64PFR2_EL1_MTEFAR_SHIFT		U(8)
+#define ID_AA64PFR2_EL1_MTEFAR_MASK		ULL(0xf)
+
 #define VDISR_EL2				S3_4_C12_C1_1
 #define VSESR_EL2				S3_4_C5_C2_3
 
@@ -576,7 +596,7 @@
 #define MDCR_SBRBE_MASK		ULL(0x3)
 #define MDCR_NSTB(x)		((x) << 24)
 #define MDCR_NSTB_EL1		ULL(0x3)
-#define MDCR_NSTBE		(ULL(1) << 26)
+#define MDCR_NSTBE_BIT		(ULL(1) << 26)
 #define MDCR_MTPME_BIT		(ULL(1) << 28)
 #define MDCR_TDCC_BIT		(ULL(1) << 27)
 #define MDCR_SCCD_BIT		(ULL(1) << 23)
@@ -592,19 +612,20 @@
 #define MDCR_SPD32_ENABLE	ULL(0x3)
 #define MDCR_NSPB(x)		((x) << 12)
 #define MDCR_NSPB_EL1		ULL(0x3)
+#define MDCR_NSPBE_BIT		(ULL(1) << 11)
 #define MDCR_TDOSA_BIT		(ULL(1) << 10)
 #define MDCR_TDA_BIT		(ULL(1) << 9)
 #define MDCR_TPM_BIT		(ULL(1) << 6)
-#define MDCR_EL3_RESET_VAL	ULL(0x0)
+#define MDCR_EL3_RESET_VAL	MDCR_MTPME_BIT
 
 /* MDCR_EL2 definitions */
 #define MDCR_EL2_MTPME		(U(1) << 28)
-#define MDCR_EL2_HLP		(U(1) << 26)
+#define MDCR_EL2_HLP_BIT	(U(1) << 26)
 #define MDCR_EL2_E2TB(x)	((x) << 24)
 #define MDCR_EL2_E2TB_EL1	U(0x3)
-#define MDCR_EL2_HCCD		(U(1) << 23)
+#define MDCR_EL2_HCCD_BIT	(U(1) << 23)
 #define MDCR_EL2_TTRF		(U(1) << 19)
-#define MDCR_EL2_HPMD		(U(1) << 17)
+#define MDCR_EL2_HPMD_BIT	(U(1) << 17)
 #define MDCR_EL2_TPMS		(U(1) << 14)
 #define MDCR_EL2_E2PB(x)	((x) << 12)
 #define MDCR_EL2_E2PB_EL1	U(0x3)
@@ -615,6 +636,7 @@
 #define MDCR_EL2_HPME_BIT	(U(1) << 7)
 #define MDCR_EL2_TPM_BIT	(U(1) << 6)
 #define MDCR_EL2_TPMCR_BIT	(U(1) << 5)
+#define MDCR_EL2_HPMN_MASK	U(0x1f)
 #define MDCR_EL2_RESET_VAL	U(0x0)
 
 /* HSTR_EL2 definitions */
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 609a95b..9f11f15 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -148,6 +148,11 @@
 	return ISOLATE_FIELD(read_id_aa64mmfr0_el1(), ID_AA64MMFR0_EL1_FGT);
 }
 
+static unsigned int read_feat_mte_perm_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64pfr2_el1(), ID_AA64PFR2_EL1_MTEPERM);
+}
+
 static inline bool is_feat_fgt_supported(void)
 {
 	if (ENABLE_FEAT_FGT == FEAT_STATE_DISABLED) {
@@ -161,6 +166,19 @@
 	return read_feat_fgt_id_field() != 0U;
 }
 
+static inline bool is_feat_mte_perm_supported(void)
+{
+	if (ENABLE_FEAT_MTE_PERM == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_MTE_PERM == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_mte_perm_id_field() != 0U;
+}
+
 static unsigned int read_feat_ecv_id_field(void)
 {
 	return ISOLATE_FIELD(read_id_aa64mmfr0_el1(), ID_AA64MMFR0_EL1_ECV);
@@ -639,6 +657,7 @@
 	return read_feat_trbe_id_field() != 0U;
 
 }
+
 /*******************************************************************************
  * Function to identify the presence of FEAT_SMEx (Scalar Matrix Extension)
  ******************************************************************************/
@@ -699,4 +718,29 @@
 			     ID_AA64MMFR0_EL1_TGRAN64);
 }
 
+static inline unsigned int read_feat_pmuv3_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64dfr0_el1(), ID_AA64DFR0_PMUVER);
+}
+
+static inline unsigned int read_feat_mtpmu_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64dfr0_el1(), ID_AA64DFR0_MTPMU);
+}
+
+static inline bool is_feat_mtpmu_supported(void)
+{
+	if (DISABLE_MTPMU == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (DISABLE_MTPMU == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	unsigned int mtpmu = read_feat_mtpmu_id_field();
+
+	return (mtpmu != 0U) && (mtpmu != ID_AA64DFR0_MTPMU_DISABLED);
+}
+
 #endif /* ARCH_FEATURES_H */
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 5b3d4c2..3121079 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -269,6 +269,7 @@
 DEFINE_RENAME_IDREG_READ_FUNC(id_aa64isar2_el1, ID_AA64ISAR2_EL1)
 DEFINE_IDREG_READ_FUNC(id_aa64pfr0_el1)
 DEFINE_IDREG_READ_FUNC(id_aa64pfr1_el1)
+DEFINE_RENAME_IDREG_READ_FUNC(id_aa64pfr2_el1, ID_AA64PFR2_EL1)
 DEFINE_IDREG_READ_FUNC(id_aa64dfr0_el1)
 DEFINE_IDREG_READ_FUNC(id_afr0_el1)
 DEFINE_SYSREG_READ_FUNC(CurrentEl)
diff --git a/include/arch/aarch64/el2_common_macros.S b/include/arch/aarch64/el2_common_macros.S
index dcaea3d..9609c0d 100644
--- a/include/arch/aarch64/el2_common_macros.S
+++ b/include/arch/aarch64/el2_common_macros.S
@@ -103,7 +103,7 @@
 	 */
 	mov_imm	x0, ((MDCR_EL2_RESET_VAL | \
 		      MDCR_SPD32(MDCR_SPD32_DISABLE)) \
-		      & ~(MDCR_EL2_HPMD | MDCR_TDOSA_BIT | \
+		      & ~(MDCR_EL2_HPMD_BIT | MDCR_TDOSA_BIT | \
 		      MDCR_TDA_BIT | MDCR_TPM_BIT))
 
 	msr	mdcr_el2, x0
@@ -244,10 +244,6 @@
 		isb
 	.endif /* _init_sctlr */
 
-#if DISABLE_MTPMU
-		bl	mtpmu_disable
-#endif
-
 	.if \_warm_boot_mailbox
 		/* -------------------------------------------------------------
 		 * This code will be executed for both warm and cold resets.
diff --git a/include/arch/aarch64/el3_common_macros.S b/include/arch/aarch64/el3_common_macros.S
index 2dee07d..fa9310e 100644
--- a/include/arch/aarch64/el3_common_macros.S
+++ b/include/arch/aarch64/el3_common_macros.S
@@ -115,83 +115,14 @@
 	 * MDCR_EL3.TDA: Set to zero to allow EL0, EL1 and EL2 access to the
 	 *  debug registers, other than those registers that are controlled by
 	 *  MDCR_EL3.TDOSA.
-	 *
-	 * MDCR_EL3.TPM: Set to zero so that EL0, EL1, and EL2 System register
-	 *  accesses to all Performance Monitors registers do not trap to EL3.
-	 *
-	 * MDCR_EL3.SCCD: Set to one so that cycle counting by PMCCNTR_EL0 is
-	 *  prohibited in Secure state. This bit is RES0 in versions of the
-	 *  architecture with FEAT_PMUv3p5 not implemented, setting it to 1
-	 *  doesn't have any effect on them.
-	 *
-	 * MDCR_EL3.MCCD: Set to one so that cycle counting by PMCCNTR_EL0 is
-	 *  prohibited in EL3. This bit is RES0 in versions of the
-	 *  architecture with FEAT_PMUv3p7 not implemented, setting it to 1
-	 *  doesn't have any effect on them.
-	 *
-	 * MDCR_EL3.SPME: Set to zero so that event counting by the programmable
-	 *  counters PMEVCNTR<n>_EL0 is prohibited in Secure state. If ARMv8.2
-	 *  Debug is not implemented this bit does not have any effect on the
-	 *  counters unless there is support for the implementation defined
-	 *  authentication interface ExternalSecureNoninvasiveDebugEnabled().
-	 *
-	 * MDCR_EL3.NSTB, MDCR_EL3.NSTBE: Set to zero so that Trace Buffer
-	 *  owning security state is Secure state. If FEAT_TRBE is implemented,
-	 *  accesses to Trace Buffer control registers at EL2 and EL1 in any
-	 *  security state generates trap exceptions to EL3.
-	 *  If FEAT_TRBE is not implemented, these bits are RES0.
-	 *
-	 * MDCR_EL3.TTRF: Set to one so that access to trace filter control
-	 *  registers in non-monitor mode generate EL3 trap exception,
-	 *  unless the access generates a higher priority exception when trace
-	 *  filter control(FEAT_TRF) is implemented.
-	 *  When FEAT_TRF is not implemented, this bit is RES0.
-	 * ---------------------------------------------------------------------
 	 */
 	mov_imm	x0, ((MDCR_EL3_RESET_VAL | MDCR_SDD_BIT | \
-		      MDCR_SPD32(MDCR_SPD32_DISABLE) | MDCR_SCCD_BIT | \
-		      MDCR_MCCD_BIT) & ~(MDCR_SPME_BIT | MDCR_TDOSA_BIT | \
-		      MDCR_TDA_BIT | MDCR_TPM_BIT | MDCR_NSTB(MDCR_NSTB_EL1) | \
-		      MDCR_NSTBE | MDCR_TTRF_BIT))
+		      MDCR_SPD32(MDCR_SPD32_DISABLE)) & \
+		    ~(MDCR_TDOSA_BIT | MDCR_TDA_BIT))
 
-	mrs	x1, id_aa64dfr0_el1
-	ubfx	x1, x1, #ID_AA64DFR0_TRACEFILT_SHIFT, #ID_AA64DFR0_TRACEFILT_LENGTH
-	cbz	x1, 1f
-	orr	x0, x0, #MDCR_TTRF_BIT
-1:
 	msr	mdcr_el3, x0
 
 	/* ---------------------------------------------------------------------
-	 * Initialise PMCR_EL0 setting all fields rather than relying
-	 * on hw. Some fields are architecturally UNKNOWN on reset.
-	 *
-	 * PMCR_EL0.LP: Set to one so that event counter overflow, that
-	 *  is recorded in PMOVSCLR_EL0[0-30], occurs on the increment
-	 *  that changes PMEVCNTR<n>_EL0[63] from 1 to 0, when ARMv8.5-PMU
-	 *  is implemented. This bit is RES0 in versions of the architecture
-	 *  earlier than ARMv8.5, setting it to 1 doesn't have any effect
-	 *  on them.
-	 *
-	 * PMCR_EL0.LC: Set to one so that cycle counter overflow, that
-	 *  is recorded in PMOVSCLR_EL0[31], occurs on the increment
-	 *  that changes PMCCNTR_EL0[63] from 1 to 0.
-	 *
-	 * PMCR_EL0.DP: Set to one so that the cycle counter,
-	 *  PMCCNTR_EL0 does not count when event counting is prohibited.
-	 *
-	 * PMCR_EL0.X: Set to zero to disable export of events.
-	 *
-	 * PMCR_EL0.D: Set to zero so that, when enabled, PMCCNTR_EL0
-	 *  counts on every clock cycle.
-	 * ---------------------------------------------------------------------
-	 */
-	mov_imm	x0, ((PMCR_EL0_RESET_VAL | PMCR_EL0_LP_BIT | \
-		      PMCR_EL0_LC_BIT | PMCR_EL0_DP_BIT) & \
-		    ~(PMCR_EL0_X_BIT | PMCR_EL0_D_BIT))
-
-	msr	pmcr_el0, x0
-
-	/* ---------------------------------------------------------------------
 	 * Enable External Aborts and SError Interrupts now that the exception
 	 * vectors have been setup.
 	 * ---------------------------------------------------------------------
@@ -205,15 +136,6 @@
 	 * CPTR_EL3.TCPAC: Set to zero so that any accesses to CPACR_EL1,
 	 *  CPTR_EL2, CPACR, or HCPTR do not trap to EL3.
 	 *
-	 * CPTR_EL3.TTA: Set to one so that accesses to the trace system
-	 *  registers trap to EL3 from all exception levels and security
-	 *  states when system register trace is implemented.
-	 *  When system register trace is not implemented, this bit is RES0 and
-	 *  hence set to zero.
-	 *
-	 * CPTR_EL3.TTA: Set to zero so that System register accesses to the
-	 *  trace registers do not trap to EL3.
-	 *
 	 * CPTR_EL3.TFP: Set to zero so that accesses to the V- or Z- registers
 	 *  by Advanced SIMD, floating-point or SVE instructions (if implemented)
 	 *  do not trap to EL3.
@@ -228,12 +150,7 @@
 	 *  to EL3 by default.
 	 */
 
-	mov_imm x0, (CPTR_EL3_RESET_VAL & ~(TCPAC_BIT | TTA_BIT | TFP_BIT))
-	mrs	x1, id_aa64dfr0_el1
-	ubfx	x1, x1, #ID_AA64DFR0_TRACEVER_SHIFT, #ID_AA64DFR0_TRACEVER_LENGTH
-	cbz	x1, 1f
-	orr	x0, x0, #TTA_BIT
-1:
+	mov_imm x0, (CPTR_EL3_RESET_VAL & ~(TCPAC_BIT | TFP_BIT))
 	msr	cptr_el3, x0
 
 	/*
@@ -340,10 +257,6 @@
 		isb
 	.endif /* _init_sctlr */
 
-#if DISABLE_MTPMU
-		bl	mtpmu_disable
-#endif
-
 	.if \_warm_boot_mailbox
 		/* -------------------------------------------------------------
 		 * This code will be executed for both warm and cold resets.
diff --git a/include/common/fdt_wrappers.h b/include/common/fdt_wrappers.h
index b16510f..abbf976 100644
--- a/include/common/fdt_wrappers.h
+++ b/include/common/fdt_wrappers.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -49,7 +49,7 @@
 
 static inline uint32_t fdt_blob_size(const void *dtb)
 {
-	const uint32_t *dtb_header = dtb;
+	const uint32_t *dtb_header = (const uint32_t *)dtb;
 
 	return fdt32_to_cpu(dtb_header[1]);
 }
@@ -60,7 +60,8 @@
 	const void *prop = fdt_getprop(fdt, node, "status", &len);
 
 	/* A non-existing status property means the device is enabled. */
-	return (prop == NULL) || (len == 5 && strcmp(prop, "okay") == 0);
+	return (prop == NULL) || (len == 5 && strcmp((const char *)prop,
+		"okay") == 0);
 }
 
 #define fdt_for_each_compatible_node(dtb, node, compatible_str)       \
diff --git a/include/drivers/cadence/cdns_combo_phy.h b/include/drivers/cadence/cdns_combo_phy.h
new file mode 100644
index 0000000..f5dabda
--- /dev/null
+++ b/include/drivers/cadence/cdns_combo_phy.h
@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CDN_COMBOPHY_H
+#define CDN_COMBOPHY_H
+
+/* SRS */
+#define SDMMC_CDN_SRS02			0x8
+#define SDMMC_CDN_SRS03			0xC
+#define SDMMC_CDN_SRS04			0x10
+#define SDMMC_CDN_SRS05			0x14
+#define SDMMC_CDN_SRS06			0x18
+#define SDMMC_CDN_SRS07			0x1C
+#define SDMMC_CDN_SRS09			0x24
+#define SDMMC_CDN_SRS10			0x28
+#define SDMMC_CDN_SRS11			0x2C
+#define SDMMC_CDN_SRS12			0x30
+#define SDMMC_CDN_SRS13			0x34
+#define SDMMC_CDN_SRS14			0x38
+
+/* SRS03 */
+/* Response Type Select
+ * Defines the expected response length.
+ */
+#define SDMMC_CDN_RTS				16
+
+/* Command CRC Check Enable
+ * When set to 1, the host checks if the CRC field of the response is valid.
+ * When 0, the CRC check is disabled and the CRC field of the response is ignored.
+ */
+#define SDMMC_CDN_CRCCE				19
+
+/* Command Index
+ * This field contains a command number (index) of the command to be sent.
+ */
+#define SDMMC_CDN_CIDX				24
+
+/* SRS09 */
+/* Card Inserted
+ * Indicates if the card is inserted inside the slot.
+ */
+#define SDMMC_CDN_CI				16
+
+/* SRS10 */
+/* Data Transfer Width
+ * Bit used to configure DAT bus width to 1 or 4.
+ */
+#define SDMMC_CDN_DTW				1
+
+/* Extended Data Transfer Width
+ * This bit is to enable/disable 8-bit DAT bus width mode.
+ */
+#define SDMMC_CDN_EDTW				5
+
+/* SD Bus Power for VDD1
+ * When set to 1, the VDD1 voltage is supplied to card/device.
+ */
+#define SDMMC_CDN_BP				8
+
+/* SD Bus Voltage Select
+ * This field is used to configure VDD1 voltage level.
+ */
+#define SDMMC_CDN_BVS				9
+
+/* SRS11 */
+/* Internal Clock Enable
+ * This field is designated to controls (enable/disable) external clock generator.
+ */
+#define SDMMC_CDN_ICE				0
+
+/* Internal Clock Stable
+ * When 1, indicates that the clock on sdmclk pin of the host is stable.
+ * When 0, indicates that the clock is not stable .
+ */
+#define SDMMC_CDN_ICS				1
+
+/* SD Clock Enable
+ * When set, SDCLK clock is enabled.
+ * When clear, SDCLK clock is stopped.
+ */
+#define SDMMC_CDN_SDCE				2
+
+/* USDCLK Frequency Select
+ * This is used to calculate frequency of USDCLK clock.
+ */
+#define SDMMC_CDN_USDCLKFS			6
+
+/* SDCLK Frequency Select
+ * This is used to calculate frequency of SDCLK clock.
+ */
+#define SDMMC_CDN_SDCLKFS				8
+
+/* Data Timeout Counter Value
+ * This value determines the interval by which DAT line timeouts are detected
+ */
+#define SDMMC_CDN_DTCV				16
+
+/* SRS12 */
+/* Command Complete
+ * Generated when the end bit of the response is received.
+ */
+#define SDMMC_CDN_CC				0
+
+/* Transfer Complete
+ * Generated when the transfer which uses the DAT line is complete.
+ */
+#define SDMMC_CDN_TC				1
+
+/* Error Interrupt
+ * The software can check for an error by reading this single bit first.
+ */
+#define SDMMC_CDN_EINT				15
+
+/* SRS14 */
+/* Command Complete Interrupt Enable */
+#define SDMMC_CDN_CC_IE				0
+
+/* Transfer Complete Interrupt Enable */
+#define SDMMC_CDN_TC_IE				1
+
+/* DMA Interrupt Enable */
+#define SDMMC_CDN_DMAINT_IE			3
+
+/* Combo PHY DLL registers */
+#define CP_DLL_REG_BASE			(0x10B92000)
+#define CP_DLL_DQ_TIMING_REG		(0x00)
+#define CP_DLL_DQS_TIMING_REG		(0x04)
+#define CP_DLL_GATE_LPBK_CTRL_REG	(0x08)
+#define CP_DLL_MASTER_CTRL_REG		(0x0C)
+#define CP_DLL_SLAVE_CTRL_REG		(0x10)
+#define CP_DLL_IE_TIMING_REG		(0x14)
+
+#define CP_DQ_TIMING_REG_SDR		(0x00000002)
+#define CP_DQS_TIMING_REG_SDR		(0x00100004)
+#define CP_GATE_LPBK_CTRL_REG_SDR	(0x00D80000)
+#define CP_DLL_MASTER_CTRL_REG_SDR	(0x00800000)
+#define CP_DLL_SLAVE_CTRL_REG_SDR	(0x00000000)
+
+#define CP_DLL(_reg)			(CP_DLL_REG_BASE \
+					+ (CP_DLL_##_reg))
+
+/* Control Timing Block registers */
+#define CP_CTB_REG_BASE			(0x10B92080)
+#define CP_CTB_CTRL_REG			(0x00)
+#define CP_CTB_TSEL_REG			(0x04)
+#define CP_CTB_GPIO_CTRL0		(0x08)
+#define CP_CTB_GPIO_CTRL1		(0x0C)
+#define CP_CTB_GPIO_STATUS0		(0x10)
+#define CP_CTB_GPIO_STATUS1		(0x14)
+
+#define CP_CTRL_REG_SDR			(0x00004040)
+#define CP_TSEL_REG_SDR			(0x00000000)
+
+#define CP_CTB(_reg)			(CP_CTB_REG_BASE \
+					+ (CP_CTB_##_reg))
+
+/* Combo PHY */
+#define SDMMC_CDN_REG_BASE		0x10808200
+#define PHY_DQ_TIMING_REG		0x2000
+#define PHY_DQS_TIMING_REG		0x2004
+#define PHY_GATE_LPBK_CTRL_REG		0x2008
+#define PHY_DLL_MASTER_CTRL_REG		0x200C
+#define PHY_DLL_SLAVE_CTRL_REG		0x2010
+#define PHY_CTRL_REG			0x2080
+#define PHY_REG_ADDR_MASK		0xFFFF
+#define PHY_REG_DATA_MASK		0xFFFFFFFF
+
+/* PHY_DQS_TIMING_REG */
+#define CP_USE_EXT_LPBK_DQS(x)		((x) << 22) //0x1
+#define CP_USE_LPBK_DQS(x)		((x) << 21) //0x1
+#define CP_USE_PHONY_DQS(x)		((x) << 20) //0x1
+#define CP_USE_PHONY_DQS_CMD(x)		((x) << 19) //0x1
+
+/* PHY_GATE_LPBK_CTRL_REG */
+#define CP_SYNC_METHOD(x)		((x) << 31) //0x1
+#define CP_SW_HALF_CYCLE_SHIFT(x)	((x) << 28) //0x1
+#define CP_RD_DEL_SEL(x)		((x) << 19) //0x3f
+#define CP_UNDERRUN_SUPPRESS(x)		((x) << 18) //0x1
+#define CP_GATE_CFG_ALWAYS_ON(x)	((x) << 6) //0x1
+
+/* PHY_DLL_MASTER_CTRL_REG */
+#define CP_DLL_BYPASS_MODE(x)		((x) << 23) //0x1
+#define CP_DLL_START_POINT(x)		((x) << 0) //0xff
+
+/* PHY_DLL_SLAVE_CTRL_REG */
+#define CP_READ_DQS_CMD_DELAY(x)	((x) << 24) //0xff
+#define CP_CLK_WRDQS_DELAY(x)		((x) << 16) //0xff
+#define CP_CLK_WR_DELAY(x)		((x) << 8) //0xff
+#define CP_READ_DQS_DELAY(x)		((x) << 0) //0xff
+
+/* PHY_DQ_TIMING_REG */
+#define CP_IO_MASK_ALWAYS_ON(x)		((x) << 31) //0x1
+#define CP_IO_MASK_END(x)		((x) << 27) //0x7
+#define CP_IO_MASK_START(x)		((x) << 24) //0x7
+#define CP_DATA_SELECT_OE_END(x)	((x) << 0) //0x7
+
+/* PHY_CTRL_REG */
+#define CP_PHONY_DQS_TIMING_MASK	0x3F
+#define CP_PHONY_DQS_TIMING_SHIFT	4
+
+/* Shared Macros */
+#define SDMMC_CDN(_reg)			(SDMMC_CDN_REG_BASE + \
+					(SDMMC_CDN_##_reg))
+
+struct cdns_sdmmc_combo_phy {
+	uint32_t	cp_clk_wr_delay;
+	uint32_t	cp_clk_wrdqs_delay;
+	uint32_t	cp_data_select_oe_end;
+	uint32_t	cp_dll_bypass_mode;
+	uint32_t	cp_dll_locked_mode;
+	uint32_t	cp_dll_start_point;
+	uint32_t	cp_gate_cfg_always_on;
+	uint32_t	cp_io_mask_always_on;
+	uint32_t	cp_io_mask_end;
+	uint32_t	cp_io_mask_start;
+	uint32_t	cp_rd_del_sel;
+	uint32_t	cp_read_dqs_cmd_delay;
+	uint32_t	cp_read_dqs_delay;
+	uint32_t	cp_sw_half_cycle_shift;
+	uint32_t	cp_sync_method;
+	uint32_t	cp_underrun_suppress;
+	uint32_t	cp_use_ext_lpbk_dqs;
+	uint32_t	cp_use_lpbk_dqs;
+	uint32_t	cp_use_phony_dqs;
+	uint32_t	cp_use_phony_dqs_cmd;
+};
+
+/* Function Prototype */
+
+int cdns_sdmmc_write_phy_reg(uint32_t phy_reg_addr, uint32_t phy_reg_addr_value,
+			uint32_t phy_reg_data, uint32_t phy_reg_data_value);
+int cdns_sd_card_detect(void);
+int cdns_emmc_card_reset(void);
+
+#endif
diff --git a/include/drivers/cadence/cdns_nand.h b/include/drivers/cadence/cdns_nand.h
new file mode 100644
index 0000000..64ba267
--- /dev/null
+++ b/include/drivers/cadence/cdns_nand.h
@@ -0,0 +1,256 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CDN_NAND_H
+#define CDN_NAND_H
+
+#include <drivers/cadence/cdns_combo_phy.h>
+
+/* NAND flash device information */
+typedef struct cnf_dev_info {
+	uint8_t type;
+	uint8_t nluns;
+	uint8_t sector_cnt;
+	uint16_t npages_per_block;
+	uint16_t sector_size;
+	uint16_t last_sector_size;
+	uint16_t page_size;
+	uint16_t spare_size;
+	uint32_t nblocks_per_lun;
+	uint32_t block_size;
+	unsigned long long total_size;
+} cnf_dev_info_t;
+
+/* Shared Macros */
+
+/* Default values */
+#define CNF_DEF_VOL_ID					0
+#define CNF_DEF_DEVICE					0
+#define CNF_DEF_TRD					0
+#define CNF_READ_SINGLE_PAGE				1
+#define CNF_DEF_DELAY_US				500
+#define CNF_READ_INT_DELAY_US				10
+
+/* Work modes */
+#define CNF_WORK_MODE_CDMA				0
+#define CNF_WORK_MODE_PIO				1
+
+/* Command types */
+#define CNF_CT_SET_FEATURE				0x0100
+#define CNF_CT_RESET_ASYNC				0x1100
+#define CNF_CT_RESET_SYNC				0x1101
+#define CNF_CT_RESET_LUN				0x1102
+#define CNF_CT_ERASE					0x1000
+#define CNF_CT_PAGE_PROGRAM				0x2100
+#define CNF_CT_PAGE_READ				0x2200
+
+/* Interrupts enable or disable */
+#define CNF_INT_EN					1
+#define CNF_INT_DIS					0
+
+/* Device types */
+#define CNF_DT_UNKNOWN					0x00
+#define CNF_DT_ONFI					0x01
+#define CNF_DT_JEDEC					0x02
+#define CNF_DT_LEGACY					0x03
+
+/* Command and status registers */
+#define CNF_CMDREG_REG_BASE				SOCFPGA_NAND_REG_BASE
+
+/* DMA maximum burst size 0-127*/
+#define CNF_DMA_BURST_SIZE_MAX				127
+
+/* DMA settings register field offsets */
+#define CNF_DMA_SETTINGS_BURST				0
+#define CNF_DMA_SETTINGS_OTE				16
+#define CNF_DMA_SETTINGS_SDMA_ERR			17
+
+#define CNF_DMA_MASTER_SEL				1
+#define CNF_DMA_SLAVE_SEL				0
+
+/* DMA FIFO trigger level register field offsets */
+#define CNF_FIFO_TLEVEL_POS				0
+#define CNF_FIFO_TLEVEL_DMA_SIZE			16
+#define CNF_DMA_PREFETCH_SIZE				(1024 / 8)
+
+#define CNF_GET_CTRL_BUSY(x)				(x & (1 << 8))
+#define CNF_GET_INIT_COMP(x)				(x & (1 << 9))
+
+/* Command register0 field offsets */
+#define CNF_CMDREG0_CT					30
+#define CNF_CMDREG0_TRD					24
+#define CNF_CMDREG0_INTR				20
+#define CNF_CMDREG0_DMA					21
+#define CNF_CMDREG0_VOL					16
+#define CNF_CMDREG0_CMD					0
+#define CNF_CMDREG4_MEM					24
+
+/* Command status register field offsets */
+#define CNF_ECMD					BIT(0)
+#define CNF_EECC					BIT(1)
+#define CNF_EMAX					BIT(2)
+#define CNF_EDEV					BIT(12)
+#define CNF_EDQS					BIT(13)
+#define CNF_EFAIL					BIT(14)
+#define CNF_CMPLT					BIT(15)
+#define CNF_EBUS					BIT(16)
+#define CNF_EDI						BIT(17)
+#define CNF_EPAR					BIT(18)
+#define CNF_ECTX					BIT(19)
+#define CNF_EPRO					BIT(20)
+#define CNF_EIDX					BIT(24)
+
+#define CNF_CMDREG_CMD_REG0				0x00
+#define CNF_CMDREG_CMD_REG1				0x04
+#define CNF_CMDREG_CMD_REG2				0x08
+#define CNF_CMDREG_CMD_REG3				0x0C
+#define CNF_CMDREG_CMD_STAT_PTR				0x10
+#define CNF_CMDREG_CMD_STAT				0x14
+#define CNF_CMDREG_CMD_REG4				0x20
+#define CNF_CMDREG_CTRL_STATUS				0x118
+#define CNF_CMDREG_TRD_STATUS				0x120
+
+#define CNF_CMDREG(_reg)				(CNF_CMDREG_REG_BASE \
+							+ (CNF_CMDREG_##_reg))
+
+/* Controller configuration registers */
+#define CNF_LSB16_MASK					0xFFFF
+#define CNF_GET_NPAGES_PER_BLOCK(x)			(x & CNF_LSB16_MASK)
+
+#define CNF_GET_SCTR_SIZE(x)				(x & CNF_LSB16_MASK)
+#define CNF_GET_LAST_SCTR_SIZE(x)			((x >> 16) & CNF_LSB16_MASK)
+
+#define CNF_GET_PAGE_SIZE(x)				(x & CNF_LSB16_MASK)
+#define CNF_GET_SPARE_SIZE(x)				((x >> 16) & CNF_LSB16_MASK)
+
+#define CNF_CTRLCFG_REG_BASE				0x10B80400
+#define CNF_CTRLCFG_TRANS_CFG0				0x00
+#define CNF_CTRLCFG_TRANS_CFG1				0x04
+#define CNF_CTRLCFG_LONG_POLL				0x08
+#define CNF_CTRLCFG_SHORT_POLL				0x0C
+#define CNF_CTRLCFG_DEV_STAT				0x10
+#define CNF_CTRLCFG_DEV_LAYOUT				0x24
+#define CNF_CTRLCFG_ECC_CFG0				0x28
+#define CNF_CTRLCFG_ECC_CFG1				0x2C
+#define CNF_CTRLCFG_MULTIPLANE_CFG			0x34
+#define CNF_CTRLCFG_CACHE_CFG				0x38
+#define CNF_CTRLCFG_DMA_SETTINGS			0x3C
+#define CNF_CTRLCFG_FIFO_TLEVEL				0x54
+
+#define CNF_CTRLCFG(_reg)				(CNF_CTRLCFG_REG_BASE \
+							+ (CNF_CTRLCFG_##_reg))
+
+/* Data integrity registers */
+#define CNF_DI_PAR_EN					0
+#define CNF_DI_CRC_EN					1
+
+#define CNF_DI_REG_BASE					0x10B80700
+#define CNF_DI_CONTROL					0x00
+#define CNF_DI_INJECT0					0x04
+#define CNF_DI_INJECT1					0x08
+#define CNF_DI_ERR_REG_ADDR				0x0C
+#define CNF_DI_INJECT2					0x10
+
+#define CNF_DI(_reg)					(CNF_DI_REG_BASE \
+							+ (CNF_DI_##_reg))
+
+/* Controller parameter registers */
+#define CNF_NTHREADS_MASK				0x07
+#define CNF_GET_NLUNS(x)				(x & 0xFF)
+#define CNF_GET_DEV_TYPE(x)				((x >> 30) & 0x03)
+#define CNF_GET_NTHREADS(x)				(1 << (x & CNF_NTHREADS_MASK))
+
+#define CNF_CTRLPARAM_REG_BASE				0x10B80800
+#define CNF_CTRLPARAM_VERSION				0x00
+#define CNF_CTRLPARAM_FEATURE				0x04
+#define CNF_CTRLPARAM_MFR_ID				0x08
+#define CNF_CTRLPARAM_DEV_AREA				0x0C
+#define CNF_CTRLPARAM_DEV_PARAMS0			0x10
+#define CNF_CTRLPARAM_DEV_PARAMS1			0x14
+#define CNF_CTRLPARAM_DEV_FEATUERS			0x18
+#define CNF_CTRLPARAM_DEV_BLOCKS_PLUN			0x1C
+
+#define CNF_CTRLPARAM(_reg)				(CNF_CTRLPARAM_REG_BASE \
+							+ (CNF_CTRLPARAM_##_reg))
+
+/* Protection mechanism registers */
+#define CNF_PROT_REG_BASE				0x10B80900
+#define CNF_PROT_CTRL0					0x00
+#define CNF_PROT_DOWN0					0x04
+#define CNF_PROT_UP0					0x08
+#define CNF_PROT_CTRL1					0x10
+#define CNF_PROT_DOWN1					0x14
+#define CNF_PROT_UP1					0x18
+
+#define CNF_PROT(_reg)					(CNF_PROT_REG_BASE \
+							+ (CNF_PROT_##_reg))
+
+/* Mini controller registers */
+#define CNF_MINICTRL_REG_BASE				0x10B81000
+
+/* Operation work modes */
+#define CNF_OPR_WORK_MODE_SDR				0
+#define CNF_OPR_WORK_MODE_NVDDR				1
+#define CNF_OPR_WORK_MODE_TOGGLE_NVDDR2_3		2
+#define CNF_OPR_WORK_MODE_RES				3
+
+/* Mini controller common settings register field offsets */
+#define CNF_CMN_SETTINGS_WR_WUP				20
+#define CNF_CMN_SETTINGS_RD_WUP				16
+#define CNF_CMN_SETTINGS_DEV16				8
+#define CNF_CMN_SETTINGS_OPR				0
+
+/* Async mode register field offsets */
+#define CNF_ASYNC_TIMINGS_TRH				24
+#define CNF_ASYNC_TIMINGS_TRP				16
+#define CNF_ASYNC_TIMINGS_TWH				8
+#define CNF_ASYNC_TIMINGS_TWP				0
+
+/* Mini controller DLL PHY controller register field offsets */
+#define CNF_DLL_PHY_RST_N				24
+#define CNF_DLL_PHY_EXT_WR_MODE				17
+#define CNF_DLL_PHY_EXT_RD_MODE				16
+
+#define CNF_MINICTRL_WP_SETTINGS			0x00
+#define CNF_MINICTRL_RBN_SETTINGS			0x04
+#define CNF_MINICTRL_CMN_SETTINGS			0x08
+#define CNF_MINICTRL_SKIP_BYTES_CFG			0x0C
+#define CNF_MINICTRL_SKIP_BYTES_OFFSET			0x10
+#define CNF_MINICTRL_TOGGLE_TIMINGS0			0x14
+#define CNF_MINICTRL_TOGGLE_TIMINGS1			0x18
+#define CNF_MINICTRL_ASYNC_TOGGLE_TIMINGS		0x1C
+#define CNF_MINICTRL_SYNC_TIMINGS			0x20
+#define CNF_MINICTRL_DLL_PHY_CTRL			0x34
+
+#define CNF_MINICTRL(_reg)				(CNF_MINICTRL_REG_BASE \
+							+ (CNF_MINICTRL_##_reg))
+
+/*
+ * @brief Nand IO MTD initialization routine
+ *
+ * @total_size: [out] Total size of the NAND flash device
+ * @erase_size: [out] Minimum erase size of the NAND flash device
+ * Return: 0 on success, a negative errno on failure
+ */
+int cdns_nand_init_mtd(unsigned long long *total_size,
+						unsigned int *erase_size);
+
+/*
+ * @brief Read bytes from the NAND flash device
+ *
+ * @offset: Byte offset to read from in device
+ * @buffer: [out] Bytes read from device
+ * @length: Number of bytes to read
+ * @out_length: [out] Number of bytes read from device
+ * Return: 0 on success, a negative errno on failure
+ */
+int cdns_nand_read(unsigned int offset, uintptr_t buffer,
+					size_t length, size_t *out_length);
+
+/* NAND Flash Controller/Host initialization */
+int cdns_nand_host_init(void);
+
+#endif
diff --git a/include/drivers/cadence/cdns_sdmmc.h b/include/drivers/cadence/cdns_sdmmc.h
new file mode 100644
index 0000000..6452725
--- /dev/null
+++ b/include/drivers/cadence/cdns_sdmmc.h
@@ -0,0 +1,474 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CDN_MMC_H
+#define CDN_MMC_H
+
+#include <drivers/cadence/cdns_combo_phy.h>
+#include <drivers/mmc.h>
+#include "socfpga_plat_def.h"
+
+#if MMC_DEVICE_TYPE == 0
+#define CONFIG_DMA_ADDR_T_64BIT		0
+#endif
+
+#define MMC_REG_BASE			SOCFPGA_MMC_REG_BASE
+#define COMBO_PHY_REG		0x0
+#define SDHC_EXTENDED_WR_MODE_MASK	0xFFFFFFF7
+#define SDHC_DLL_RESET_MASK	0x00000001
+/* HRS09 */
+#define SDHC_PHY_SW_RESET			BIT(0)
+#define SDHC_PHY_INIT_COMPLETE		BIT(1)
+#define SDHC_EXTENDED_RD_MODE(x)	((x) << 2)
+#define EXTENDED_WR_MODE			3
+#define SDHC_EXTENDED_WR_MODE(x)	((x) << 3)
+#define RDCMD_EN					15
+#define SDHC_RDCMD_EN(x)			((x) << 15)
+#define SDHC_RDDATA_EN(x)			((x) << 16)
+
+/* CMD_DATA_OUTPUT */
+#define SDHC_CDNS_HRS16				0x40
+
+/* This value determines the interval by which DAT line timeouts are detected */
+/* The interval can be computed as below: */
+/* • 1111b - Reserved */
+/* • 1110b - t_sdmclk*2(27+2) */
+/* • 1101b - t_sdmclk*2(26+2) */
+#define READ_CLK					0xa << 16
+#define WRITE_CLK					0xe << 16
+#define DTC_VAL						0xE
+
+/* SRS00 */
+/* System Address / Argument 2 / 32-bit block count
+ * This field is used as:
+ * • 32-bit Block Count register
+ * • SDMA system memory address
+ * • Auto CMD23 Argument
+ */
+#define SAAR						(1)
+
+/* SRS01 */
+/* Transfer Block Size
+ * This field defines block size for block data transfers
+ */
+#define BLOCK_SIZE					0
+
+/* SDMA Buffer Boundary
+ * System address boundary can be set for SDMA engine.
+ */
+#define SDMA_BUF					7 << 12
+
+/* Block Count For Current Transfer
+ * To set the number of data blocks can be defined for next transfer
+ */
+#define BLK_COUNT_CT				16
+
+/* SRS03 */
+#define CMD_START					(U(1) << 31)
+#define CMD_USE_HOLD_REG			(1 << 29)
+#define CMD_UPDATE_CLK_ONLY			(1 << 21)
+#define CMD_SEND_INIT				(1 << 15)
+#define CMD_STOP_ABORT_CMD			(4 << 22)
+#define CMD_RESUME_CMD				(2 << 22)
+#define CMD_SUSPEND_CMD				(1 << 22)
+#define DATA_PRESENT				(1 << 21)
+#define CMD_IDX_CHK_ENABLE			(1 << 20)
+#define CMD_WRITE					(0 << 4)
+#define CMD_READ					(1 << 4)
+#define	MULTI_BLK_READ				(1 << 5)
+#define RESP_ERR					(1 << 7)
+#define CMD_CHECK_RESP_CRC			(1 << 19)
+#define RES_TYPE_SEL_48				(2 << 16)
+#define RES_TYPE_SEL_136			(1 << 16)
+#define RES_TYPE_SEL_48_B			(3 << 16)
+#define RES_TYPE_SEL_NO				(0 << 16)
+#define DMA_ENABLED					(1 << 0)
+#define BLK_CNT_EN					(1 << 1)
+#define AUTO_CMD_EN					(2 << 2)
+#define COM_IDX						24
+#define ERROR_INT					(1 << 15)
+#define INT_SBE						(1 << 13)
+#define INT_HLE						(1 << 12)
+#define INT_FRUN					(1 << 11)
+#define INT_DRT						(1 << 9)
+#define INT_RTO						(1 << 8)
+#define INT_DCRC					(1 << 7)
+#define INT_RCRC					(1 << 6)
+#define INT_RXDR					(1 << 5)
+#define INT_TXDR					(1 << 4)
+#define INT_DTO						(1 << 3)
+#define INT_CMD_DONE				(1 << 0)
+#define TRAN_COMP					(1 << 1)
+
+/* SRS09 */
+#define STATUS_DATA_BUSY			BIT(2)
+
+/* SRS10 */
+/* LED Control
+ * State of this bit directly drives led port of the host
+ * in order to control the external LED diode
+ * Default value 0 << 1
+ */
+#define LEDC						BIT(0)
+#define LEDC_OFF					0 << 1
+
+/* Data Transfer Width
+ * Bit used to configure DAT bus width to 1 or 4
+ * Default value 1 << 1
+ */
+#define DT_WIDTH					BIT(1)
+#define DTW_4BIT					1 << 1
+
+/* Extended Data Transfer Width
+ * This bit is to enable/disable 8-bit DAT bus width mode
+ * Default value 1 << 5
+ */
+#define EDTW_8BIT					1 << 5
+
+/* High Speed Enable
+ * Selects operating mode to Default Speed (HSE=0) or High Speed (HSE=1)
+ */
+#define HS_EN						BIT(2)
+
+/* here 0 defines the 64 Kb size */
+#define MAX_64KB_PAGE				0
+#define EMMC_DESC_SIZE		(1<<20)
+
+/* SRS11 */
+/* Software Reset For All
+ * When set to 1, the entire slot is reset
+ * After completing the reset operation, SRFA bit is automatically cleared
+ */
+#define SRFA						BIT(24)
+
+/* Software Reset For CMD Line
+ * When set to 1, resets the logic related to the command generation and response checking
+ */
+#define SRCMD						BIT(25)
+
+/* Software Reset For DAT Line
+ * When set to 1, resets the logic related to the data path,
+ * including data buffers and the DMA logic
+ */
+#define SRDAT						BIT(26)
+
+/* SRS15 */
+/* UHS Mode Select
+ * Used to select one of UHS-I modes.
+ * • 000b - SDR12
+ * • 001b - SDR25
+ * • 010b - SDR50
+ * • 011b - SDR104
+ * • 100b - DDR50
+ */
+#define SDR12_MODE					0 << 16
+#define SDR25_MODE					1 << 16
+#define SDR50_MODE					2 << 16
+#define SDR104_MODE					3 << 16
+#define DDR50_MODE					4 << 16
+/* 1.8V Signaling Enable
+ * • 0 - for Default Speed, High Speed mode
+ * • 1 - for UHS-I mode
+ */
+#define V18SE						BIT(19)
+
+/* CMD23 Enable
+ * In result of Card Identification process,
+ * Host Driver set this bit to 1 if Card supports CMD23
+ */
+#define CMD23_EN					BIT(27)
+
+/* Host Version 4.00 Enable
+ * • 0 - Version 3.00
+ * • 1 - Version 4.00
+ */
+#define HV4E						BIT(28)
+/* Conf depends on SRS15.HV4E */
+#define SDMA						0 << 3
+#define ADMA2_32					2 << 3
+#define ADMA2_64					3 << 3
+
+/* Preset Value Enable
+ * Setting this bit to 1 triggers an automatically update of SRS11
+ */
+#define PVE							BIT(31)
+
+#define BIT_AD_32					0 << 29
+#define BIT_AD_64					1 << 29
+
+/* SW RESET REG*/
+#define SDHC_CDNS_HRS00				(0x00)
+#define SDHC_CDNS_HRS00_SWR			BIT(0)
+
+/* PHY access port */
+#define SDHC_CDNS_HRS04				0x10
+#define SDHC_CDNS_HRS04_ADDR		GENMASK(5, 0)
+
+/* PHY data access port */
+#define SDHC_CDNS_HRS05				0x14
+
+/* eMMC control registers */
+#define SDHC_CDNS_HRS06				0x18
+
+/* SRS */
+#define SDHC_CDNS_SRS_BASE			0x200
+#define SDHC_CDNS_SRS00				0x200
+#define SDHC_CDNS_SRS01				0x204
+#define SDHC_CDNS_SRS02				0x208
+#define SDHC_CDNS_SRS03				0x20c
+#define SDHC_CDNS_SRS04				0x210
+#define SDHC_CDNS_SRS05				0x214
+#define SDHC_CDNS_SRS06				0x218
+#define SDHC_CDNS_SRS07				0x21C
+#define SDHC_CDNS_SRS08				0x220
+#define SDHC_CDNS_SRS09				0x224
+#define SDHC_CDNS_SRS09_CI			BIT(16)
+#define SDHC_CDNS_SRS10				0x228
+#define SDHC_CDNS_SRS11				0x22C
+#define SDHC_CDNS_SRS12				0x230
+#define SDHC_CDNS_SRS13				0x234
+#define SDHC_CDNS_SRS14				0x238
+#define SDHC_CDNS_SRS15				0x23c
+#define SDHC_CDNS_SRS21				0x254
+#define SDHC_CDNS_SRS22				0x258
+#define SDHC_CDNS_SRS23				0x25c
+
+/* HRS07 */
+#define SDHC_CDNS_HRS07				0x1c
+#define SDHC_IDELAY_VAL(x)			((x) << 0)
+#define SDHC_RW_COMPENSATE(x)		((x) << 16)
+
+/* PHY reset port */
+#define SDHC_CDNS_HRS09				0x24
+
+/* HRS10 */
+/* PHY reset port */
+#define SDHC_CDNS_HRS10				0x28
+
+/* HCSDCLKADJ DATA; DDR Mode */
+#define SDHC_HCSDCLKADJ(x)			((x) << 16)
+
+/* Pinmux headers will reomove after ATF driver implementation */
+#define PINMUX_SDMMC_SEL			0x0
+#define PIN0SEL						0x00
+#define PIN1SEL						0x04
+#define PIN2SEL						0x08
+#define PIN3SEL						0x0C
+#define PIN4SEL						0x10
+#define PIN5SEL						0x14
+#define PIN6SEL						0x18
+#define PIN7SEL						0x1C
+#define PIN8SEL						0x20
+#define PIN9SEL						0x24
+#define PIN10SEL					0x28
+
+/* HRS16 */
+#define SDHC_WRCMD0_DLY(x)			((x) << 0)
+#define SDHC_WRCMD1_DLY(x)			((x) << 4)
+#define SDHC_WRDATA0_DLY(x)			((x) << 8)
+#define SDHC_WRDATA1_DLY(x)			((x) << 12)
+#define SDHC_WRCMD0_SDCLK_DLY(x)	((x) << 16)
+#define SDHC_WRCMD1_SDCLK_DLY(x)	((x) << 20)
+#define SDHC_WRDATA0_SDCLK_DLY(x)	((x) << 24)
+#define SDHC_WRDATA1_SDCLK_DLY(x)	((x) << 28)
+
+/* Shared Macros */
+#define SDMMC_CDN(_reg)				(SDMMC_CDN_REG_BASE + \
+								(SDMMC_CDN_##_reg))
+
+/* Refer to atf/tools/cert_create/include/debug.h */
+#define BIT_32(nr)					(U(1) << (nr))
+
+/* MMC Peripheral Definition */
+#define SOCFPGA_MMC_BLOCK_SIZE		U(8192)
+#define SOCFPGA_MMC_BLOCK_MASK		(SOCFPGA_MMC_BLOCK_SIZE - U(1))
+#define SOCFPGA_MMC_BOOT_CLK_RATE	(400 * 1000)
+#define MMC_RESPONSE_NONE			0
+#define SDHC_CDNS_SRS03_VALUE		0x01020013
+
+/* Value randomly chosen for eMMC RCA, it should be > 1 */
+#define MMC_FIX_RCA					6
+#define RCA_SHIFT_OFFSET			16
+
+#define CMD_EXTCSD_PARTITION_CONFIG	179
+#define CMD_EXTCSD_BUS_WIDTH		183
+#define CMD_EXTCSD_HS_TIMING		185
+#define CMD_EXTCSD_SEC_CNT			212
+
+#define PART_CFG_BOOT_PARTITION1_ENABLE	(U(1) << 3)
+#define PART_CFG_PARTITION1_ACCESS	(U(1) << 0)
+
+/* Values in EXT CSD register */
+#define MMC_BUS_WIDTH_1				U(0)
+#define MMC_BUS_WIDTH_4				U(1)
+#define MMC_BUS_WIDTH_8				U(2)
+#define MMC_BUS_WIDTH_DDR_4			U(5)
+#define MMC_BUS_WIDTH_DDR_8			U(6)
+#define MMC_BOOT_MODE_BACKWARD		(U(0) << 3)
+#define MMC_BOOT_MODE_HS_TIMING		(U(1) << 3)
+#define MMC_BOOT_MODE_DDR			(U(2) << 3)
+
+#define EXTCSD_SET_CMD				(U(0) << 24)
+#define EXTCSD_SET_BITS				(U(1) << 24)
+#define EXTCSD_CLR_BITS				(U(2) << 24)
+#define EXTCSD_WRITE_BYTES			(U(3) << 24)
+#define EXTCSD_CMD(x)				(((x) & 0xff) << 16)
+#define EXTCSD_VALUE(x)				(((x) & 0xff) << 8)
+#define EXTCSD_CMD_SET_NORMAL		U(1)
+
+#define CSD_TRAN_SPEED_UNIT_MASK	GENMASK(2, 0)
+#define CSD_TRAN_SPEED_MULT_MASK	GENMASK(6, 3)
+#define CSD_TRAN_SPEED_MULT_SHIFT	3
+
+#define STATUS_CURRENT_STATE(x)		(((x) & 0xf) << 9)
+#define STATUS_READY_FOR_DATA		BIT(8)
+#define STATUS_SWITCH_ERROR			BIT(7)
+#define MMC_GET_STATE(x)			(((x) >> 9) & 0xf)
+#define MMC_STATE_IDLE				0
+#define MMC_STATE_READY				1
+#define MMC_STATE_IDENT				2
+#define MMC_STATE_STBY				3
+#define MMC_STATE_TRAN				4
+#define MMC_STATE_DATA				5
+#define MMC_STATE_RCV				6
+#define MMC_STATE_PRG				7
+#define MMC_STATE_DIS				8
+#define MMC_STATE_BTST				9
+#define MMC_STATE_SLP				10
+
+#define MMC_FLAG_CMD23				(U(1) << 0)
+
+#define CMD8_CHECK_PATTERN			U(0xAA)
+#define VHS_2_7_3_6_V				BIT(8)
+
+/*ADMA table component*/
+#define ADMA_DESC_ATTR_VALID		BIT(0)
+#define ADMA_DESC_ATTR_END			BIT(1)
+#define ADMA_DESC_ATTR_INT			BIT(2)
+#define ADMA_DESC_ATTR_ACT1			BIT(4)
+#define ADMA_DESC_ATTR_ACT2			BIT(5)
+#define ADMA_DESC_TRANSFER_DATA		ADMA_DESC_ATTR_ACT2
+
+enum sd_opcode {
+	SD_GO_IDLE_STATE = 0,
+	SD_ALL_SEND_CID = 2,
+	SD_SEND_RELATIVE_ADDR = 3,
+	SDIO_SEND_OP_COND = 5, /* SDIO cards only */
+	SD_SWITCH = 6,
+	SD_SELECT_CARD = 7,
+	SD_SEND_IF_COND = 8,
+	SD_SEND_CSD = 9,
+	SD_SEND_CID = 10,
+	SD_VOL_SWITCH = 11,
+	SD_STOP_TRANSMISSION = 12,
+	SD_SEND_STATUS = 13,
+	SD_GO_INACTIVE_STATE = 15,
+	SD_SET_BLOCK_SIZE = 16,
+	SD_READ_SINGLE_BLOCK = 17,
+	SD_READ_MULTIPLE_BLOCK = 18,
+	SD_SEND_TUNING_BLOCK = 19,
+	SD_SET_BLOCK_COUNT = 23,
+	SD_WRITE_SINGLE_BLOCK = 24,
+	SD_WRITE_MULTIPLE_BLOCK = 25,
+	SD_ERASE_BLOCK_START = 32,
+	SD_ERASE_BLOCK_END = 33,
+	SD_ERASE_BLOCK_OPERATION = 38,
+	SD_APP_CMD = 55,
+	SD_SPI_READ_OCR = 58, /* SPI mode only */
+	SD_SPI_CRC_ON_OFF = 59, /* SPI mode only */
+};
+
+enum sd_app_cmd {
+	SD_APP_SET_BUS_WIDTH = 6,
+	SD_APP_SEND_STATUS = 13,
+	SD_APP_SEND_NUM_WRITTEN_BLK = 22,
+	SD_APP_SET_WRITE_BLK_ERASE_CNT = 23,
+	SD_APP_SEND_OP_COND = 41,
+	SD_APP_CLEAR_CARD_DETECT = 42,
+	SD_APP_SEND_SCR = 51,
+};
+
+struct cdns_sdmmc_sdhc {
+	uint32_t	sdhc_extended_rd_mode;
+	uint32_t	sdhc_extended_wr_mode;
+	uint32_t	sdhc_hcsdclkadj;
+	uint32_t	sdhc_idelay_val;
+	uint32_t	sdhc_rdcmd_en;
+	uint32_t	sdhc_rddata_en;
+	uint32_t	sdhc_rw_compensate;
+	uint32_t	sdhc_sdcfsh;
+	uint32_t	sdhc_sdcfsl;
+	uint32_t	sdhc_wrcmd0_dly;
+	uint32_t	sdhc_wrcmd0_sdclk_dly;
+	uint32_t	sdhc_wrcmd1_dly;
+	uint32_t	sdhc_wrcmd1_sdclk_dly;
+	uint32_t	sdhc_wrdata0_dly;
+	uint32_t	sdhc_wrdata0_sdclk_dly;
+	uint32_t	sdhc_wrdata1_dly;
+	uint32_t	sdhc_wrdata1_sdclk_dly;
+};
+
+enum sdmmc_device_mode {
+	SD_DS_ID, /* Identification */
+	SD_DS, /* Default speed */
+	SD_HS, /* High speed */
+	SD_UHS_SDR12, /* Ultra high speed SDR12 */
+	SD_UHS_SDR25, /* Ultra high speed SDR25 */
+	SD_UHS_SDR50, /* Ultra high speed SDR`50 */
+	SD_UHS_SDR104, /* Ultra high speed SDR104 */
+	SD_UHS_DDR50, /* Ultra high speed DDR50 */
+	EMMC_SDR_BC, /* SDR backward compatible */
+	EMMC_SDR, /* SDR */
+	EMMC_DDR, /* DDR */
+	EMMC_HS200, /* High speed 200Mhz in SDR */
+	EMMC_HS400, /* High speed 200Mhz in DDR */
+	EMMC_HS400es, /* High speed 200Mhz in SDR with enhanced strobe*/
+};
+
+struct cdns_sdmmc_params {
+	uintptr_t	reg_base;
+	uintptr_t	reg_pinmux;
+	uintptr_t	reg_phy;
+	uintptr_t	desc_base;
+	size_t		desc_size;
+	int		clk_rate;
+	int		bus_width;
+	unsigned int	flags;
+	enum sdmmc_device_mode	cdn_sdmmc_dev_mode;
+	enum mmc_device_type	cdn_sdmmc_dev_type;
+	uint32_t	combophy;
+};
+
+/* read and write API */
+size_t sdmmc_read_blocks(int lba, uintptr_t buf, size_t size);
+size_t sdmmc_write_blocks(int lba, const uintptr_t buf, size_t size);
+
+struct cdns_idmac_desc {
+	/*8 bit attribute*/
+	uint8_t attr;
+	/*reserved bits in desc*/
+	uint8_t reserved;
+	/*page length for the descriptor*/
+	uint16_t len;
+	/*lower 32 bits for buffer (64 bit addressing)*/
+	uint32_t addr_lo;
+#if CONFIG_DMA_ADDR_T_64BIT == 1
+	/*higher 32 bits for buffer (64 bit addressing)*/
+	uint32_t addr_hi;
+} __aligned(8);
+#else
+} __packed;
+#endif
+
+
+
+/* Function Prototype */
+int cdns_sd_host_init(struct cdns_sdmmc_combo_phy *mmc_combo_phy_reg,
+struct cdns_sdmmc_sdhc *mmc_sdhc_reg);
+void cdns_set_sdmmc_var(struct cdns_sdmmc_combo_phy *combo_phy_reg,
+struct cdns_sdmmc_sdhc *sdhc_reg);
+#endif
diff --git a/include/drivers/measured_boot/rss/rss_measured_boot.h b/include/drivers/measured_boot/rss/rss_measured_boot.h
index fe88576..76affd8 100644
--- a/include/drivers/measured_boot/rss/rss_measured_boot.h
+++ b/include/drivers/measured_boot/rss/rss_measured_boot.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -44,12 +44,14 @@
 };
 
 /* 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,
+void rss_measured_boot_init(struct rss_mboot_metadata *metadata_ptr);
+int rss_mboot_measure_and_record(struct rss_mboot_metadata *metadata_ptr,
+				 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);
+int rss_mboot_set_signer_id(struct rss_mboot_metadata *metadata_ptr,
+			    unsigned int img_id, const void *pk_ptr,
+			    size_t pk_len);
 
 #endif /* RSS_MEASURED_BOOT_H */
diff --git a/include/drivers/nxp/trdc/imx_trdc.h b/include/drivers/nxp/trdc/imx_trdc.h
new file mode 100644
index 0000000..0b41fcf
--- /dev/null
+++ b/include/drivers/nxp/trdc/imx_trdc.h
@@ -0,0 +1,172 @@
+/*
+ * Copyright 2022-2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_TRDC_H
+#define IMX_XRDC_H
+
+#define MBC_BLK_ALL	U(255)
+#define MRC_REG_ALL	U(16)
+#define GLBAC_NUM	U(8)
+
+#define DID_NUM		U(16)
+#define MBC_MAX_NUM	U(4)
+#define MRC_MAX_NUM	U(2)
+#define MBC_NUM(HWCFG)	(((HWCFG) >> 16) & 0xF)
+#define MRC_NUM(HWCFG)	(((HWCFG) >> 24) & 0x1F)
+
+#define MBC_BLK_NUM(GLBCFG)	((GLBCFG) & 0x3FF)
+#define MRC_RGN_NUM(GLBCFG)	((GLBCFG) & 0x1F)
+
+#define MDAC_W_X(m, r)	(0x800 + (m) * 0x20 + (r) * 0x4)
+
+/* CPU/non-CPU domain common bits */
+#define MDA_VLD		BIT(31)
+#define MDA_LK1		BIT(30)
+#define MDA_DFMT	BIT(29)
+
+/* CPU domain bits */
+#define MDA_DFMT0_DID(x)	((x) & 0xF)
+#define MDA_DFMT0_DIDS(x)	(((x) & 0x3) << 4)
+#define MDA_DFMT0_PE(x)		(((x) & 0x3) << 6)
+#define MDA_DFMT0_PIDM(x)	(((x) & 0x3F) << 8)
+#define MDA_DFMT0_SA(x)		(((x) & 0x3) << 14)
+#define MDA_DFMT0_PID(x)	(((x) & 0x3F) << 16)
+
+/* non-CPU domain bits */
+#define MDA_DFMT1_DID(x)	((x) & 0xF)
+#define MDA_DFMT1_PA(x)		(((x) & 0x3) << 4)
+#define MDA_DFMT1_SA(x)		(((x) & 0x3) << 6)
+#define MDA_DFMT1_DIDB(x)	((x) << 8)
+
+#define SP(X)	((X) << 12)
+#define SU(X)	((X) << 8)
+#define NP(X)	((X) << 4)
+#define NU(X)	((X) << 0)
+
+#define RWX	U(7)
+#define RW	U(6)
+#define RX	U(5)
+#define R	U(4)
+#define X	U(1)
+
+struct mbc_mem_dom {
+	uint32_t mem_glbcfg[4];
+	uint32_t nse_blk_index;
+	uint32_t nse_blk_set;
+	uint32_t nse_blk_clr;
+	uint32_t nsr_blk_clr_all;
+	uint32_t memn_glbac[8];
+	/* The upper only existed in the beginning of each MBC */
+	uint32_t mem0_blk_cfg_w[64];
+	uint32_t mem0_blk_nse_w[16];
+	uint32_t mem1_blk_cfg_w[8];
+	uint32_t mem1_blk_nse_w[2];
+	uint32_t mem2_blk_cfg_w[8];
+	uint32_t mem2_blk_nse_w[2];
+	uint32_t mem3_blk_cfg_w[8];
+	uint32_t mem3_blk_nse_w[2]; /*0x1F0, 0x1F4 */
+	uint32_t reserved[2];
+};
+
+struct mrc_rgn_dom {
+	uint32_t mrc_glbcfg[4];
+	uint32_t nse_rgn_indirect;
+	uint32_t nse_rgn_set;
+	uint32_t nse_rgn_clr;
+	uint32_t nse_rgn_clr_all;
+	uint32_t memn_glbac[8];
+	/* The upper only existed in the beginning of each MRC */
+	uint32_t rgn_desc_words[16][2]; /* 16 regions at max, 2 words per region */
+	uint32_t rgn_nse;
+	uint32_t reserved2[15];
+};
+
+struct mda_inst {
+	uint32_t mda_w[8];
+};
+
+struct trdc_mgr {
+	uint32_t trdc_cr;
+	uint32_t res0[59];
+	uint32_t trdc_hwcfg0;
+	uint32_t trdc_hwcfg1;
+	uint32_t res1[450];
+	struct mda_inst mda[128];
+};
+
+struct trdc_mbc {
+	struct mbc_mem_dom mem_dom[DID_NUM];
+};
+
+struct trdc_mrc {
+	struct mrc_rgn_dom mrc_dom[DID_NUM];
+};
+
+/***************************************************************
+ * Below structs used fro provding the TRDC configuration info
+ * that will be used to init the TRDC based on use case.
+ ***************************************************************/
+struct trdc_glbac_config {
+	uint8_t mbc_mrc_id;
+	uint8_t glbac_id;
+	uint32_t glbac_val;
+};
+
+struct trdc_mbc_config {
+	uint8_t mbc_id;
+	uint8_t dom_id;
+	uint8_t mem_id;
+	uint8_t blk_id;
+	uint8_t glbac_id;
+	bool secure;
+};
+
+struct trdc_mrc_config {
+	uint8_t mrc_id;
+	uint8_t dom_id;
+	uint8_t region_id;
+	uint32_t region_start;
+	uint32_t region_size;
+	uint8_t glbac_id;
+	bool secure;
+};
+
+struct trdc_mgr_info {
+	uintptr_t trdc_base;
+	uint8_t mbc_id;
+	uint8_t mbc_mem_id;
+	uint8_t blk_mgr;
+	uint8_t blk_mc;
+};
+
+struct trdc_config_info {
+	uintptr_t trdc_base;
+	struct trdc_glbac_config *mbc_glbac;
+	uint32_t num_mbc_glbac;
+	struct trdc_mbc_config *mbc_cfg;
+	uint32_t num_mbc_cfg;
+	struct trdc_glbac_config *mrc_glbac;
+	uint32_t num_mrc_glbac;
+	struct trdc_mrc_config *mrc_cfg;
+	uint32_t num_mrc_cfg;
+};
+
+extern struct trdc_mgr_info trdc_mgr_blks[];
+extern unsigned int trdc_mgr_num;
+/* APIs to apply and enable TRDC */
+int trdc_mda_set_cpu(uintptr_t trdc_base, uint32_t mda_inst,
+		     uint32_t mda_reg, uint8_t sa, uint8_t dids,
+		     uint8_t did, uint8_t pe, uint8_t pidm, uint8_t pid);
+
+int trdc_mda_set_noncpu(uintptr_t trdc_base, uint32_t mda_inst,
+			bool did_bypass, uint8_t sa, uint8_t pa,
+			uint8_t did);
+
+void trdc_mgr_mbc_setup(struct trdc_mgr_info *mgr);
+void trdc_setup(struct trdc_config_info *cfg);
+void trdc_config(void);
+
+#endif /* IMX_TRDC_H */
diff --git a/include/export/lib/utils_def_exp.h b/include/export/lib/utils_def_exp.h
index d4a4a85..2ee5769 100644
--- a/include/export/lib/utils_def_exp.h
+++ b/include/export/lib/utils_def_exp.h
@@ -27,10 +27,14 @@
 #else
 # define  U_(_x)	(_x##U)
 # define   U(_x)	U_(_x)
-# define  UL(_x)	(_x##UL)
-# define ULL(_x)	(_x##ULL)
-# define   L(_x)	(_x##L)
-# define  LL(_x)	(_x##LL)
+# define  UL_(_x)	(_x##UL)
+# define  UL(_x)	UL_(_x)
+# define  ULL_(_x)	(_x##ULL)
+# define  ULL(_x)	ULL_(_x)
+# define  L_(_x)	(_x##L)
+# define  L(_x)	L_(_x)
+# define  LL_(_x)	(_x##LL)
+# define  LL(_x)	LL_(_x)
 
 #endif
 
diff --git a/include/lib/cpus/aarch64/cortex_a510.h b/include/lib/cpus/aarch64/cortex_a510.h
index 6af85a8..337aac3 100644
--- a/include/lib/cpus/aarch64/cortex_a510.h
+++ b/include/lib/cpus/aarch64/cortex_a510.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, ARM Limited. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,11 +14,13 @@
  ******************************************************************************/
 #define CORTEX_A510_CPUECTLR_EL1				S3_0_C15_C1_4
 #define CORTEX_A510_CPUECTLR_EL1_READPREFERUNIQUE_SHIFT		U(19)
+#define CORTEX_A510_CPUECTLR_EL1_READPREFERUNIQUE_WIDTH		U(1)
 #define CORTEX_A510_CPUECTLR_EL1_READPREFERUNIQUE_DISABLE	U(1)
 #define CORTEX_A510_CPUECTLR_EL1_RSCTL_SHIFT			U(23)
 #define CORTEX_A510_CPUECTLR_EL1_NTCTL_SHIFT			U(46)
 #define CORTEX_A510_CPUECTLR_EL1_ATOM_EXECALLINSTRNEAR		U(2)
-#define CORTEX_A510_CPUECTLR_EL1_ATOM				U(38)
+#define CORTEX_A510_CPUECTLR_EL1_ATOM_SHIFT			U(38)
+#define CORTEX_A510_CPUECTLR_EL1_ATOM_WIDTH			U(3)
 
 /*******************************************************************************
  * CPU Power Control register specific definitions
@@ -30,6 +32,12 @@
  * Complex auxiliary control register specific definitions
  ******************************************************************************/
 #define CORTEX_A510_CMPXACTLR_EL1				S3_0_C15_C1_3
+#define CORTEX_A510_CMPXACTLR_EL1_ALIAS_LOADSTORE_DISABLE	U(1)
+#define CORTEX_A510_CMPXACTLR_EL1_ALIAS_LOADSTORE_SHIFT		U(25)
+#define CORTEX_A510_CMPXACTLR_EL1_ALIAS_LOADSTORE_WIDTH		U(1)
+#define CORTEX_A510_CMPXACTLR_EL1_SNPPREFERUNIQUE_DISABLE	U(3)
+#define CORTEX_A510_CMPXACTLR_EL1_SNPPREFERUNIQUE_SHIFT		U(10)
+#define CORTEX_A510_CMPXACTLR_EL1_SNPPREFERUNIQUE_WIDTH		U(2)
 
 /*******************************************************************************
  * Auxiliary control register specific definitions
@@ -37,5 +45,11 @@
 #define CORTEX_A510_CPUACTLR_EL1				S3_0_C15_C1_0
 #define CORTEX_A510_CPUACTLR_EL1_BIT_17				(ULL(1) << 17)
 #define CORTEX_A510_CPUACTLR_EL1_BIT_38				(ULL(1) << 38)
+#define CORTEX_A510_CPUACTLR_EL1_ALIAS_LOADSTORE_DISABLE	U(1)
+#define CORTEX_A510_CPUACTLR_EL1_ALIAS_LOADSTORE_SHIFT		U(18)
+#define CORTEX_A510_CPUACTLR_EL1_ALIAS_LOADSTORE_WIDTH		U(1)
+#define CORTEX_A510_CPUACTLR_EL1_DATA_CORRUPT_DISABLE		U(1)
+#define CORTEX_A510_CPUACTLR_EL1_DATA_CORRUPT_SHIFT		U(18)
+#define CORTEX_A510_CPUACTLR_EL1_DATA_CORRUPT_WIDTH		U(1)
 
-#endif /* CORTEX_A510_H */
\ No newline at end of file
+#endif /* CORTEX_A510_H */
diff --git a/include/lib/cpus/aarch64/cortex_a520.h b/include/lib/cpus/aarch64/cortex_a520.h
new file mode 100644
index 0000000..4176981
--- /dev/null
+++ b/include/lib/cpus/aarch64/cortex_a520.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CORTEX_A520_H
+#define CORTEX_A520_H
+
+#define CORTEX_A520_MIDR					U(0x410FD800)
+
+/*******************************************************************************
+ * CPU Extended Control register specific definitions
+ ******************************************************************************/
+#define CORTEX_A520_CPUECTLR_EL1				S3_0_C15_C1_4
+
+/*******************************************************************************
+ * CPU Power Control register specific definitions
+ ******************************************************************************/
+#define CORTEX_A520_CPUPWRCTLR_EL1				S3_0_C15_C2_7
+#define CORTEX_A520_CPUPWRCTLR_EL1_CORE_PWRDN_BIT		U(1)
+
+#endif /* CORTEX_A520_H */
diff --git a/include/lib/cpus/aarch64/cortex_a720.h b/include/lib/cpus/aarch64/cortex_a720.h
new file mode 100644
index 0000000..47bbbc0
--- /dev/null
+++ b/include/lib/cpus/aarch64/cortex_a720.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CORTEX_A720_H
+#define CORTEX_A720_H
+
+#define CORTEX_A720_MIDR					U(0x410FD810)
+
+/* Cortex A720 loop count for CVE-2022-23960 mitigation */
+#define CORTEX_A720_BHB_LOOP_COUNT				U(132)
+
+/*******************************************************************************
+ * CPU Extended Control register specific definitions
+ ******************************************************************************/
+#define CORTEX_A720_CPUECTLR_EL1				S3_0_C15_C1_4
+
+/*******************************************************************************
+ * CPU Power Control register specific definitions
+ ******************************************************************************/
+#define CORTEX_A720_CPUPWRCTLR_EL1				S3_0_C15_C2_7
+#define CORTEX_A720_CPUPWRCTLR_EL1_CORE_PWRDN_BIT		U(1)
+
+#endif /* CORTEX_A720_H */
diff --git a/include/lib/cpus/aarch64/cortex_hayes.h b/include/lib/cpus/aarch64/cortex_hayes.h
deleted file mode 100644
index 82022e9..0000000
--- a/include/lib/cpus/aarch64/cortex_hayes.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef CORTEX_HAYES_H
-#define CORTEX_HAYES_H
-
-#define CORTEX_HAYES_MIDR					U(0x410FD800)
-
-/*******************************************************************************
- * CPU Extended Control register specific definitions
- ******************************************************************************/
-#define CORTEX_HAYES_CPUECTLR_EL1				S3_0_C15_C1_4
-
-/*******************************************************************************
- * CPU Power Control register specific definitions
- ******************************************************************************/
-#define CORTEX_HAYES_CPUPWRCTLR_EL1				S3_0_C15_C2_7
-#define CORTEX_HAYES_CPUPWRCTLR_EL1_CORE_PWRDN_BIT		U(1)
-
-#endif /* CORTEX_HAYES_H */
diff --git a/include/lib/cpus/aarch64/cortex_hunter.h b/include/lib/cpus/aarch64/cortex_hunter.h
deleted file mode 100644
index 24bd217..0000000
--- a/include/lib/cpus/aarch64/cortex_hunter.h
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
- * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef CORTEX_HUNTER_H
-#define CORTEX_HUNTER_H
-
-#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
- ******************************************************************************/
-#define CORTEX_HUNTER_CPUECTLR_EL1				S3_0_C15_C1_4
-
-/*******************************************************************************
- * CPU Power Control register specific definitions
- ******************************************************************************/
-#define CORTEX_HUNTER_CPUPWRCTLR_EL1				S3_0_C15_C2_7
-#define CORTEX_HUNTER_CPUPWRCTLR_EL1_CORE_PWRDN_BIT		U(1)
-
-#endif /* CORTEX_HUNTER_H */
diff --git a/include/lib/cpus/aarch64/cortex_hunter_elp_arm.h b/include/lib/cpus/aarch64/cortex_hunter_elp_arm.h
deleted file mode 100644
index f9bb0f3..0000000
--- a/include/lib/cpus/aarch64/cortex_hunter_elp_arm.h
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef CORTEX_HUNTER_ELP_ARM_H
-#define CORTEX_HUNTER_ELP_ARM_H
-
-#define CORTEX_HUNTER_ELP_ARM_MIDR					U(0x410FD821)
-
-/* Cortex Hunter ELP loop count for CVE-2022-23960 mitigation */
-#define CORTEX_HUNTER_ELP_ARM_BHB_LOOP_COUNT				U(132)
-
-/*******************************************************************************
- * CPU Extended Control register specific definitions
- ******************************************************************************/
-#define CORTEX_HUNTER_ELP_ARM_CPUECTLR_EL1				S3_0_C15_C1_4
-
-/*******************************************************************************
- * CPU Power Control register specific definitions
- ******************************************************************************/
-#define CORTEX_HUNTER_ELP_ARM_CPUPWRCTLR_EL1				S3_0_C15_C2_7
-#define CORTEX_HUNTER_ELP_ARM_CPUPWRCTLR_EL1_CORE_PWRDN_BIT		U(1)
-
-#endif /* CORTEX_HUNTER_ELP_ARM_H */
diff --git a/include/lib/cpus/aarch64/cortex_x4.h b/include/lib/cpus/aarch64/cortex_x4.h
new file mode 100644
index 0000000..17d07c8
--- /dev/null
+++ b/include/lib/cpus/aarch64/cortex_x4.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CORTEX_X4_H
+#define CORTEX_X4_H
+
+#define CORTEX_X4_MIDR					U(0x410FD821)
+
+/* Cortex X4 loop count for CVE-2022-23960 mitigation */
+#define CORTEX_X4_BHB_LOOP_COUNT			U(132)
+
+/*******************************************************************************
+ * CPU Extended Control register specific definitions
+ ******************************************************************************/
+#define CORTEX_X4_CPUECTLR_EL1				S3_0_C15_C1_4
+
+/*******************************************************************************
+ * CPU Power Control register specific definitions
+ ******************************************************************************/
+#define CORTEX_X4_CPUPWRCTLR_EL1			S3_0_C15_C2_7
+#define CORTEX_X4_CPUPWRCTLR_EL1_CORE_PWRDN_BIT		U(1)
+
+#endif /* CORTEX_X4_H */
diff --git a/include/lib/cpus/aarch64/cpu_macros.S b/include/lib/cpus/aarch64/cpu_macros.S
index 404b7f9..d945d7c 100644
--- a/include/lib/cpus/aarch64/cpu_macros.S
+++ b/include/lib/cpus/aarch64/cpu_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2023, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -444,6 +444,19 @@
 .endm
 
 /*
+ * BFI : Inserts bitfield into a system register.
+ *
+ * BFI{cond} Rd, Rn, #lsb, #width
+ */
+.macro sysreg_bitfield_insert _reg:req, _src:req, _lsb:req, _width:req
+	/* Source value for BFI */
+	mov	x1, #\_src
+	mrs	x0, \_reg
+	bfi	x0, x1, #\_lsb, #\_width
+	msr	\_reg, x0
+.endm
+
+/*
  * Apply erratum
  *
  * _cpu:
diff --git a/include/lib/cpus/aarch64/neoverse_hermes.h b/include/lib/cpus/aarch64/neoverse_hermes.h
new file mode 100644
index 0000000..22492c3
--- /dev/null
+++ b/include/lib/cpus/aarch64/neoverse_hermes.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef NEOVERSE_HERMES_H
+#define NEOVERSE_HERMES_H
+
+#define NEOVERSE_HERMES_MIDR				U(0x410FD8E0)
+
+/*******************************************************************************
+ * CPU Extended Control register specific definitions
+ ******************************************************************************/
+#define NEOVERSE_HERMES_CPUECTLR_EL1			S3_0_C15_C1_4
+
+/*******************************************************************************
+ * CPU Power Control register specific definitions
+ ******************************************************************************/
+#define NEOVERSE_HERMES_CPUPWRCTLR_EL1			S3_0_C15_C2_7
+#define NEOVERSE_HERMES_CPUPWRCTLR_EL1_CORE_PWRDN_BIT	U(1)
+
+#endif /* NEOVERSE_HERMES_H */
diff --git a/include/lib/el3_runtime/context_mgmt.h b/include/lib/el3_runtime/context_mgmt.h
index 1a76d8e..aa76f3b 100644
--- a/include/lib/el3_runtime/context_mgmt.h
+++ b/include/lib/el3_runtime/context_mgmt.h
@@ -37,6 +37,9 @@
 void cm_prepare_el3_exit_ns(void);
 
 #ifdef __aarch64__
+#if IMAGE_BL31
+void cm_manage_extensions_el3(void);
+#endif
 #if CTX_INCLUDE_EL2_REGS
 void cm_el2_sysregs_context_save(uint32_t security_state);
 void cm_el2_sysregs_context_restore(uint32_t security_state);
@@ -84,6 +87,7 @@
 #else
 void *cm_get_next_context(void);
 void cm_set_next_context(void *context);
+static inline void cm_manage_extensions_el3(void) {}
 #endif /* __aarch64__ */
 
 #endif /* CONTEXT_MGMT_H */
diff --git a/include/lib/extensions/amu.h b/include/lib/extensions/amu.h
index de476e4..80ad68c 100644
--- a/include/lib/extensions/amu.h
+++ b/include/lib/extensions/amu.h
@@ -16,13 +16,21 @@
 
 #if ENABLE_FEAT_AMU
 #if __aarch64__
-void amu_enable(bool el2_unused, cpu_context_t *ctx);
+void amu_enable(cpu_context_t *ctx);
+void amu_init_el3(void);
+void amu_init_el2_unused(void);
 #else
 void amu_enable(bool el2_unused);
 #endif
 #else
 #if __aarch64__
-static inline void amu_enable(bool el2_unused, cpu_context_t *ctx)
+void amu_enable(cpu_context_t *ctx)
+{
+}
+void amu_init_el3(void)
+{
+}
+void amu_init_el2_unused(void)
 {
 }
 #else
diff --git a/include/lib/extensions/brbe.h b/include/lib/extensions/brbe.h
index 9ee2444..194efba 100644
--- a/include/lib/extensions/brbe.h
+++ b/include/lib/extensions/brbe.h
@@ -8,9 +8,9 @@
 #define BRBE_H
 
 #if ENABLE_BRBE_FOR_NS
-void brbe_enable(void);
+void brbe_init_el3(void);
 #else
-static inline void brbe_enable(void)
+static inline void brbe_init_el3(void)
 {
 }
 #endif /* ENABLE_BRBE_FOR_NS */
diff --git a/include/lib/extensions/mpam.h b/include/lib/extensions/mpam.h
index 4327278..e5438ce 100644
--- a/include/lib/extensions/mpam.h
+++ b/include/lib/extensions/mpam.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,11 +10,15 @@
 #include <stdbool.h>
 
 #if ENABLE_MPAM_FOR_LOWER_ELS
-void mpam_enable(bool el2_unused);
+void mpam_init_el3(void);
+void mpam_init_el2_unused(void);
 #else
-static inline void mpam_enable(bool el2_unused)
+static inline void mpam_init_el3(void)
 {
 }
-#endif
+static inline void mpam_init_el2_unused(void)
+{
+}
+#endif /* ENABLE_MPAM_FOR_LOWER_ELS */
 
 #endif /* MPAM_H */
diff --git a/include/lib/extensions/pmuv3.h b/include/lib/extensions/pmuv3.h
new file mode 100644
index 0000000..62fee7b
--- /dev/null
+++ b/include/lib/extensions/pmuv3.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PMUV3_H
+#define PMUV3_H
+
+#include <context.h>
+
+void pmuv3_init_el3(void);
+
+#ifdef __aarch64__
+void pmuv3_enable(cpu_context_t *ctx);
+void pmuv3_init_el2_unused(void);
+#endif /* __aarch64__ */
+
+#endif /* PMUV3_H */
diff --git a/include/lib/extensions/sme.h b/include/lib/extensions/sme.h
index 0e9c4b9..dbefdfc 100644
--- a/include/lib/extensions/sme.h
+++ b/include/lib/extensions/sme.h
@@ -22,11 +22,19 @@
 
 #if ENABLE_SME_FOR_NS
 void sme_enable(cpu_context_t *context);
+void sme_init_el3(void);
+void sme_init_el2_unused(void);
 void sme_disable(cpu_context_t *context);
 #else
 static inline void sme_enable(cpu_context_t *context)
 {
 }
+static inline void sme_init_el3(void)
+{
+}
+static inline void sme_init_el2_unused(void)
+{
+}
 static inline void sme_disable(cpu_context_t *context)
 {
 }
diff --git a/include/lib/extensions/spe.h b/include/lib/extensions/spe.h
index 02fccae..7b39037 100644
--- a/include/lib/extensions/spe.h
+++ b/include/lib/extensions/spe.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,15 +10,19 @@
 #include <stdbool.h>
 
 #if ENABLE_SPE_FOR_NS
-void spe_enable(bool el2_unused);
+void spe_init_el3(void);
+void spe_init_el2_unused(void);
 void spe_disable(void);
 #else
-static inline void spe_enable(bool el2_unused)
+static inline void spe_init_el3(void)
 {
 }
+static inline void spe_init_el2_unused(void)
+{
+}
 static inline void spe_disable(void)
 {
 }
-#endif
+#endif /* ENABLE_SPE_FOR_NS */
 
 #endif /* SPE_H */
diff --git a/include/lib/extensions/sve.h b/include/lib/extensions/sve.h
index 1faed2d..fc76a16 100644
--- a/include/lib/extensions/sve.h
+++ b/include/lib/extensions/sve.h
@@ -11,11 +11,15 @@
 
 #if (ENABLE_SME_FOR_NS || ENABLE_SVE_FOR_NS)
 void sve_enable(cpu_context_t *context);
+void sve_init_el2_unused(void);
 void sve_disable(cpu_context_t *context);
 #else
 static inline void sve_enable(cpu_context_t *context)
 {
 }
+static inline void sve_init_el2_unused(void)
+{
+}
 static inline void sve_disable(cpu_context_t *context)
 {
 }
diff --git a/include/lib/extensions/sys_reg_trace.h b/include/lib/extensions/sys_reg_trace.h
index 5915c55..beda88a 100644
--- a/include/lib/extensions/sys_reg_trace.h
+++ b/include/lib/extensions/sys_reg_trace.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,10 +10,13 @@
 #include <context.h>
 
 #if ENABLE_SYS_REG_TRACE_FOR_NS
+
 #if __aarch64__
 void sys_reg_trace_enable(cpu_context_t *context);
+void sys_reg_trace_disable(cpu_context_t *context);
+void sys_reg_trace_init_el2_unused(void);
 #else
-void sys_reg_trace_enable(void);
+void sys_reg_trace_init_el3(void);
 #endif /* __aarch64__ */
 
 #else /* !ENABLE_SYS_REG_TRACE_FOR_NS */
@@ -22,11 +25,18 @@
 static inline void sys_reg_trace_enable(cpu_context_t *context)
 {
 }
+static inline void sys_reg_trace_disable(cpu_context_t *context)
+{
+}
+static inline void sys_reg_trace_init_el2_unused(void)
+{
+}
 #else
-static inline void sys_reg_trace_enable(void)
+static inline void sys_reg_trace_init_el3(void)
 {
 }
 #endif /* __aarch64__ */
+
 #endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
 
 #endif /* SYS_REG_TRACE_H */
diff --git a/include/lib/extensions/trbe.h b/include/lib/extensions/trbe.h
index 861a4ad..0bed433 100644
--- a/include/lib/extensions/trbe.h
+++ b/include/lib/extensions/trbe.h
@@ -8,9 +8,13 @@
 #define TRBE_H
 
 #if ENABLE_TRBE_FOR_NS
-void trbe_enable(void);
+void trbe_init_el3(void);
+void trbe_init_el2_unused(void);
 #else
-static inline void trbe_enable(void)
+static inline void trbe_init_el3(void)
+{
+}
+static inline void trbe_init_el2_unused(void)
 {
 }
 #endif /* ENABLE_TRBE_FOR_NS */
diff --git a/include/lib/extensions/trf.h b/include/lib/extensions/trf.h
index 91a9615..1ac7cda 100644
--- a/include/lib/extensions/trf.h
+++ b/include/lib/extensions/trf.h
@@ -8,9 +8,13 @@
 #define TRF_H
 
 #if ENABLE_TRF_FOR_NS
-void trf_enable(void);
+void trf_init_el3(void);
+void trf_init_el2_unused(void);
 #else
-static inline void trf_enable(void)
+static inline void trf_init_el3(void)
+{
+}
+static inline void trf_init_el2_unused(void)
 {
 }
 #endif /* ENABLE_TRF_FOR_NS */
diff --git a/include/lib/libc/string.h b/include/lib/libc/string.h
index 9894483..7ddeed9 100644
--- a/include/lib/libc/string.h
+++ b/include/lib/libc/string.h
@@ -5,6 +5,7 @@
  */
 /*
  * Portions copyright (c) 2018-2020, ARM Limited and Contributors.
+ * Portions copyright (c) 2023, Intel Corporation. All rights reserved.
  * All rights reserved.
  */
 
@@ -14,6 +15,7 @@
 #include <stddef.h>
 
 void *memcpy(void *dst, const void *src, size_t len);
+int memcpy_s(void *dst, size_t dsize, void *src, size_t ssize);
 void *memmove(void *dst, const void *src, size_t len);
 int memcmp(const void *s1, const void *s2, size_t len);
 int strcmp(const char *s1, const char *s2);
diff --git a/include/lib/psa/psa_manifest/sid.h b/include/lib/psa/psa_manifest/sid.h
index be78bae..7183112 100644
--- a/include/lib/psa/psa_manifest/sid.h
+++ b/include/lib/psa/psa_manifest/sid.h
@@ -8,6 +8,9 @@
 #ifndef PSA_MANIFEST_SID_H
 #define PSA_MANIFEST_SID_H
 
+/******** RSS_SP_CRYPTO ********/
+#define RSS_CRYPTO_HANDLE				(0x40000100U)
+
 /******** RSS_SP_PLATFORM ********/
 #define RSS_PLATFORM_SERVICE_HANDLE			(0x40000105U)
 
diff --git a/include/lib/psa/rss_crypto_defs.h b/include/lib/psa/rss_crypto_defs.h
new file mode 100644
index 0000000..b8c7426
--- /dev/null
+++ b/include/lib/psa/rss_crypto_defs.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef RSS_CRYPTO_DEFS_H
+#define RSS_CRYPTO_DEFS_H
+
+/* Declares types that encode errors, algorithms, key types, policies, etc. */
+#include "psa/crypto_types.h"
+
+/*
+ * Value identifying export public key function API, used to dispatch the request
+ * to the corresponding API implementation in the Crypto service backend.
+ *
+ */
+#define RSS_CRYPTO_EXPORT_PUBLIC_KEY_SID	(uint16_t)(0x701)
+
+/*
+ * The persistent key identifiers for RSS builtin keys.
+ */
+enum rss_key_id_builtin_t {
+	RSS_BUILTIN_KEY_ID_HOST_S_ROTPK = 0x7FFF816Cu,
+	RSS_BUILTIN_KEY_ID_HOST_NS_ROTPK,
+	RSS_BUILTIN_KEY_ID_HOST_CCA_ROTPK,
+};
+
+/*
+ * This type is used to overcome a limitation within RSS firmware in the number of maximum
+ * IOVECs it can use especially in psa_aead_encrypt and psa_aead_decrypt.
+ */
+#define RSS_CRYPTO_MAX_NONCE_LENGTH (16u)
+struct rss_crypto_aead_pack_input {
+	uint8_t nonce[RSS_CRYPTO_MAX_NONCE_LENGTH];
+	uint32_t nonce_length;
+};
+
+/*
+ * Structure used to pack non-pointer types in a call
+ */
+struct rss_crypto_pack_iovec {
+	psa_key_id_t key_id;	/* Key id */
+	psa_algorithm_t alg;	/* Algorithm */
+	uint32_t op_handle;	/* Frontend context handle associated
+				   to a multipart operation */
+	uint32_t capacity;	/* Key derivation capacity */
+	uint32_t ad_length;	/* Additional Data length for multipart AEAD */
+	uint32_t plaintext_length;	/* Plaintext length for multipart AEAD */
+	struct rss_crypto_aead_pack_input aead_in;	/* Packs AEAD-related inputs */
+	uint16_t function_id;	/* Used to identify the function in the API dispatcher
+				   to the service backend. See rss_crypto_func_sid for
+				   detail */
+	uint16_t step;		/* Key derivation step */
+};
+
+#endif /* RSS_CRYPTO_DEFS_H */
diff --git a/include/lib/psa/rss_platform_api.h b/include/lib/psa/rss_platform_api.h
index 1dd7d05..8f74a51 100644
--- a/include/lib/psa/rss_platform_api.h
+++ b/include/lib/psa/rss_platform_api.h
@@ -11,6 +11,7 @@
 #include <stdint.h>
 
 #include "psa/error.h"
+#include <rss_crypto_defs.h>
 
 #define RSS_PLATFORM_API_ID_NV_READ       (1010)
 #define RSS_PLATFORM_API_ID_NV_INCREMENT  (1011)
@@ -41,4 +42,19 @@
 rss_platform_nv_counter_read(uint32_t counter_id,
 		uint32_t size, uint8_t *val);
 
+/*
+ * Reads the public key or the public part of a key pair in binary format.
+ *
+ * key		Identifier of the key to export.
+ * data		Buffer where the key data is to be written.
+ * data_size	Size of the data buffer in bytes.
+ * data_length	On success, the number of bytes that make up the key data.
+ *
+ * PSA_SUCCESS if the value is read correctly. Otherwise,
+ *	it returns a PSA_ERROR.
+ */
+psa_status_t
+rss_platform_key_read(enum rss_key_id_builtin_t key, uint8_t *data,
+		size_t data_size, size_t *data_length);
+
 #endif /* RSS_PLATFORM_API_H */
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index 16be971..bf1f93a 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -539,7 +539,8 @@
  * Define limit of firmware configuration memory:
  * ARM_FW_CONFIG + ARM_BL2_MEM_DESC memory
  */
-#define ARM_FW_CONFIGS_LIMIT		(ARM_BL_RAM_BASE + (PAGE_SIZE * 2))
+#define ARM_FW_CONFIGS_SIZE		(PAGE_SIZE * 2)
+#define ARM_FW_CONFIGS_LIMIT		(ARM_BL_RAM_BASE + ARM_FW_CONFIGS_SIZE)
 
 #if ENABLE_RME
 /*
@@ -775,7 +776,9 @@
 #define PLAT_PERCPU_BAKERY_LOCK_SIZE		(1 * CACHE_WRITEBACK_GRANULE)
 
 /* Priority levels for ARM platforms */
+#if RAS_FFH_SUPPORT
 #define PLAT_RAS_PRI			0x10
+#endif
 #define PLAT_SDEI_CRITICAL_PRI		0x60
 #define PLAT_SDEI_NORMAL_PRI		0x70
 
diff --git a/lib/cpus/aarch32/cortex_a17.S b/lib/cpus/aarch32/cortex_a17.S
index b8abd33..8d76ab2 100644
--- a/lib/cpus/aarch32/cortex_a17.S
+++ b/lib/cpus/aarch32/cortex_a17.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -63,6 +63,8 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_852421
 
+add_erratum_entry cortex_a17, ERRATUM(852421), ERRATA_A17_852421
+
 	/* ----------------------------------------------------
 	 * Errata Workaround for Cortex A17 Errata #852423.
 	 * This applies only to revision <= r1p2 of Cortex A17.
@@ -91,6 +93,8 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_852423
 
+add_erratum_entry cortex_a17, ERRATUM(852423), ERRATA_A17_852423
+
 func check_errata_cve_2017_5715
 #if WORKAROUND_CVE_2017_5715
 	mov	r0, #ERRATA_APPLIES
@@ -100,28 +104,9 @@
 	bx	lr
 endfunc check_errata_cve_2017_5715
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Cortex A17. Must follow AAPCS.
- */
-func cortex_a17_errata_report
-	push	{r12, lr}
-
-	bl	cpu_get_rev_var
-	mov	r4, r0
+add_erratum_entry cortex_a17, CVE(2017, 5715), WORKAROUND_CVE_2017_5715
 
-	/*
-	 * Report all errata. The revision-variant information is passed to
-	 * checking functions of each errata.
-	 */
-	report_errata ERRATA_A17_852421, cortex_a17, 852421
-	report_errata ERRATA_A17_852423, cortex_a17, 852423
-	report_errata WORKAROUND_CVE_2017_5715, cortex_a17, cve_2017_5715
-
-	pop	{r12, lr}
-	bx	lr
-endfunc cortex_a17_errata_report
-#endif
+errata_report_shim cortex_a17
 
 func cortex_a17_reset_func
 	mov	r5, lr
@@ -139,7 +124,7 @@
 #endif
 
 #if IMAGE_BL32 && WORKAROUND_CVE_2017_5715
-	ldr	r0, =workaround_bpiall_runtime_exceptions
+	ldr	r0, =wa_cve_2017_5715_bpiall_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_a32.S b/lib/cpus/aarch32/cortex_a32.S
index c262276..dfa159f 100644
--- a/lib/cpus/aarch32/cortex_a32.S
+++ b/lib/cpus/aarch32/cortex_a32.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -117,14 +117,7 @@
 	b	cortex_a32_disable_smp
 endfunc cortex_a32_cluster_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Cortex-A32. Must follow AAPCS.
- */
-func cortex_a32_errata_report
-	bx	lr
-endfunc cortex_a32_errata_report
-#endif
+errata_report_shim cortex_a32
 
 declare_cpu_ops cortex_a32, CORTEX_A32_MIDR, \
 	cortex_a32_reset_func, \
diff --git a/lib/cpus/aarch32/cortex_a53.S b/lib/cpus/aarch32/cortex_a53.S
index 6e3ff81..89b238a 100644
--- a/lib/cpus/aarch32/cortex_a53.S
+++ b/lib/cpus/aarch32/cortex_a53.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -44,6 +44,8 @@
 	bx	lr
 endfunc check_errata_819472
 
+add_erratum_entry cortex_a53, ERRATUM(819472), ERRATA_A53_819472
+
 	/* ---------------------------------------------------
 	 * Errata Workaround for Cortex A53 Errata #824069.
 	 * This applies only to revision <= r0p2 of Cortex A53.
@@ -59,6 +61,8 @@
 	bx	lr
 endfunc check_errata_824069
 
+add_erratum_entry cortex_a53, ERRATUM(824069), ERRATA_A53_824069
+
 	/* --------------------------------------------------
 	 * Errata Workaround for Cortex A53 Errata #826319.
 	 * This applies only to revision <= r0p2 of Cortex A53.
@@ -89,6 +93,8 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_826319
 
+add_erratum_entry cortex_a53, ERRATUM(826319), ERRATA_A53_826319
+
 	/* ---------------------------------------------------
 	 * Errata Workaround for Cortex A53 Errata #827319.
 	 * This applies only to revision <= r0p2 of Cortex A53.
@@ -104,6 +110,8 @@
 	bx	lr
 endfunc check_errata_827319
 
+add_erratum_entry cortex_a53, ERRATUM(827319), ERRATA_A53_827319
+
 	/* ---------------------------------------------------------------------
 	 * Disable the cache non-temporal hint.
 	 *
@@ -142,6 +150,9 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_disable_non_temporal_hint
 
+add_erratum_entry cortex_a53, ERRATUM(836870), ERRATA_A53_836870 | A53_DISABLE_NON_TEMPORAL_HINT, \
+	disable_non_temporal_hint
+
 	/* --------------------------------------------------
 	 * Errata Workaround for Cortex A53 Errata #855873.
 	 *
@@ -176,6 +187,8 @@
 	b	cpu_rev_var_hs
 endfunc check_errata_855873
 
+add_erratum_entry cortex_a53, ERRATUM(855873), ERRATA_A53_855873
+
 	/* -------------------------------------------------
 	 * The CPU Ops reset function for Cortex-A53.
 	 * Shall clobber: r0-r6
@@ -284,31 +297,7 @@
 	b	cortex_a53_disable_smp
 endfunc cortex_a53_cluster_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Cortex A53. Must follow AAPCS.
- */
-func cortex_a53_errata_report
-	push	{r12, lr}
-
-	bl	cpu_get_rev_var
-	mov	r4, r0
-
-	/*
-	 * Report all errata. The revision-variant information is passed to
-	 * checking functions of each errata.
-	 */
-	report_errata ERRATA_A53_819472, cortex_a53, 819472
-	report_errata ERRATA_A53_824069, cortex_a53, 824069
-	report_errata ERRATA_A53_826319, cortex_a53, 826319
-	report_errata ERRATA_A53_827319, cortex_a53, 827319
-	report_errata ERRATA_A53_836870, cortex_a53, disable_non_temporal_hint
-	report_errata ERRATA_A53_855873, cortex_a53, 855873
-
-	pop	{r12, lr}
-	bx	lr
-endfunc cortex_a53_errata_report
-#endif
+errata_report_shim cortex_a53
 
 declare_cpu_ops cortex_a53, CORTEX_A53_MIDR, \
 	cortex_a53_reset_func, \
diff --git a/lib/cpus/aarch32/cortex_a72.S b/lib/cpus/aarch32/cortex_a72.S
index 03914b2..77cf84d 100644
--- a/lib/cpus/aarch32/cortex_a72.S
+++ b/lib/cpus/aarch32/cortex_a72.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -87,11 +87,15 @@
 	b		cpu_rev_var_ls
 endfunc check_errata_859971
 
+add_erratum_entry cortex_a72, ERRATUM(859971), ERRATA_A72_859971
+
 func check_errata_cve_2017_5715
 	mov	r0, #ERRATA_MISSING
 	bx	lr
 endfunc check_errata_cve_2017_5715
 
+add_erratum_entry cortex_a72, CVE(2017, 5715), WORKAROUND_CVE_2017_5715
+
 func check_errata_cve_2018_3639
 #if WORKAROUND_CVE_2018_3639
 	mov	r0, #ERRATA_APPLIES
@@ -101,11 +105,15 @@
 	bx	lr
 endfunc check_errata_cve_2018_3639
 
+add_erratum_entry cortex_a72, CVE(2018, 3639), WORKAROUND_CVE_2018_3639
+
 func check_errata_cve_2022_23960
 	mov	r0, #ERRATA_MISSING
 	bx	lr
 endfunc check_errata_cve_2022_23960
 
+add_erratum_entry cortex_a72, CVE(2022, 23960), WORKAROUND_CVE_2022_23960
+
 	/* -------------------------------------------------
 	 * The CPU Ops reset function for Cortex-A72.
 	 * -------------------------------------------------
@@ -248,29 +256,7 @@
 	b	cortex_a72_disable_ext_debug
 endfunc cortex_a72_cluster_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Cortex A72. Must follow AAPCS.
- */
-func cortex_a72_errata_report
-	push	{r12, lr}
-
-	bl	cpu_get_rev_var
-	mov	r4, r0
-
-	/*
-	 * Report all errata. The revision-variant information is passed to
-	 * checking functions of each errata.
-	 */
-	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
-endfunc cortex_a72_errata_report
-#endif
+errata_report_shim cortex_a72
 
 declare_cpu_ops cortex_a72, CORTEX_A72_MIDR, \
 	cortex_a72_reset_func, \
diff --git a/lib/cpus/aarch32/cortex_a9.S b/lib/cpus/aarch32/cortex_a9.S
index 7200343..762f75f 100644
--- a/lib/cpus/aarch32/cortex_a9.S
+++ b/lib/cpus/aarch32/cortex_a9.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -42,7 +42,7 @@
 	mov	r0, #ERRATA_MISSING
 #endif
 	bx	lr
-endfunc check_errata_cve_2017_5715
+endfunc check_errata_a9_794073
 
 func check_errata_cve_2017_5715
 #if WORKAROUND_CVE_2017_5715
@@ -77,7 +77,7 @@
 
 func cortex_a9_reset_func
 #if IMAGE_BL32 && WORKAROUND_CVE_2017_5715
-	ldr	r0, =workaround_bpiall_runtime_exceptions
+	ldr	r0, =wa_cve_2017_5715_bpiall_vbar
 	stcopr	r0, VBAR
 	stcopr	r0, MVBAR
 	/* isb will be applied in the course of the reset func */
diff --git a/lib/cpus/aarch64/cortex_a510.S b/lib/cpus/aarch64/cortex_a510.S
index e10ebb0..6fce24e 100644
--- a/lib/cpus/aarch64/cortex_a510.S
+++ b/lib/cpus/aarch64/cortex_a510.S
@@ -21,110 +21,15 @@
 #error "Cortex-A510 supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #1922240.
-	 * This applies only to revision r0p0 (fixed in r0p1)
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_cortex_a510_1922240_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_1922240
-	cbz	x0, 1f
-
+workaround_reset_start cortex_a510, ERRATUM(1922240), ERRATA_A510_1922240
 	/* Apply the workaround by setting IMP_CMPXACTLR_EL1[11:10] = 0b11. */
-	mrs	x0, CORTEX_A510_CMPXACTLR_EL1
-	mov	x1, #3
-	bfi	x0, x1, #10, #2
-	msr	CORTEX_A510_CMPXACTLR_EL1, x0
-
-1:
-	ret	x17
-endfunc errata_cortex_a510_1922240_wa
-
-func check_errata_1922240
-	/* Applies to r0p0 only */
-	mov	x1, #0x00
-	b	cpu_rev_var_ls
-endfunc check_errata_1922240
-
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #2288014.
-	 * This applies only to revisions r0p0, r0p1, r0p2,
-	 * r0p3 and r1p0. (fixed in r1p1)
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_cortex_a510_2288014_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2288014
-	cbz	x0, 1f
-
-	/* Apply the workaround by setting IMP_CPUACTLR_EL1[18] = 0b1. */
-	mrs	x0, CORTEX_A510_CPUACTLR_EL1
-	mov	x1, #1
-	bfi	x0, x1, #18, #1
-	msr	CORTEX_A510_CPUACTLR_EL1, x0
-
-1:
-	ret	x17
-endfunc errata_cortex_a510_2288014_wa
-
-func check_errata_2288014
-	/* Applies to r1p0 and below */
-	mov	x1, #0x10
-	b	cpu_rev_var_ls
-endfunc check_errata_2288014
-
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #2042739.
-	 * This applies only to revisions r0p0, r0p1 and r0p2.
-	 * (fixed in r0p3)
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_cortex_a510_2042739_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2042739
-	cbz	x0, 1f
-
-	/* Apply the workaround by disabling ReadPreferUnique. */
-	mrs	x0, CORTEX_A510_CPUECTLR_EL1
-	mov	x1, #CORTEX_A510_CPUECTLR_EL1_READPREFERUNIQUE_DISABLE
-	bfi	x0, x1, #CORTEX_A510_CPUECTLR_EL1_READPREFERUNIQUE_SHIFT, #1
-	msr	CORTEX_A510_CPUECTLR_EL1, x0
-
-1:
-	ret	x17
-endfunc errata_cortex_a510_2042739_wa
+	sysreg_bitfield_insert CORTEX_A510_CMPXACTLR_EL1, CORTEX_A510_CMPXACTLR_EL1_SNPPREFERUNIQUE_DISABLE, \
+	CORTEX_A510_CMPXACTLR_EL1_SNPPREFERUNIQUE_SHIFT, CORTEX_A510_CMPXACTLR_EL1_SNPPREFERUNIQUE_WIDTH
+workaround_reset_end cortex_a510, ERRATUM(1922240)
 
-func check_errata_2042739
-	/* Applies to revisions r0p0 - r0p2 */
-	mov	x1, #0x02
-	b	cpu_rev_var_ls
-endfunc check_errata_2042739
+check_erratum_ls cortex_a510, ERRATUM(1922240), CPU_REV(0, 0)
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #2041909.
-	 * This applies only to revision r0p2 and it is fixed in
-	 * r0p3. The issue is also present in r0p0 and r0p1 but
-	 * there is no workaround in those revisions.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x2, x17
-	 * --------------------------------------------------
-	 */
-func errata_cortex_a510_2041909_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2041909
-	cbz	x0, 1f
-
+workaround_reset_start cortex_a510, ERRATUM(2041909), ERRATA_A510_2041909
 	/* Apply workaround */
 	mov	x0, xzr
 	msr	S3_6_C15_C4_0, x0
@@ -140,39 +45,55 @@
 	mov	x0, #0x3F1
 	movk	x0, #0x110, lsl #16
 	msr	S3_6_C15_C4_1, x0
-	isb
+workaround_reset_end cortex_a510, ERRATUM(2041909)
+
+check_erratum_range cortex_a510, ERRATUM(2041909), CPU_REV(0, 2), CPU_REV(0, 2)
+
+workaround_reset_start cortex_a510, ERRATUM(2042739), ERRATA_A510_2042739
+	/* Apply the workaround by disabling ReadPreferUnique. */
+	sysreg_bitfield_insert CORTEX_A510_CPUECTLR_EL1, CORTEX_A510_CPUECTLR_EL1_READPREFERUNIQUE_DISABLE, \
+		CORTEX_A510_CPUECTLR_EL1_READPREFERUNIQUE_SHIFT, CORTEX_A510_CPUECTLR_EL1_READPREFERUNIQUE_WIDTH
+workaround_reset_end cortex_a510, ERRATUM(2042739)
 
-1:
-	ret	x17
-endfunc errata_cortex_a510_2041909_wa
+check_erratum_ls cortex_a510, ERRATUM(2042739), CPU_REV(0, 2)
 
-func check_errata_2041909
-	/* Applies only to revision r0p2 */
-	mov	x1, #0x02
-	mov	x2, #0x02
-	b	cpu_rev_var_range
-endfunc check_errata_2041909
+workaround_reset_start cortex_a510, ERRATUM(2172148), ERRATA_A510_2172148
+	/*
+	 * Force L2 allocation of transient lines by setting
+	 * CPUECTLR_EL1.RSCTL=0b01 and CPUECTLR_EL1.NTCTL=0b01.
+	 */
+	mrs	x0, CORTEX_A510_CPUECTLR_EL1
+	mov	x1, #1
+	bfi	x0, x1, #CORTEX_A510_CPUECTLR_EL1_RSCTL_SHIFT, #2
+	bfi	x0, x1, #CORTEX_A510_CPUECTLR_EL1_NTCTL_SHIFT, #2
+	msr	CORTEX_A510_CPUECTLR_EL1, x0
+workaround_reset_end cortex_a510, ERRATUM(2172148)
+
+check_erratum_ls cortex_a510, ERRATUM(2172148), CPU_REV(1, 0)
+
+workaround_reset_start cortex_a510, ERRATUM(2218950), ERRATA_A510_2218950
+	/* Set bit 18 in CPUACTLR_EL1 */
+	sysreg_bitfield_insert CORTEX_A510_CPUACTLR_EL1, CORTEX_A510_CPUACTLR_EL1_ALIAS_LOADSTORE_DISABLE, \
+	CORTEX_A510_CPUACTLR_EL1_ALIAS_LOADSTORE_SHIFT, CORTEX_A510_CPUACTLR_EL1_ALIAS_LOADSTORE_WIDTH
+
+	/* Set bit 25 in CMPXACTLR_EL1 */
+	sysreg_bitfield_insert CORTEX_A510_CMPXACTLR_EL1, CORTEX_A510_CMPXACTLR_EL1_ALIAS_LOADSTORE_DISABLE, \
+	CORTEX_A510_CMPXACTLR_EL1_ALIAS_LOADSTORE_SHIFT, CORTEX_A510_CMPXACTLR_EL1_ALIAS_LOADSTORE_WIDTH
+
+workaround_reset_end cortex_a510, ERRATUM(2218950)
+
+check_erratum_ls cortex_a510, ERRATUM(2218950), CPU_REV(1, 0)
 
 	/* --------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #2250311.
-	 * This applies only to revisions r0p0, r0p1, r0p2,
-	 * r0p3 and r1p0, and is fixed in r1p1.
 	 * This workaround is not a typical errata fix. MPMM
 	 * is disabled here, but this conflicts with the BL31
 	 * MPMM support. So in addition to simply disabling
 	 * the feature, a flag is set in the MPMM library
 	 * indicating that it should not be enabled even if
 	 * ENABLE_MPMM=1.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
 	 * --------------------------------------------------
 	 */
-func errata_cortex_a510_2250311_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2250311
-	cbz	x0, 1f
-
+workaround_reset_start cortex_a510, ERRATUM(2250311), ERRATA_A510_2250311
 	/* Disable MPMM */
 	mrs	x0, CPUMPMMCR_EL3
 	bfm	x0, xzr, #0, #0 /* bfc instruction does not work in GCC */
@@ -182,227 +103,68 @@
 	/* If ENABLE_MPMM is set, tell the runtime lib to skip enabling it. */
 	bl mpmm_errata_disable
 #endif
-
-1:
-	ret x17
-endfunc errata_cortex_a510_2250311_wa
-
-func check_errata_2250311
-	/* Applies to r1p0 and lower */
-	mov	x1, #0x10
-	b	cpu_rev_var_ls
-endfunc check_errata_2250311
-
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #2218950.
-	 * This applies only to revisions r0p0, r0p1, r0p2,
-	 * r0p3 and r1p0, and is fixed in r1p1.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_cortex_a510_2218950_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2218950
-	cbz	x0, 1f
-
-	/* Source register for BFI */
-	mov	x1, #1
-
-	/* Set bit 18 in CPUACTLR_EL1 */
-	mrs	x0, CORTEX_A510_CPUACTLR_EL1
-	bfi	x0, x1, #18, #1
-	msr	CORTEX_A510_CPUACTLR_EL1, x0
-
-	/* Set bit 25 in CMPXACTLR_EL1 */
-	mrs	x0, CORTEX_A510_CMPXACTLR_EL1
-	bfi	x0, x1, #25, #1
-	msr	CORTEX_A510_CMPXACTLR_EL1, x0
-
-1:
-	ret x17
-endfunc errata_cortex_a510_2218950_wa
+workaround_reset_end cortex_a510, ERRATUM(2250311)
 
-func check_errata_2218950
-	/* Applies to r1p0 and lower */
-	mov	x1, #0x10
-	b	cpu_rev_var_ls
-endfunc check_errata_2218950
+check_erratum_ls cortex_a510, ERRATUM(2250311), CPU_REV(1, 0)
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #2172148.
-	 * This applies only to revisions r0p0, r0p1, r0p2,
-	 * r0p3 and r1p0, and is fixed in r1p1.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_cortex_a510_2172148_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2172148
-	cbz	x0, 1f
-
-	/*
-	 * Force L2 allocation of transient lines by setting
-	 * CPUECTLR_EL1.RSCTL=0b01 and CPUECTLR_EL1.NTCTL=0b01.
-	 */
-	mrs	x0, CORTEX_A510_CPUECTLR_EL1
-	mov	x1, #1
-	bfi	x0, x1, #CORTEX_A510_CPUECTLR_EL1_RSCTL_SHIFT, #2
-	bfi	x0, x1, #CORTEX_A510_CPUECTLR_EL1_NTCTL_SHIFT, #2
-	msr	CORTEX_A510_CPUECTLR_EL1, x0
-
-1:
-	ret x17
-endfunc errata_cortex_a510_2172148_wa
-
-func check_errata_2172148
-	/* Applies to r1p0 and lower */
-	mov	x1, #0x10
-	b	cpu_rev_var_ls
-endfunc check_errata_2172148
+workaround_reset_start cortex_a510, ERRATUM(2288014), ERRATA_A510_2288014
+	/* Apply the workaround by setting IMP_CPUACTLR_EL1[18] = 0b1. */
+	sysreg_bitfield_insert CORTEX_A510_CPUACTLR_EL1, CORTEX_A510_CPUACTLR_EL1_DATA_CORRUPT_DISABLE, \
+	CORTEX_A510_CPUACTLR_EL1_DATA_CORRUPT_SHIFT, CORTEX_A510_CPUACTLR_EL1_DATA_CORRUPT_WIDTH
+workaround_reset_end cortex_a510, ERRATUM(2288014)
 
-	/* ----------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #2347730.
-	 * This applies to revisions r0p0 - r0p3, r1p0, r1p1.
-	 * It is fixed in r1p2.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0-x1, x17
-	 * ----------------------------------------------------
-	 */
-func errata_cortex_a510_2347730_wa
-	mov	x17, x30
-	bl	check_errata_2347730
-	cbz	x0, 1f
+check_erratum_ls cortex_a510, ERRATUM(2288014), CPU_REV(1, 0)
 
+workaround_reset_start cortex_a510, ERRATUM(2347730), ERRATA_A510_2347730
 	/*
 	 * Set CPUACTLR_EL1[17] to 1'b1, which disables
 	 * specific microarchitectural clock gating
 	 * behaviour.
 	 */
-	mrs	x1, CORTEX_A510_CPUACTLR_EL1
-	orr	x1, x1, CORTEX_A510_CPUACTLR_EL1_BIT_17
-	msr	CORTEX_A510_CPUACTLR_EL1, x1
-1:
-	ret x17
-endfunc errata_cortex_a510_2347730_wa
-
-func check_errata_2347730
-	/* Applies to revisions r1p1 and lower. */
-	mov	x1, #0x11
-	b	cpu_rev_var_ls
-endfunc check_errata_2347730
+	sysreg_bit_set CORTEX_A510_CPUACTLR_EL1, CORTEX_A510_CPUACTLR_EL1_BIT_17
+workaround_reset_end cortex_a510, ERRATUM(2347730)
 
-	/*---------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #2371937.
-	 * This applies to revisions r1p1 and lower, and is
-	 * fixed in r1p2.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 *---------------------------------------------------
-	 */
-func errata_cortex_a510_2371937_wa
-	mov	x17, x30
-	bl	check_errata_2371937
-	cbz	x0, 1f
+check_erratum_ls cortex_a510, ERRATUM(2347730), CPU_REV(1, 1)
 
+workaround_reset_start cortex_a510, ERRATUM(2371937), ERRATA_A510_2371937
 	/*
 	 * Cacheable atomic operations can be forced
 	 * to be executed near by setting
 	 * IMP_CPUECTLR_EL1.ATOM=0b010. ATOM is found
 	 * in [40:38] of CPUECTLR_EL1.
 	 */
-	mrs 	x0, CORTEX_A510_CPUECTLR_EL1
-	mov 	x1, CORTEX_A510_CPUECTLR_EL1_ATOM_EXECALLINSTRNEAR
-	bfi 	x0, x1, CORTEX_A510_CPUECTLR_EL1_ATOM, #3
-	msr 	CORTEX_A510_CPUECTLR_EL1, x0
-1:
-	ret 	x17
-endfunc errata_cortex_a510_2371937_wa
-
-func check_errata_2371937
-	/* Applies to r1p1 and lower */
-	mov 	x1, #0x11
-	b	cpu_rev_var_ls
-endfunc check_errata_2371937
-
-	/* ------------------------------------------------------
-	 * Errata Workaround for Cortex-A510 Errata #2666669
-	 * This applies to revisions r1p1 and lower, and is fixed
-	 * in r1p2.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * ------------------------------------------------------
-	 */
-func errata_cortex_a510_2666669_wa
-	mov	x17, x30
-	bl	check_errata_2666669
-	cbz	x0, 1f
-
-	/*
-	 * Workaround will set IMP_CPUACTLR_EL1[38]
-	 * to 0b1.
-	 */
-	mrs	x1, CORTEX_A510_CPUACTLR_EL1
-	orr	x1, x1, CORTEX_A510_CPUACTLR_EL1_BIT_38
-	msr	CORTEX_A510_CPUACTLR_EL1, x1
-1:
-	ret	x17
-endfunc errata_cortex_a510_2666669_wa
+	sysreg_bitfield_insert CORTEX_A510_CPUECTLR_EL1, CORTEX_A510_CPUECTLR_EL1_ATOM_EXECALLINSTRNEAR, \
+		CORTEX_A510_CPUECTLR_EL1_ATOM_SHIFT, CORTEX_A510_CPUECTLR_EL1_ATOM_WIDTH
+workaround_reset_end cortex_a510, ERRATUM(2371937)
 
-func check_errata_2666669
-	/* Applies to r1p1 and lower */
-	mov	x1, #0x11
-	b	cpu_rev_var_ls
-endfunc check_errata_2666669
+check_erratum_ls cortex_a510, ERRATUM(2371937), CPU_REV(1, 1)
 
-/* ------------------------------------------------------
- * Errata Workaround for Cortex-A510 Erratum 2684597.
- * This erratum applies to revision r0p0, r0p1, r0p2,
- * r0p3, r1p0, r1p1 and r1p2 of the Cortex-A510 cpu and
- * is fixed in r1p3.
- * Shall clobber: x0-x17
- * ------------------------------------------------------
- */
-	.globl	errata_cortex_a510_2684597_wa
-func errata_cortex_a510_2684597_wa
-	mov	x17, x30
-	/* Ensure this errata is only applied to Cortex-A510 cores */
-	jump_if_cpu_midr	CORTEX_A510_MIDR,	1f
-	b	2f
+workaround_reset_start cortex_a510, ERRATUM(2666669), ERRATA_A510_2666669
+	sysreg_bit_set CORTEX_A510_CPUACTLR_EL1, CORTEX_A510_CPUACTLR_EL1_BIT_38
+workaround_reset_end cortex_a510, ERRATUM(2666669)
 
-1:
-	/* Check workaround compatibility. */
-	mov	x0, x18
-	bl	check_errata_2684597
-	cbz	x0, 2f
+check_erratum_ls cortex_a510, ERRATUM(2666669), CPU_REV(1, 1)
 
+.global erratum_cortex_a510_2684597_wa
+workaround_runtime_start cortex_a510, ERRATUM(2684597), ERRATA_A510_2684597, CORTEX_A510_MIDR
 	/*
 	 * Many assemblers do not yet understand the "tsb csync" mnemonic,
 	 * so use the equivalent hint instruction.
 	 */
 	hint	#18			/* tsb csync */
-2:
-	ret	x17
-endfunc errata_cortex_a510_2684597_wa
-/* ------------------------------------------------------
- * Errata Workaround for Cortex-A510 Erratum 2684597.
- * This erratum applies to revision r0p0, r0p1, r0p2,
- * r0p3, r1p0, r1p1 and r1p2 of the Cortex-A510 cpu and
- * is fixed in r1p3.
- * Shall clobber: x0-x17
- * ------------------------------------------------------
+workaround_runtime_end cortex_a510, ERRATUM(2684597)
+
+check_erratum_ls cortex_a510, ERRATUM(2684597), CPU_REV(1, 2)
+
+/*
+ * ERRATA_DSU_2313941 :
+ * The errata is defined in dsu_helpers.S but applies to cortex_a510
+ * as well. Henceforth creating symbolic names to the already existing errata
+ * workaround functions to get them registered under the Errata Framework.
  */
-func check_errata_2684597
-	/* Applies to revision < r1p3 */
-	mov	x1, #0x12
-	b	cpu_rev_var_ls
-endfunc check_errata_2684597
+.equ check_erratum_cortex_a510_2313941, check_errata_dsu_2313941
+.equ erratum_cortex_a510_2313941_wa, errata_dsu_2313941_wa
+add_erratum_entry cortex_a510, ERRATUM(2313941), ERRATA_DSU_2313941, APPLY_AT_RESET
 
 	/* ----------------------------------------------------
 	 * HW will do the cache maintenance while powering down
@@ -413,112 +175,17 @@
 	 * Enable CPU power down bit in power control register
 	 * ---------------------------------------------------
 	 */
-	mrs	x0, CORTEX_A510_CPUPWRCTLR_EL1
-	orr	x0, x0, #CORTEX_A510_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
-	msr	CORTEX_A510_CPUPWRCTLR_EL1, x0
+	sysreg_bit_set CORTEX_A510_CPUPWRCTLR_EL1, CORTEX_A510_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
 	isb
 	ret
 endfunc cortex_a510_core_pwr_dwn
 
-	/*
-	 * Errata printing function for Cortex-A510. Must follow AAPCS.
-	 */
-#if REPORT_ERRATA
-func cortex_a510_errata_report
-	stp	x8, x30, [sp, #-16]!
+errata_report_shim cortex_a510
 
-	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_A510_1922240, cortex_a510, 1922240
-	report_errata ERRATA_A510_2041909, cortex_a510, 2041909
-	report_errata ERRATA_A510_2042739, cortex_a510, 2042739
-	report_errata ERRATA_A510_2172148, cortex_a510, 2172148
-	report_errata ERRATA_A510_2218950, cortex_a510, 2218950
-	report_errata ERRATA_A510_2250311, cortex_a510, 2250311
-	report_errata ERRATA_A510_2288014, cortex_a510, 2288014
-	report_errata ERRATA_A510_2347730, cortex_a510, 2347730
-	report_errata ERRATA_A510_2371937, cortex_a510, 2371937
-	report_errata ERRATA_A510_2666669, cortex_a510, 2666669
-	report_errata ERRATA_A510_2684597, cortex_a510, 2684597
-	report_errata ERRATA_DSU_2313941, cortex_a510, dsu_2313941
-
-	ldp	x8, x30, [sp], #16
-	ret
-endfunc cortex_a510_errata_report
-#endif
-
-func cortex_a510_reset_func
-	mov	x19, x30
-
+cpu_reset_func_start cortex_a510
 	/* Disable speculative loads */
 	msr	SSBS, xzr
-
-	/* 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
-#endif
-
-#if ERRATA_A510_2288014
-	mov	x0, x18
-	bl	errata_cortex_a510_2288014_wa
-#endif
-
-#if ERRATA_A510_2042739
-	mov	x0, x18
-	bl	errata_cortex_a510_2042739_wa
-#endif
-
-#if ERRATA_A510_2041909
-	mov	x0, x18
-	bl	errata_cortex_a510_2041909_wa
-#endif
-
-#if ERRATA_A510_2250311
-	mov	x0, x18
-	bl	errata_cortex_a510_2250311_wa
-#endif
-
-#if ERRATA_A510_2218950
-	mov	x0, x18
-	bl	errata_cortex_a510_2218950_wa
-#endif
-
-#if ERRATA_A510_2371937
-	mov 	x0, x18
-	bl	errata_cortex_a510_2371937_wa
-#endif
-
-#if ERRATA_A510_2172148
-	mov	x0, x18
-	bl	errata_cortex_a510_2172148_wa
-#endif
-
-#if ERRATA_A510_2347730
-	mov	x0, x18
-	bl	errata_cortex_a510_2347730_wa
-#endif
-
-#if ERRATA_A510_2666669
-	mov	x0, x18
-	bl	errata_cortex_a510_2666669_wa
-#endif
-
-	isb
-	ret	x19
-endfunc cortex_a510_reset_func
+cpu_reset_func_end cortex_a510
 
 	/* ---------------------------------------------
 	 * This function provides Cortex-A510 specific
diff --git a/lib/cpus/aarch64/cortex_a520.S b/lib/cpus/aarch64/cortex_a520.S
new file mode 100644
index 0000000..5bbe862
--- /dev/null
+++ b/lib/cpus/aarch64/cortex_a520.S
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <cortex_a520.h>
+#include <cpu_macros.S>
+#include <plat_macros.S>
+
+/* Hardware handled coherency */
+#if HW_ASSISTED_COHERENCY == 0
+#error "Cortex A520 must be compiled with HW_ASSISTED_COHERENCY enabled"
+#endif
+
+/* 64-bit only core */
+#if CTX_INCLUDE_AARCH32_REGS == 1
+#error "Cortex A520 supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
+#endif
+
+	/* ----------------------------------------------------
+	 * HW will do the cache maintenance while powering down
+	 * ----------------------------------------------------
+	 */
+func cortex_a520_core_pwr_dwn
+	/* ---------------------------------------------------
+	 * Enable CPU power down bit in power control register
+	 * ---------------------------------------------------
+	 */
+	mrs	x0, CORTEX_A520_CPUPWRCTLR_EL1
+	orr	x0, x0, #CORTEX_A520_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
+	msr	CORTEX_A520_CPUPWRCTLR_EL1, x0
+	isb
+	ret
+endfunc cortex_a520_core_pwr_dwn
+
+	/*
+	 * Errata printing function for Cortex A520. Must follow AAPCS.
+	 */
+#if REPORT_ERRATA
+func cortex_a520_errata_report
+	ret
+endfunc cortex_a520_errata_report
+#endif
+
+func cortex_a520_reset_func
+	/* Disable speculative loads */
+	msr	SSBS, xzr
+	isb
+	ret
+endfunc cortex_a520_reset_func
+
+	/* ---------------------------------------------
+	 * This function provides Cortex A520 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_a520_regs, "aS"
+cortex_a520_regs:  /* The ascii list of register names to be reported */
+	.asciz	"cpuectlr_el1", ""
+
+func cortex_a520_cpu_reg_dump
+	adr	x6, cortex_a520_regs
+	mrs	x8, CORTEX_A520_CPUECTLR_EL1
+	ret
+endfunc cortex_a520_cpu_reg_dump
+
+declare_cpu_ops cortex_a520, CORTEX_A520_MIDR, \
+	cortex_a520_reset_func, \
+	cortex_a520_core_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_a65ae.S b/lib/cpus/aarch64/cortex_a65ae.S
index ac6583e..85d1894 100644
--- a/lib/cpus/aarch64/cortex_a65ae.S
+++ b/lib/cpus/aarch64/cortex_a65ae.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -22,49 +22,26 @@
 #error "Cortex-A65AE supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
-/* -------------------------------------------------
- * The CPU Ops reset function for Cortex-A65.
- * Shall clobber: x0-x19
- * -------------------------------------------------
- */
-func cortex_a65ae_reset_func
-	mov	x19, x30
-
-#if ERRATA_DSU_936184
-	bl	errata_dsu_936184_wa
-#endif
+ /*
+  * ERRATA_DSU_936184 :
+  * The errata is defined in dsu_helpers.S but applies to cortex_a65ae
+  * as well. Henceforth creating symbolic names to the already existing errata
+  * workaround functions to get them registered under the Errata Framework.
+  */
+.equ check_erratum_cortex_a65ae_936184, check_errata_dsu_936184
+.equ erratum_cortex_a65ae_936184_wa, errata_dsu_936184_wa
+add_erratum_entry cortex_a65ae, ERRATUM(936184), ERRATA_DSU_936184, APPLY_AT_RESET
 
-	ret	x19
-endfunc cortex_a65ae_reset_func
+cpu_reset_func_start cortex_a65ae
+cpu_reset_func_end cortex_a65ae
 
 func cortex_a65ae_cpu_pwr_dwn
-	mrs	x0, CORTEX_A65AE_CPUPWRCTLR_EL1
-	orr	x0, x0, #CORTEX_A65AE_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
-	msr	CORTEX_A65AE_CPUPWRCTLR_EL1, x0
+	sysreg_bit_set CORTEX_A65AE_CPUPWRCTLR_EL1, CORTEX_A65AE_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
 	isb
 	ret
 endfunc cortex_a65ae_cpu_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Cortex-A65AE. Must follow AAPCS.
- */
-func cortex_a65ae_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_DSU_936184, cortex_a65ae, dsu_936184
-
-	ldp	x8, x30, [sp], #16
-	ret
-endfunc cortex_a65ae_errata_report
-#endif
+errata_report_shim cortex_a65ae
 
 .section .rodata.cortex_a65ae_regs, "aS"
 cortex_a65ae_regs:  /* The ascii list of register names to be reported */
diff --git a/lib/cpus/aarch64/cortex_a720.S b/lib/cpus/aarch64/cortex_a720.S
new file mode 100644
index 0000000..529ab50
--- /dev/null
+++ b/lib/cpus/aarch64/cortex_a720.S
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <cortex_a720.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
+#error "Cortex A720 must be compiled with HW_ASSISTED_COHERENCY enabled"
+#endif
+
+/* 64-bit only core */
+#if CTX_INCLUDE_AARCH32_REGS == 1
+#error "Cortex A720 supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
+#endif
+
+#if WORKAROUND_CVE_2022_23960
+        wa_cve_2022_23960_bhb_vector_table CORTEX_A720_BHB_LOOP_COUNT, cortex_a720
+#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_a720_reset_func
+	/* Disable speculative loads */
+	msr	SSBS, xzr
+
+#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
+	/*
+	 * The Cortex A720 generic vectors are overridden to apply errata
+	 * mitigation on exception entry from lower ELs.
+	 */
+	adr	x0, wa_cve_vbar_cortex_a720
+	msr	vbar_el3, x0
+#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
+
+	isb
+	ret
+endfunc cortex_a720_reset_func
+
+	/* ----------------------------------------------------
+	 * HW will do the cache maintenance while powering down
+	 * ----------------------------------------------------
+	 */
+func cortex_a720_core_pwr_dwn
+	/* ---------------------------------------------------
+	 * Enable CPU power down bit in power control register
+	 * ---------------------------------------------------
+	 */
+	mrs	x0, CORTEX_A720_CPUPWRCTLR_EL1
+	orr	x0, x0, #CORTEX_A720_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
+	msr	CORTEX_A720_CPUPWRCTLR_EL1, x0
+	isb
+	ret
+endfunc cortex_a720_core_pwr_dwn
+
+#if REPORT_ERRATA
+/*
+ * Errata printing function for Cortex A720. Must follow AAPCS.
+ */
+func cortex_a720_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_a720, cve_2022_23960
+
+	ldp	x8, x30, [sp], #16
+	ret
+endfunc cortex_a720_errata_report
+#endif
+
+	/* ---------------------------------------------
+	 * This function provides Cortex A720-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_a720_regs, "aS"
+cortex_a720_regs:  /* The ascii list of register names to be reported */
+	.asciz	"cpuectlr_el1", ""
+
+func cortex_a720_cpu_reg_dump
+	adr	x6, cortex_a720_regs
+	mrs	x8, CORTEX_A720_CPUECTLR_EL1
+	ret
+endfunc cortex_a720_cpu_reg_dump
+
+declare_cpu_ops cortex_a720, CORTEX_A720_MIDR, \
+	cortex_a720_reset_func, \
+	cortex_a720_core_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_a75.S b/lib/cpus/aarch64/cortex_a75.S
index e22c828..0a7b9fb 100644
--- a/lib/cpus/aarch64/cortex_a75.S
+++ b/lib/cpus/aarch64/cortex_a75.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,139 +15,43 @@
 #error "Cortex-A75 must be compiled with HW_ASSISTED_COHERENCY enabled"
 #endif
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex A75 Errata #764081.
-	 * This applies only to revision r0p0 of Cortex A75.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0-x17
-	 * --------------------------------------------------
-	 */
-func errata_a75_764081_wa
-	/*
-	 * Compare x0 against revision r0p0
-	 */
-	mov	x17, x30
-	bl	check_errata_764081
-	cbz	x0, 1f
-	mrs	x1, sctlr_el3
-	orr	x1, x1 ,#SCTLR_IESB_BIT
-	msr	sctlr_el3, x1
-	isb
-1:
-	ret	x17
-endfunc errata_a75_764081_wa
+workaround_reset_start cortex_a75, ERRATUM(764081), ERRATA_A75_764081
+	sysreg_bit_set sctlr_el3, SCTLR_IESB_BIT
+workaround_reset_end cortex_a75, ERRATUM(764081)
 
-func check_errata_764081
-	mov	x1, #0x00
-	b	cpu_rev_var_ls
-endfunc check_errata_764081
+check_erratum_ls cortex_a75, ERRATUM(764081), CPU_REV(0, 0)
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex A75 Errata #790748.
-	 * This applies only to revision r0p0 of Cortex A75.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0-x17
-	 * --------------------------------------------------
-	 */
-func errata_a75_790748_wa
-	/*
-	 * Compare x0 against revision r0p0
-	 */
-	mov	x17, x30
-	bl	check_errata_790748
-	cbz	x0, 1f
-	mrs	x1, CORTEX_A75_CPUACTLR_EL1
-	orr	x1, x1 ,#(1 << 13)
-	msr	CORTEX_A75_CPUACTLR_EL1, x1
-	isb
-1:
-	ret	x17
-endfunc errata_a75_790748_wa
+workaround_reset_start cortex_a75, ERRATUM(790748), ERRATA_A75_790748
+	sysreg_bit_set CORTEX_A75_CPUACTLR_EL1, (1 << 13)
+workaround_reset_end cortex_a75, ERRATUM(790748)
 
-func check_errata_790748
-	mov	x1, #0x00
-	b	cpu_rev_var_ls
-endfunc check_errata_790748
+check_erratum_ls cortex_a75, ERRATUM(790748), CPU_REV(0, 0)
 
-	/* -------------------------------------------------
-	 * The CPU Ops reset function for Cortex-A75.
-	 * -------------------------------------------------
-	 */
-func cortex_a75_reset_func
-	mov	x19, x30
-	bl	cpu_get_rev_var
-	mov	x18, x0
-
-#if ERRATA_A75_764081
-	mov	x0, x18
-	bl	errata_a75_764081_wa
-#endif
-
-#if ERRATA_A75_790748
-	mov	x0, x18
-	bl	errata_a75_790748_wa
-#endif
-
-#if IMAGE_BL31 && (WORKAROUND_CVE_2017_5715 || WORKAROUND_CVE_2022_23960)
-	cpu_check_csv2	x0, 1f
-	adr	x0, wa_cve_2017_5715_bpiall_vbar
-	msr	vbar_el3, x0
-	isb
-	/* Skip installing vector table again for CVE_2022_23960 */
-        b       2f
-1:
-#if WORKAROUND_CVE_2022_23960
-	adr	x0, wa_cve_2017_5715_bpiall_vbar
-	msr	vbar_el3, x0
-	isb
-#endif
-2:
-#endif /* IMAGE_BL31 &&  (WORKAROUND_CVE_2017_5715 || WORKAROUND_CVE_2022_23960) */
-
-#if WORKAROUND_CVE_2018_3639
-	mrs	x0, CORTEX_A75_CPUACTLR_EL1
-	orr	x0, x0, #CORTEX_A75_CPUACTLR_EL1_DISABLE_LOAD_PASS_STORE
-	msr	CORTEX_A75_CPUACTLR_EL1, x0
-	isb
-#endif
-
-#if ERRATA_DSU_798953
-	bl	errata_dsu_798953_wa
-#endif
-
-#if ERRATA_DSU_936184
-	bl	errata_dsu_936184_wa
-#endif
-
-#if ENABLE_FEAT_AMU
-	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
-	mrs	x0, actlr_el3
-	orr	x0, x0, #CORTEX_A75_ACTLR_AMEN_BIT
-	msr	actlr_el3, x0
-	isb
-
-	/* Make sure accesses from EL0/EL1 are not trapped to EL2 */
-	mrs	x0, actlr_el2
-	orr	x0, x0, #CORTEX_A75_ACTLR_AMEN_BIT
-	msr	actlr_el2, x0
-	isb
+/* ERRATA_DSU_798953 :
+ * The errata is defined in dsu_helpers.S but applies to cortex_a75
+ * as well. Henceforth creating symbolic names to the already existing errata
+ * workaround functions to get them registered under the Errata Framework.
+ */
+.equ check_erratum_cortex_a75_798953, check_errata_dsu_798953
+.equ erratum_cortex_a75_798953_wa, errata_dsu_798953_wa
+add_erratum_entry cortex_a75, ERRATUM(798953), ERRATA_DSU_798953, APPLY_AT_RESET
 
-	/* Enable group0 counters */
-	mov	x0, #CORTEX_A75_AMU_GROUP0_MASK
-	msr	CPUAMCNTENSET_EL0, x0
-	isb
+/* ERRATA_DSU_936184 :
+ * The errata is defined in dsu_helpers.S but applies to cortex_a75
+ * as well. Henceforth creating symbolic names to the already existing errata
+ * workaround functions to get them registered under the Errata Framework.
+ */
+.equ check_erratum_cortex_a75_936184, check_errata_dsu_936184
+.equ erratum_cortex_a75_936184_wa, errata_dsu_936184_wa
+add_erratum_entry cortex_a75, ERRATUM(936184), ERRATA_DSU_936184, APPLY_AT_RESET
 
-	/* Enable group1 counters */
-	mov	x0, #CORTEX_A75_AMU_GROUP1_MASK
-	msr	CPUAMCNTENSET_EL0, x0
-	isb
-#endif
-	ret	x19
-endfunc cortex_a75_reset_func
+workaround_reset_start cortex_a75, CVE(2017, 5715), WORKAROUND_CVE_2017_5715
+#if IMAGE_BL31
+	override_vector_table wa_cve_2017_5715_bpiall_vbar
+#endif /* IMAGE_BL31 */
+workaround_reset_end cortex_a75, CVE(2017, 5715)
 
-func check_errata_cve_2017_5715
+check_erratum_custom_start cortex_a75, CVE(2017, 5715)
 	cpu_check_csv2	x0, 1f
 #if WORKAROUND_CVE_2017_5715
 	mov	x0, #ERRATA_APPLIES
@@ -158,18 +62,27 @@
 1:
 	mov	x0, #ERRATA_NOT_APPLIES
 	ret
-endfunc check_errata_cve_2017_5715
+check_erratum_custom_end cortex_a75, CVE(2017, 5715)
 
-func check_errata_cve_2018_3639
-#if WORKAROUND_CVE_2018_3639
-	mov	x0, #ERRATA_APPLIES
-#else
-	mov	x0, #ERRATA_MISSING
-#endif
-	ret
-endfunc check_errata_cve_2018_3639
+workaround_reset_start cortex_a75, CVE(2018, 3639), WORKAROUND_CVE_2018_3639
+	sysreg_bit_set CORTEX_A75_CPUACTLR_EL1, CORTEX_A75_CPUACTLR_EL1_DISABLE_LOAD_PASS_STORE
+workaround_reset_end cortex_a75, CVE(2018, 3639)
 
-func check_errata_cve_2022_23960
+check_erratum_chosen cortex_a75, CVE(2018, 3639), WORKAROUND_CVE_2018_3639
+
+workaround_reset_start cortex_a75, CVE(2022, 23960), WORKAROUND_CVE_2022_23960
+#if IMAGE_BL31
+	/* Skip installing vector table again if already done for CVE(2017, 5715) */
+	adr	x0, wa_cve_2017_5715_bpiall_vbar
+	mrs	x1, vbar_el3
+	cmp	x0, x1
+	b.eq	1f
+	msr	vbar_el3, x0
+1:
+#endif /* IMAGE_BL31 */
+workaround_reset_end cortex_a75, CVE(2022, 23960)
+
+check_erratum_custom_start cortex_a75, CVE(2022, 23960)
 #if WORKAROUND_CVE_2017_5715 || WORKAROUND_CVE_2022_23960
 	cpu_check_csv2	x0, 1f
 	mov	x0, #ERRATA_APPLIES
@@ -184,7 +97,34 @@
 #endif /* WORKAROUND_CVE_2017_5715 || WORKAROUND_CVE_2022_23960 */
 	mov	x0, #ERRATA_MISSING
 	ret
-endfunc check_errata_cve_2022_23960
+check_erratum_custom_end cortex_a75, CVE(2022, 23960)
+
+	/* -------------------------------------------------
+	 * The CPU Ops reset function for Cortex-A75.
+	 * -------------------------------------------------
+	 */
+
+cpu_reset_func_start cortex_a75
+#if ENABLE_FEAT_AMU
+	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
+	sysreg_bit_set actlr_el3, CORTEX_A75_ACTLR_AMEN_BIT
+	isb
+
+	/* Make sure accesses from EL0/EL1 are not trapped to EL2 */
+	sysreg_bit_set actlr_el2, CORTEX_A75_ACTLR_AMEN_BIT
+	isb
+
+	/* Enable group0 counters */
+	mov	x0, #CORTEX_A75_AMU_GROUP0_MASK
+	msr	CPUAMCNTENSET_EL0, x0
+	isb
+
+	/* Enable group1 counters */
+	mov	x0, #CORTEX_A75_AMU_GROUP1_MASK
+	msr	CPUAMCNTENSET_EL0, x0
+	/* isb included in cpu_reset_func_end macro */
+#endif
+cpu_reset_func_end cortex_a75
 
 func check_smccc_arch_workaround_3
 	mov	x0, #ERRATA_APPLIES
@@ -200,39 +140,13 @@
 	 * Enable CPU power down bit in power control register
 	 * ---------------------------------------------
 	 */
-	mrs	x0, CORTEX_A75_CPUPWRCTLR_EL1
-	orr	x0, x0, #CORTEX_A75_CORE_PWRDN_EN_MASK
-	msr	CORTEX_A75_CPUPWRCTLR_EL1, x0
+	sysreg_bit_set CORTEX_A75_CPUPWRCTLR_EL1, \
+		CORTEX_A75_CORE_PWRDN_EN_MASK
 	isb
 	ret
 endfunc cortex_a75_core_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Cortex A75. Must follow AAPCS.
- */
-func cortex_a75_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_A75_764081, cortex_a75, 764081
-	report_errata ERRATA_A75_790748, cortex_a75, 790748
-	report_errata WORKAROUND_CVE_2017_5715, cortex_a75, cve_2017_5715
-	report_errata WORKAROUND_CVE_2018_3639, cortex_a75, cve_2018_3639
-	report_errata ERRATA_DSU_798953, cortex_a75, dsu_798953
-	report_errata ERRATA_DSU_936184, cortex_a75, dsu_936184
-	report_errata WORKAROUND_CVE_2022_23960, cortex_a75, cve_2022_23960
-
-	ldp	x8, x30, [sp], #16
-	ret
-endfunc cortex_a75_errata_report
-#endif
+errata_report_shim cortex_a75
 
 	/* ---------------------------------------------
 	 * This function provides cortex_a75 specific
@@ -255,7 +169,7 @@
 
 declare_cpu_ops_wa cortex_a75, CORTEX_A75_MIDR, \
 	cortex_a75_reset_func, \
-	check_errata_cve_2017_5715, \
+	check_erratum_cortex_a75_5715, \
 	CPU_NO_EXTRA2_FUNC, \
 	check_smccc_arch_workaround_3, \
 	cortex_a75_core_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_hayes.S b/lib/cpus/aarch64/cortex_hayes.S
deleted file mode 100644
index 445a691..0000000
--- a/lib/cpus/aarch64/cortex_hayes.S
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-#include <common/bl_common.h>
-#include <cortex_hayes.h>
-#include <cpu_macros.S>
-#include <plat_macros.S>
-
-/* Hardware handled coherency */
-#if HW_ASSISTED_COHERENCY == 0
-#error "Cortex Hayes must be compiled with HW_ASSISTED_COHERENCY enabled"
-#endif
-
-/* 64-bit only core */
-#if CTX_INCLUDE_AARCH32_REGS == 1
-#error "Cortex Hayes supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
-#endif
-
-	/* ----------------------------------------------------
-	 * HW will do the cache maintenance while powering down
-	 * ----------------------------------------------------
-	 */
-func cortex_hayes_core_pwr_dwn
-	/* ---------------------------------------------------
-	 * Enable CPU power down bit in power control register
-	 * ---------------------------------------------------
-	 */
-	mrs	x0, CORTEX_HAYES_CPUPWRCTLR_EL1
-	orr	x0, x0, #CORTEX_HAYES_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
-	msr	CORTEX_HAYES_CPUPWRCTLR_EL1, x0
-	isb
-	ret
-endfunc cortex_hayes_core_pwr_dwn
-
-	/*
-	 * Errata printing function for Cortex Hayes. Must follow AAPCS.
-	 */
-#if REPORT_ERRATA
-func cortex_hayes_errata_report
-	ret
-endfunc cortex_hayes_errata_report
-#endif
-
-func cortex_hayes_reset_func
-	/* Disable speculative loads */
-	msr	SSBS, xzr
-	isb
-	ret
-endfunc cortex_hayes_reset_func
-
-	/* ---------------------------------------------
-	 * This function provides Cortex Hayes 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_hayes_regs, "aS"
-cortex_hayes_regs:  /* The ascii list of register names to be reported */
-	.asciz	"cpuectlr_el1", ""
-
-func cortex_hayes_cpu_reg_dump
-	adr	x6, cortex_hayes_regs
-	mrs	x8, CORTEX_HAYES_CPUECTLR_EL1
-	ret
-endfunc cortex_hayes_cpu_reg_dump
-
-declare_cpu_ops cortex_hayes, CORTEX_HAYES_MIDR, \
-	cortex_hayes_reset_func, \
-	cortex_hayes_core_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_hunter.S b/lib/cpus/aarch64/cortex_hunter.S
deleted file mode 100644
index 973637e..0000000
--- a/lib/cpus/aarch64/cortex_hunter.S
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-#include <common/bl_common.h>
-#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
-#error "Cortex Hunter must be compiled with HW_ASSISTED_COHERENCY enabled"
-#endif
-
-/* 64-bit only core */
-#if CTX_INCLUDE_AARCH32_REGS == 1
-#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
-
-	/* ----------------------------------------------------
-	 * HW will do the cache maintenance while powering down
-	 * ----------------------------------------------------
-	 */
-func cortex_hunter_core_pwr_dwn
-	/* ---------------------------------------------------
-	 * Enable CPU power down bit in power control register
-	 * ---------------------------------------------------
-	 */
-	mrs	x0, CORTEX_HUNTER_CPUPWRCTLR_EL1
-	orr	x0, x0, #CORTEX_HUNTER_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
-	msr	CORTEX_HUNTER_CPUPWRCTLR_EL1, x0
-	isb
-	ret
-endfunc cortex_hunter_core_pwr_dwn
-
-#if REPORT_ERRATA
-/*
- * 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
-
-	/* ---------------------------------------------
-	 * This function provides Cortex Hunter-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_hunter_regs, "aS"
-cortex_hunter_regs:  /* The ascii list of register names to be reported */
-	.asciz	"cpuectlr_el1", ""
-
-func cortex_hunter_cpu_reg_dump
-	adr	x6, cortex_hunter_regs
-	mrs	x8, CORTEX_HUNTER_CPUECTLR_EL1
-	ret
-endfunc cortex_hunter_cpu_reg_dump
-
-declare_cpu_ops cortex_hunter, CORTEX_HUNTER_MIDR, \
-	cortex_hunter_reset_func, \
-	cortex_hunter_core_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_hunter_elp_arm.S b/lib/cpus/aarch64/cortex_hunter_elp_arm.S
deleted file mode 100644
index 5f86d4e..0000000
--- a/lib/cpus/aarch64/cortex_hunter_elp_arm.S
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-#include <common/bl_common.h>
-#include <cortex_hunter_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
-#error "Cortex Hunter ELP must be compiled with HW_ASSISTED_COHERENCY enabled"
-#endif
-
-/* 64-bit only core */
-#if CTX_INCLUDE_AARCH32_REGS == 1
-#error "Cortex Hunter 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_HUNTER_ELP_ARM_BHB_LOOP_COUNT, cortex_hunter_elp_arm
-#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_elp_arm_reset_func
-	/* Disable speculative loads */
-	msr	SSBS, xzr
-
-#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
-	/*
-	 * The Cortex Hunter ELP generic vectors are overridden to apply errata
-	 * mitigation on exception entry from lower ELs.
-	 */
-	adr	x0, wa_cve_vbar_cortex_hunter_elp_arm
-	msr	vbar_el3, x0
-#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
-
-	isb
-	ret
-endfunc cortex_hunter_elp_arm_reset_func
-
-	/* ----------------------------------------------------
-	 * HW will do the cache maintenance while powering down
-	 * ----------------------------------------------------
-	 */
-func cortex_hunter_elp_arm_core_pwr_dwn
-	/* ---------------------------------------------------
-	 * Enable CPU power down bit in power control register
-	 * ---------------------------------------------------
-	 */
-	mrs	x0, CORTEX_HUNTER_ELP_ARM_CPUPWRCTLR_EL1
-	orr	x0, x0, #CORTEX_HUNTER_ELP_ARM_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
-	msr	CORTEX_HUNTER_ELP_ARM_CPUPWRCTLR_EL1, x0
-	isb
-	ret
-endfunc cortex_hunter_elp_arm_core_pwr_dwn
-
-#if REPORT_ERRATA
-/*
- * Errata printing function for Cortex Hunter ELP. Must follow AAPCS.
- */
-func cortex_hunter_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_hunter_elp_arm, cve_2022_23960
-
-	ldp	x8, x30, [sp], #16
-	ret
-endfunc cortex_hunter_elp_arm_errata_report
-#endif
-
-	/* ---------------------------------------------
-	 * This function provides Cortex Hunter ELP-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_hunter_elp_arm_regs, "aS"
-cortex_hunter_elp_arm_regs:  /* The ascii list of register names to be reported */
-	.asciz	"cpuectlr_el1", ""
-
-func cortex_hunter_elp_arm_cpu_reg_dump
-	adr	x6, cortex_hunter_elp_arm_regs
-	mrs	x8, CORTEX_HUNTER_ELP_ARM_CPUECTLR_EL1
-	ret
-endfunc cortex_hunter_elp_arm_cpu_reg_dump
-
-declare_cpu_ops cortex_hunter_elp_arm, CORTEX_HUNTER_ELP_ARM_MIDR, \
-	cortex_hunter_elp_arm_reset_func, \
-	cortex_hunter_elp_arm_core_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_x2.S b/lib/cpus/aarch64/cortex_x2.S
index 497bd52..816a58f 100644
--- a/lib/cpus/aarch64/cortex_x2.S
+++ b/lib/cpus/aarch64/cortex_x2.S
@@ -26,20 +26,7 @@
 	wa_cve_2022_23960_bhb_vector_table CORTEX_X2_BHB_LOOP_COUNT, cortex_x2
 #endif /* WORKAROUND_CVE_2022_23960 */
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex X2 Errata #2002765.
-	 * This applies to revisions r0p0, r1p0, and r2p0 and
-	 * is open.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_cortex_x2_2002765_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2002765
-	cbz	x0, 1f
-
+workaround_reset_start cortex_x2, ERRATUM(2002765), ERRATA_X2_2002765
 	ldr	x0, =0x6
 	msr	S3_6_C15_C8_0, x0 /* CPUPSELR_EL3 */
 	ldr	x0, =0xF3A08002
@@ -48,119 +35,24 @@
 	msr	S3_6_C15_C8_3, x0 /* CPUPMR_EL3 */
 	ldr	x0, =0x40000001003ff
 	msr	S3_6_C15_C8_1, x0 /* CPUPCR_EL3 */
-	isb
-
-1:
-	ret	x17
-endfunc errata_cortex_x2_2002765_wa
-
-func check_errata_2002765
-	/* Applies to r0p0 - r2p0 */
-	mov	x1, #0x20
-	b	cpu_rev_var_ls
-endfunc check_errata_2002765
+workaround_reset_end cortex_x2, ERRATUM(2002765)
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex X2 Errata #2058056.
-	 * This applies to revisions r0p0, r1p0, and r2p0 and
-	 * is open.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_cortex_x2_2058056_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2058056
-	cbz	x0, 1f
-
-	mrs	x1, CORTEX_X2_CPUECTLR2_EL1
-	mov	x0, #CORTEX_X2_CPUECTLR2_EL1_PF_MODE_CNSRV
-	bfi	x1, x0, #CORTEX_X2_CPUECTLR2_EL1_PF_MODE_SHIFT, #CORTEX_X2_CPUECTLR2_EL1_PF_MODE_WIDTH
-	msr	CORTEX_X2_CPUECTLR2_EL1, x1
+check_erratum_ls cortex_x2, ERRATUM(2002765), CPU_REV(2, 0)
 
-1:
-	ret	x17
-endfunc errata_cortex_x2_2058056_wa
+workaround_reset_start cortex_x2, ERRATUM(2017096), ERRATA_X2_2017096
+	sysreg_bit_set CORTEX_X2_CPUECTLR_EL1, CORTEX_X2_CPUECTLR_EL1_PFSTIDIS_BIT
+workaround_reset_end cortex_x2, ERRATUM(2017096)
 
-func check_errata_2058056
-	/* Applies to r0p0 - r2p0 */
-	mov	x1, #0x20
-	b	cpu_rev_var_ls
-endfunc check_errata_2058056
+check_erratum_ls cortex_x2, ERRATUM(2017096), CPU_REV(2, 0)
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex X2 Errata #2083908.
-	 * This applies to revision r2p0 and is open.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0-x2, x17
-	 * --------------------------------------------------
-	 */
-func errata_cortex_x2_2083908_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2083908
-	cbz	x0, 1f
+workaround_reset_start cortex_x2, ERRATUM(2058056), ERRATA_X2_2058056
+	sysreg_bitfield_insert CORTEX_X2_CPUECTLR2_EL1, CORTEX_X2_CPUECTLR2_EL1_PF_MODE_CNSRV, \
+	CORTEX_X2_CPUECTLR2_EL1_PF_MODE_SHIFT, CORTEX_X2_CPUECTLR2_EL1_PF_MODE_WIDTH
+workaround_reset_end cortex_x2, ERRATUM(2058056)
 
-	/* Apply the workaround by setting bit 13 in CPUACTLR5_EL1. */
-	mrs	x1, CORTEX_X2_CPUACTLR5_EL1
-	orr	x1, x1, #BIT(13)
-	msr	CORTEX_X2_CPUACTLR5_EL1, x1
-
-1:
-	ret	x17
-endfunc errata_cortex_x2_2083908_wa
+check_erratum_ls cortex_x2, ERRATUM(2058056), CPU_REV(2, 0)
 
-func check_errata_2083908
-	/* Applies to r2p0 */
-	mov	x1, #0x20
-	mov	x2, #0x20
-	b	cpu_rev_var_range
-endfunc check_errata_2083908
-
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex-X2 Errata 2017096.
-	 * This applies only to revisions r0p0, r1p0 and r2p0
-	 * and is fixed in r2p1.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_x2_2017096_wa
-	/* Compare x0 against revision r0p0 to r2p0 */
-	mov     x17, x30
-	bl      check_errata_2017096
-	cbz     x0, 1f
-	mrs     x1, CORTEX_X2_CPUECTLR_EL1
-	orr     x1, x1, CORTEX_X2_CPUECTLR_EL1_PFSTIDIS_BIT
-	msr     CORTEX_X2_CPUECTLR_EL1, x1
-
-1:
-	ret     x17
-endfunc errata_x2_2017096_wa
-
-func check_errata_2017096
-	/* Applies to r0p0, r1p0, r2p0 */
-	mov     x1, #0x20
-	b       cpu_rev_var_ls
-endfunc check_errata_2017096
-
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex-X2 Errata 2081180.
-	 * This applies to revision r0p0, r1p0 and r2p0
-	 * and is fixed in r2p1.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_x2_2081180_wa
-	/* Check revision. */
-	mov	x17, x30
-	bl	check_errata_2081180
-	cbz	x0, 1f
-
+workaround_reset_start cortex_x2, ERRATUM(2081180), ERRATA_X2_2081180
 	/* Apply instruction patching sequence */
 	ldr	x0, =0x3
 	msr	CORTEX_X2_IMP_CPUPSELR_EL3, x0
@@ -178,34 +70,26 @@
 	msr	CORTEX_X2_IMP_CPUPMR_EL3, x0
 	ldr	x0, =0x10002001003F3
 	msr	CORTEX_X2_IMP_CPUPCR_EL3, x0
-	isb
-1:
-	ret	x17
-endfunc errata_x2_2081180_wa
+workaround_reset_end cortex_x2, ERRATUM(2081180)
 
-func check_errata_2081180
-	/* Applies to r0p0, r1p0 and r2p0 */
-	mov	x1, #0x20
-	b	cpu_rev_var_ls
-endfunc check_errata_2081180
+check_erratum_ls cortex_x2, ERRATUM(2081180), CPU_REV(2, 0)
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex X2 Errata 2216384.
-	 * This applies to revisions r0p0, r1p0, and r2p0
-	 * and is fixed in r2p1.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * --------------------------------------------------
-	 */
-func errata_x2_2216384_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2216384
-	cbz	x0, 1f
+workaround_reset_start cortex_x2, ERRATUM(2083908), ERRATA_X2_2083908
+	/* Apply the workaround by setting bit 13 in CPUACTLR5_EL1. */
+	sysreg_bit_set CORTEX_X2_CPUACTLR5_EL1, BIT(13)
+workaround_reset_end cortex_x2, ERRATUM(2083908)
+
+check_erratum_range cortex_x2, ERRATUM(2083908), CPU_REV(2, 0), CPU_REV(2, 0)
+
+workaround_reset_start cortex_x2, ERRATUM(2147715), ERRATA_X2_2147715
+	/* Apply the workaround by setting bit 22 in CPUACTLR_EL1. */
+	sysreg_bit_set CORTEX_X2_CPUACTLR_EL1, CORTEX_X2_CPUACTLR_EL1_BIT_22
+workaround_reset_end cortex_x2, ERRATUM(2147715)
+
+check_erratum_range cortex_x2, ERRATUM(2147715), CPU_REV(2, 0), CPU_REV(2, 0)
 
-	mrs	x1, CORTEX_X2_CPUACTLR5_EL1
-	orr	x1, x1, CORTEX_X2_CPUACTLR5_EL1_BIT_17
-	msr	CORTEX_X2_CPUACTLR5_EL1, x1
+workaround_reset_start cortex_x2, ERRATUM(2216384), ERRATA_X2_2216384
+	sysreg_bit_set CORTEX_X2_CPUACTLR5_EL1, CORTEX_X2_CPUACTLR5_EL1_BIT_17
 
 	/* Apply instruction patching sequence */
 	ldr	x0, =0x5
@@ -216,138 +100,52 @@
 	msr	CORTEX_X2_IMP_CPUPMR_EL3, x0
 	ldr	x0, =0x80000000003FF
 	msr	CORTEX_X2_IMP_CPUPCR_EL3, x0
-	isb
-
-1:
-	ret	x17
-endfunc errata_x2_2216384_wa
+workaround_reset_end cortex_x2, ERRATUM(2216384)
 
-func check_errata_2216384
-	/* Applies to r0p0 - r2p0 */
-	mov	x1, #0x20
-	b	cpu_rev_var_ls
-endfunc check_errata_2216384
+check_erratum_ls cortex_x2, ERRATUM(2216384), CPU_REV(2, 0)
 
-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
-
-	/* ---------------------------------------------------------
-	 * Errata Workaround for Cortex-X2 Errata 2147715.
-	 * This applies only to revisions r2p0 and is fixed in r2p1.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * ---------------------------------------------------------
-	 */
-func errata_x2_2147715_wa
-	/* Compare x0 against revision r2p0 */
-	mov     x17, x30
-	bl      check_errata_2147715
-	cbz     x0, 1f
-
-	/* Apply the workaround by setting bit 22 in CPUACTLR_EL1. */
-	mrs     x1, CORTEX_X2_CPUACTLR_EL1
-	orr     x1, x1, CORTEX_X2_CPUACTLR_EL1_BIT_22
-	msr     CORTEX_X2_CPUACTLR_EL1, x1
-
-1:
-	ret     x17
-endfunc errata_x2_2147715_wa
-
-func check_errata_2147715
-	/* Applies to r2p0 */
-	mov	x1, #0x20
-	mov	x2, #0x20
-	b	cpu_rev_var_range
-endfunc check_errata_2147715
-
-	/* ---------------------------------------------------------------
-	 * Errata Workaround for Cortex-X2 Erratum 2282622.
-	 * This applies to revision r0p0, r1p0, r2p0 and r2p1.
-	 * It is still open.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1, x17
-	 * ---------------------------------------------------------------
-	 */
-func errata_x2_2282622_wa
-	/* Compare x0 against revision r2p1 */
-	mov     x17, x30
-	bl      check_errata_2282622
-	cbz     x0, 1f
-
+workaround_reset_start cortex_x2, ERRATUM(2282622), ERRATA_X2_2282622
 	/* Apply the workaround */
-	mrs     x1, CORTEX_X2_CPUACTLR2_EL1
-	orr     x1, x1, #BIT(0)
-	msr     CORTEX_X2_CPUACTLR2_EL1, x1
+	sysreg_bit_set CORTEX_X2_CPUACTLR2_EL1, BIT(0)
+workaround_reset_end cortex_x2, ERRATUM(2282622)
 
-1:
-	ret     x17
-endfunc errata_x2_2282622_wa
+check_erratum_ls cortex_x2, ERRATUM(2282622), CPU_REV(2, 1)
 
-func check_errata_2282622
-	/* Applies to r0p0, r1p0, r2p0 and r2p1 */
-	mov     x1, #0x21
-	b       cpu_rev_var_ls
-endfunc check_errata_2282622
+workaround_reset_start cortex_x2, ERRATUM(2371105), ERRATA_X2_2371105
+	/* Set bit 40 in CPUACTLR2_EL1 */
+	sysreg_bit_set CORTEX_X2_CPUACTLR2_EL1, CORTEX_X2_CPUACTLR2_EL1_BIT_40
+workaround_reset_end cortex_x2, ERRATUM(2371105)
 
-	/* -------------------------------------------------------
-	 * Errata Workaround for Cortex-X2 Erratum 2371105.
-	 * This applies to revisions <= r2p0 and is fixed in r2p1.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0-x17
-	 * -------------------------------------------------------
-	 */
-func errata_x2_2371105_wa
-	/* Check workaround compatibility. */
-	mov	x17, x30
-	bl	check_errata_2371105
-	cbz	x0, 1f
+check_erratum_ls cortex_x2, ERRATUM(2371105), CPU_REV(2, 0)
 
-	/* Set bit 40 in CPUACTLR2_EL1 */
-	mrs	x1, CORTEX_X2_CPUACTLR2_EL1
-	orr	x1, x1, #CORTEX_X2_CPUACTLR2_EL1_BIT_40
-	msr	CORTEX_X2_CPUACTLR2_EL1, x1
-	isb
-1:
-	ret	x17
-endfunc errata_x2_2371105_wa
+workaround_reset_start cortex_x2, ERRATUM(2768515), ERRATA_X2_2768515
+	/* dsb before isb of power down sequence */
+	dsb	sy
+workaround_reset_end cortex_x2, ERRATUM(2768515)
 
-func check_errata_2371105
-	/* Applies to <= r2p0. */
-	mov	x1, #0x20
-	b	cpu_rev_var_ls
-endfunc check_errata_2371105
+check_erratum_ls cortex_x2, ERRATUM(2768515), CPU_REV(2, 1)
 
-	/* ----------------------------------------------------
-	 * Errata Workaround for Cortex-X2 Errata #2768515
-	 * This applies to revisions <= r2p1 and is still open.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0-x17
-	 * ----------------------------------------------------
+workaround_reset_start cortex_x2, CVE(2022, 23960), WORKAROUND_CVE_2022_23960
+#if IMAGE_BL31
+	/*
+	 * The Cortex-X2 generic vectors are overridden to apply errata
+	 * mitigation on exception entry from lower ELs.
 	 */
-func errata_x2_2768515_wa
-	mov	x17, x30
-	bl	check_errata_2768515
-	cbz	x0, 1f
+	override_vector_table wa_cve_vbar_cortex_x2
+#endif /* IMAGE_BL31 */
+workaround_reset_end cortex_x2, CVE(2022, 23960)
 
-	/* dsb before isb of power down sequence */
-	dsb	sy
-1:
-	ret	x17
-endfunc errata_x2_2768515_wa
+check_erratum_chosen cortex_x2, CVE(2022, 23960), WORKAROUND_CVE_2022_23960
 
-func check_errata_2768515
-	/* Applies to all revisions <= r2p1 */
-	mov	x1, #0x21
-	b	cpu_rev_var_ls
-endfunc check_errata_2768515
+/*
+ * ERRATA_DSU_2313941 :
+ * The errata is defined in dsu_helpers.S but applies to cortex_x2
+ * as well. Henceforth creating symbolic names to the already existing errata
+ * workaround functions to get them registered under the Errata Framework.
+ */
+.equ check_erratum_cortex_x2_2313941, check_errata_dsu_2313941
+.equ erratum_cortex_x2_2313941_wa, errata_dsu_2313941_wa
+add_erratum_entry cortex_x2, ERRATUM(2313941), ERRATA_DSU_2313941, APPLY_AT_RESET
 
 	/* ----------------------------------------------------
 	 * HW will do the cache maintenance while powering down
@@ -358,122 +156,24 @@
 	 * Enable CPU power down bit in power control register
 	 * ---------------------------------------------------
 	 */
-	mrs	x0, CORTEX_X2_CPUPWRCTLR_EL1
-	orr	x0, x0, #CORTEX_X2_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
-	msr	CORTEX_X2_CPUPWRCTLR_EL1, x0
+	sysreg_bit_set CORTEX_X2_CPUPWRCTLR_EL1, CORTEX_X2_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
+
 #if ERRATA_X2_2768515
 	mov	x15, x30
 	bl	cpu_get_rev_var
-	bl	errata_x2_2768515_wa
+	bl	erratum_cortex_x2_2768515_wa
 	mov	x30, x15
 #endif /* ERRATA_X2_2768515 */
 	isb
 	ret
 endfunc cortex_x2_core_pwr_dwn
 
-	/*
-	 * Errata printing function for Cortex X2. Must follow AAPCS.
-	 */
-#if REPORT_ERRATA
-func cortex_x2_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_X2_2002765, cortex_x2, 2002765
-	report_errata ERRATA_X2_2017096, cortex_x2, 2017096
-	report_errata ERRATA_X2_2058056, cortex_x2, 2058056
-	report_errata ERRATA_X2_2081180, cortex_x2, 2081180
-	report_errata ERRATA_X2_2083908, cortex_x2, 2083908
-	report_errata ERRATA_X2_2147715, cortex_x2, 2147715
-	report_errata ERRATA_X2_2216384, cortex_x2, 2216384
-	report_errata ERRATA_X2_2282622, cortex_x2, 2282622
-	report_errata ERRATA_X2_2371105, cortex_x2, 2371105
-	report_errata ERRATA_X2_2768515, cortex_x2, 2768515
-	report_errata WORKAROUND_CVE_2022_23960, cortex_x2, cve_2022_23960
-	report_errata ERRATA_DSU_2313941, cortex_x2, dsu_2313941
-
-	ldp	x8, x30, [sp], #16
-	ret
-endfunc cortex_x2_errata_report
-#endif
-
-func cortex_x2_reset_func
-	mov	x19, x30
+errata_report_shim cortex_x2
 
+cpu_reset_func_start cortex_x2
 	/* Disable speculative loads */
 	msr	SSBS, xzr
-
-	/* 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
-#endif
-
-#if ERRATA_X2_2058056
-	mov	x0, x18
-	bl	errata_cortex_x2_2058056_wa
-#endif
-
-#if ERRATA_X2_2083908
-	mov	x0, x18
-	bl	errata_cortex_x2_2083908_wa
-#endif
-
-#if ERRATA_X2_2017096
-	mov	x0, x18
-	bl	errata_x2_2017096_wa
-#endif
-
-#if ERRATA_X2_2081180
-	mov	x0, x18
-	bl	errata_x2_2081180_wa
-#endif
-
-#if ERRATA_X2_2216384
-	mov	x0, x18
-	bl	errata_x2_2216384_wa
-#endif
-
-#if ERRATA_X2_2147715
-	mov	x0, x18
-	bl	errata_x2_2147715_wa
-#endif
-
-#if ERRATA_X2_2282622
-	mov	x0, x18
-	bl	errata_x2_2282622_wa
-#endif
-
-#if ERRATA_X2_2371105
-	mov	x0, x18
-	bl	errata_x2_2371105_wa
-#endif
-
-#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
-	/*
-	 * The Cortex-X2 generic vectors are overridden to apply errata
-         * mitigation on exception entry from lower ELs.
-         */
-	adr	x0, wa_cve_vbar_cortex_x2
-	msr	vbar_el3, x0
-#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
-
-	isb
-	ret	x19
-endfunc cortex_x2_reset_func
+cpu_reset_func_end cortex_x2
 
 	/* ---------------------------------------------
 	 * This function provides Cortex X2 specific
diff --git a/lib/cpus/aarch64/cortex_x4.S b/lib/cpus/aarch64/cortex_x4.S
new file mode 100644
index 0000000..db87008
--- /dev/null
+++ b/lib/cpus/aarch64/cortex_x4.S
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <cortex_x4.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
+#error "Cortex X4 must be compiled with HW_ASSISTED_COHERENCY enabled"
+#endif
+
+/* 64-bit only core */
+#if CTX_INCLUDE_AARCH32_REGS == 1
+#error "Cortex X4 supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
+#endif
+
+#if WORKAROUND_CVE_2022_23960
+        wa_cve_2022_23960_bhb_vector_table CORTEX_X4_BHB_LOOP_COUNT, cortex_x4
+#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_x4_reset_func
+	/* Disable speculative loads */
+	msr	SSBS, xzr
+
+#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
+	/*
+	 * The Cortex X4 generic vectors are overridden to apply errata
+	 * mitigation on exception entry from lower ELs.
+	 */
+	adr	x0, wa_cve_vbar_cortex_x4
+	msr	vbar_el3, x0
+#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
+
+	isb
+	ret
+endfunc cortex_x4_reset_func
+
+	/* ----------------------------------------------------
+	 * HW will do the cache maintenance while powering down
+	 * ----------------------------------------------------
+	 */
+func cortex_x4_core_pwr_dwn
+	/* ---------------------------------------------------
+	 * Enable CPU power down bit in power control register
+	 * ---------------------------------------------------
+	 */
+	mrs	x0, CORTEX_X4_CPUPWRCTLR_EL1
+	orr	x0, x0, #CORTEX_X4_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
+	msr	CORTEX_X4_CPUPWRCTLR_EL1, x0
+	isb
+	ret
+endfunc cortex_x4_core_pwr_dwn
+
+#if REPORT_ERRATA
+/*
+ * Errata printing function for Cortex X4. Must follow AAPCS.
+ */
+func cortex_x4_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_x4, cve_2022_23960
+
+	ldp	x8, x30, [sp], #16
+	ret
+endfunc cortex_x4_errata_report
+#endif
+
+	/* ---------------------------------------------
+	 * This function provides Cortex X4-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_x4_regs, "aS"
+cortex_x4_regs:  /* The ascii list of register names to be reported */
+	.asciz	"cpuectlr_el1", ""
+
+func cortex_x4_cpu_reg_dump
+	adr	x6, cortex_x4_regs
+	mrs	x8, CORTEX_X4_CPUECTLR_EL1
+	ret
+endfunc cortex_x4_cpu_reg_dump
+
+declare_cpu_ops cortex_x4, CORTEX_X4_MIDR, \
+	cortex_x4_reset_func, \
+	cortex_x4_core_pwr_dwn
diff --git a/lib/cpus/aarch64/dsu_helpers.S b/lib/cpus/aarch64/dsu_helpers.S
index b7e028a..a34b9a6 100644
--- a/lib/cpus/aarch64/dsu_helpers.S
+++ b/lib/cpus/aarch64/dsu_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -44,11 +44,11 @@
 	/* --------------------------------------------------
 	 * Errata Workaround for DSU erratum #798953.
 	 *
-	 * Can clobber only: x0-x17
+	 * Can clobber only: x0-x8
 	 * --------------------------------------------------
 	 */
 func errata_dsu_798953_wa
-	mov	x17, x30
+	mov	x8, x30
 	bl	check_errata_dsu_798953
 	cbz	x0, 1f
 
@@ -58,7 +58,7 @@
 	msr	CLUSTERACTLR_EL1, x0
 	isb
 1:
-	ret	x17
+	ret	x8
 endfunc errata_dsu_798953_wa
 
 	/* -----------------------------------------------------------------------
@@ -72,7 +72,7 @@
 	 * This function is called from both assembly and C environment. So it
 	 * follows AAPCS.
 	 *
-	 * Clobbers: x0-x15
+	 * Clobbers: x0-x4
 	 * -----------------------------------------------------------------------
 	 */
 	.globl	check_errata_dsu_936184
@@ -83,7 +83,7 @@
 	 * Default behaviour respresents SCU is always present with DSU.
 	 * CPUs can override this definition if required.
 	 *
-	 * Can clobber only: x0-x14
+	 * Can clobber only: x0-x3
 	 * --------------------------------------------------------------------
 	 */
 func is_scu_present_in_dsu
@@ -92,7 +92,7 @@
 endfunc is_scu_present_in_dsu
 
 func check_errata_dsu_936184
-	mov	x15, x30
+	mov	x4, x30
 	bl	is_scu_present_in_dsu
 	cmp	x0, xzr
 	/* Default error status */
@@ -116,17 +116,17 @@
 	b.hs	1f
 	mov	x0, #ERRATA_APPLIES
 1:
-	ret	x15
+	ret	x4
 endfunc check_errata_dsu_936184
 
 	/* --------------------------------------------------
 	 * Errata Workaround for DSU erratum #936184.
 	 *
-	 * Can clobber only: x0-x17
+	 * Can clobber only: x0-x8
 	 * --------------------------------------------------
 	 */
 func errata_dsu_936184_wa
-	mov	x17, x30
+	mov	x8, x30
 	bl	check_errata_dsu_936184
 	cbz	x0, 1f
 
@@ -137,7 +137,7 @@
 	msr	CLUSTERACTLR_EL1, x0
 	isb
 1:
-	ret	x17
+	ret	x8
 endfunc errata_dsu_936184_wa
 
 	/* -----------------------------------------------------------------------
@@ -176,11 +176,11 @@
 	/* --------------------------------------------------
 	 * Errata Workaround for DSU erratum #2313941.
 	 *
-	 * Can clobber only: x0-x17
+	 * Can clobber only: x0-x8
 	 * --------------------------------------------------
 	 */
 func errata_dsu_2313941_wa
-	mov	x17, x30
+	mov	x8, x30
 	bl	check_errata_dsu_2313941
 	cbz	x0, 1f
 
@@ -190,6 +190,5 @@
 	msr	CLUSTERACTLR_EL1, x0
 	isb
 1:
-	ret	x17
+	ret	x8
 endfunc errata_dsu_2313941_wa
-
diff --git a/lib/cpus/aarch64/neoverse_e1.S b/lib/cpus/aarch64/neoverse_e1.S
index 96b63cf..45bd8d3 100644
--- a/lib/cpus/aarch64/neoverse_e1.S
+++ b/lib/cpus/aarch64/neoverse_e1.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -21,20 +21,18 @@
 #error "Neoverse-E1 supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
-	/* -------------------------------------------------
-	 * The CPU Ops reset function for Neoverse-E1.
-	 * Shall clobber: x0-x19
-	 * -------------------------------------------------
-	 */
-func neoverse_e1_reset_func
-	mov	x19, x30
-
-#if ERRATA_DSU_936184
-	bl	errata_dsu_936184_wa
-#endif
+/*
+ * ERRATA_DSU_936184:
+ * The errata is defined in dsu_helpers.S and applies to neoverse_e1.
+ * Henceforth creating symbolic names to the already existing errata
+ * workaround functions to get them registered under the Errata Framework.
+ */
+.equ check_erratum_neoverse_e1_936184, check_errata_dsu_936184
+.equ erratum_neoverse_e1_936184_wa, errata_dsu_936184_wa
+add_erratum_entry neoverse_e1, ERRATUM(936184), ERRATA_DSU_936184, APPLY_AT_RESET
 
-	ret	x19
-endfunc neoverse_e1_reset_func
+cpu_reset_func_start neoverse_e1
+cpu_reset_func_end neoverse_e1
 
 func neoverse_e1_cpu_pwr_dwn
 	mrs	x0, NEOVERSE_E1_CPUPWRCTLR_EL1
@@ -44,27 +42,7 @@
 	ret
 endfunc neoverse_e1_cpu_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Neoverse N1. Must follow AAPCS.
- */
-func neoverse_e1_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_DSU_936184, neoverse_e1, dsu_936184
-
-	ldp	x8, x30, [sp], #16
-	ret
-endfunc neoverse_e1_errata_report
-#endif
-
+errata_report_shim neoverse_e1
 
 .section .rodata.neoverse_e1_regs, "aS"
 neoverse_e1_regs:  /* The ascii list of register names to be reported */
diff --git a/lib/cpus/aarch64/neoverse_hermes.S b/lib/cpus/aarch64/neoverse_hermes.S
new file mode 100644
index 0000000..cb90b71
--- /dev/null
+++ b/lib/cpus/aarch64/neoverse_hermes.S
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <neoverse_hermes.h>
+#include <cpu_macros.S>
+#include <plat_macros.S>
+
+/* Hardware handled coherency */
+#if HW_ASSISTED_COHERENCY == 0
+#error "Neoverse Hermes must be compiled with HW_ASSISTED_COHERENCY enabled"
+#endif
+
+/* 64-bit only core */
+#if CTX_INCLUDE_AARCH32_REGS == 1
+#error "Neoverse Hermes supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
+#endif
+
+cpu_reset_func_start neoverse_hermes
+	/* Disable speculative loads */
+	msr	SSBS, xzr
+cpu_reset_func_end neoverse_hermes
+
+	/* ----------------------------------------------------
+	 * HW will do the cache maintenance while powering down
+	 * ----------------------------------------------------
+	 */
+func neoverse_hermes_core_pwr_dwn
+	/* ---------------------------------------------------
+	 * Enable CPU power down bit in power control register
+	 * ---------------------------------------------------
+	 */
+	sysreg_bit_set NEOVERSE_HERMES_CPUPWRCTLR_EL1, NEOVERSE_HERMES_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
+	isb
+	ret
+endfunc neoverse_hermes_core_pwr_dwn
+
+errata_report_shim neoverse_hermes
+
+	/* ---------------------------------------------
+	 * This function provides Neoverse Hermes 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.neoverse_hermes_regs, "aS"
+neoverse_hermes_regs:  /* The ascii list of register names to be reported */
+	.asciz	"cpuectlr_el1", ""
+
+func neoverse_hermes_cpu_reg_dump
+	adr	x6, neoverse_hermes_regs
+	mrs	x8, NEOVERSE_HERMES_CPUECTLR_EL1
+	ret
+endfunc neoverse_hermes_cpu_reg_dump
+
+declare_cpu_ops neoverse_hermes, NEOVERSE_HERMES_MIDR, \
+	neoverse_hermes_reset_func, \
+	neoverse_hermes_core_pwr_dwn
diff --git a/lib/cpus/aarch64/neoverse_n1.S b/lib/cpus/aarch64/neoverse_n1.S
index 2cf94c7..36a7ee7 100644
--- a/lib/cpus/aarch64/neoverse_n1.S
+++ b/lib/cpus/aarch64/neoverse_n1.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -27,20 +27,17 @@
 	wa_cve_2022_23960_bhb_vector_table NEOVERSE_N1_BHB_LOOP_COUNT, neoverse_n1
 #endif /* WORKAROUND_CVE_2022_23960 */
 
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Erratum 1043202.
- * This applies to revision r0p0 and r1p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
+/*
+ * ERRATA_DSU_936184:
+ * The errata is defined in dsu_helpers.S and applies to Neoverse N1.
+ * Henceforth creating symbolic names to the already existing errata
+ * workaround functions to get them registered under the Errata Framework.
  */
-func errata_n1_1043202_wa
-	/* Compare x0 against revision r1p0 */
-	mov	x17, x30
-	bl	check_errata_1043202
-	cbz	x0, 1f
+.equ check_erratum_neoverse_n1_936184, check_errata_dsu_936184
+.equ erratum_neoverse_n1_936184_wa, errata_dsu_936184_wa
+add_erratum_entry neoverse_n1, ERRATUM(936184), ERRATA_DSU_936184, APPLY_AT_RESET
 
+workaround_reset_start neoverse_n1, ERRATUM(1043202), ERRATA_N1_1043202
 	/* Apply instruction patching sequence */
 	ldr	x0, =0x0
 	msr	CPUPSELR_EL3, x0
@@ -50,314 +47,72 @@
 	msr	CPUPMR_EL3, x0
 	ldr	x0, =0x800200071
 	msr	CPUPCR_EL3, x0
-	isb
-1:
-	ret	x17
-endfunc errata_n1_1043202_wa
+workaround_reset_end neoverse_n1, ERRATUM(1043202)
 
-func check_errata_1043202
-	/* Applies to r0p0 and r1p0 */
-	mov	x1, #0x10
-	b	cpu_rev_var_ls
-endfunc check_errata_1043202
+check_erratum_ls neoverse_n1, ERRATUM(1043202), CPU_REV(1, 0)
 
-/* --------------------------------------------------
- * Disable speculative loads if Neoverse N1 supports
- * SSBS.
- *
- * Shall clobber: x0.
- * --------------------------------------------------
- */
-func neoverse_n1_disable_speculative_loads
-	/* Check if the PE implements SSBS */
-	mrs	x0, id_aa64pfr1_el1
-	tst	x0, #(ID_AA64PFR1_EL1_SSBS_MASK << ID_AA64PFR1_EL1_SSBS_SHIFT)
-	b.eq	1f
+workaround_reset_start neoverse_n1, ERRATUM(1073348), ERRATA_N1_1073348
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR_EL1, NEOVERSE_N1_CPUACTLR_EL1_BIT_6
+workaround_reset_end neoverse_n1, ERRATUM(1073348)
 
-	/* Disable speculative loads */
-	msr	SSBS, xzr
+check_erratum_ls neoverse_n1, ERRATUM(1073348), CPU_REV(1, 0)
 
-1:
-	ret
-endfunc neoverse_n1_disable_speculative_loads
-
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Errata #1073348
- * This applies to revision r0p0 and r1p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1073348_wa
-	/* Compare x0 against revision r1p0 */
-	mov	x17, x30
-	bl	check_errata_1073348
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUACTLR_EL1
-	orr	x1, x1, NEOVERSE_N1_CPUACTLR_EL1_BIT_6
-	msr	NEOVERSE_N1_CPUACTLR_EL1, x1
-1:
-	ret	x17
-endfunc errata_n1_1073348_wa
-
-func check_errata_1073348
-	/* Applies to r0p0 and r1p0 */
-	mov	x1, #0x10
-	b	cpu_rev_var_ls
-endfunc check_errata_1073348
-
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Errata #1130799
- * This applies to revision <=r2p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1130799_wa
-	/* Compare x0 against revision r2p0 */
-	mov	x17, x30
-	bl	check_errata_1130799
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUACTLR2_EL1
-	orr	x1, x1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_59
-	msr	NEOVERSE_N1_CPUACTLR2_EL1, x1
-1:
-	ret	x17
-endfunc errata_n1_1130799_wa
-
-func check_errata_1130799
-	/* Applies to <=r2p0 */
-	mov	x1, #0x20
-	b	cpu_rev_var_ls
-endfunc check_errata_1130799
-
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Errata #1165347
- * This applies to revision <=r2p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1165347_wa
-	/* Compare x0 against revision r2p0 */
-	mov	x17, x30
-	bl	check_errata_1165347
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUACTLR2_EL1
-	orr	x1, x1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_0
-	orr	x1, x1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_15
-	msr	NEOVERSE_N1_CPUACTLR2_EL1, x1
-1:
-	ret	x17
-endfunc errata_n1_1165347_wa
+workaround_reset_start neoverse_n1, ERRATUM(1130799), ERRATA_N1_1130799
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR2_EL1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_59
+workaround_reset_end neoverse_n1, ERRATUM(1130799)
 
-func check_errata_1165347
-	/* Applies to <=r2p0 */
-	mov	x1, #0x20
-	b	cpu_rev_var_ls
-endfunc check_errata_1165347
-
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Errata #1207823
- * This applies to revision <=r2p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1207823_wa
-	/* Compare x0 against revision r2p0 */
-	mov	x17, x30
-	bl	check_errata_1207823
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUACTLR2_EL1
-	orr	x1, x1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_11
-	msr	NEOVERSE_N1_CPUACTLR2_EL1, x1
-1:
-	ret	x17
-endfunc errata_n1_1207823_wa
+check_erratum_ls neoverse_n1, ERRATUM(1130799), CPU_REV(2, 0)
 
-func check_errata_1207823
-	/* Applies to <=r2p0 */
-	mov	x1, #0x20
-	b	cpu_rev_var_ls
-endfunc check_errata_1207823
+workaround_reset_start neoverse_n1, ERRATUM(1165347), ERRATA_N1_1165347
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR2_EL1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_0
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR2_EL1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_15
+workaround_reset_end neoverse_n1, ERRATUM(1165347)
 
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Errata #1220197
- * This applies to revision <=r2p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1220197_wa
-	/* Compare x0 against revision r2p0 */
-	mov	x17, x30
-	bl	check_errata_1220197
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUECTLR_EL1
-	orr	x1, x1, NEOVERSE_N1_WS_THR_L2_MASK
-	msr	NEOVERSE_N1_CPUECTLR_EL1, x1
-1:
-	ret	x17
-endfunc errata_n1_1220197_wa
+check_erratum_ls neoverse_n1, ERRATUM(1165347), CPU_REV(2, 0)
 
-func check_errata_1220197
-	/* Applies to <=r2p0 */
-	mov	x1, #0x20
-	b	cpu_rev_var_ls
-endfunc check_errata_1220197
+workaround_reset_start neoverse_n1, ERRATUM(1207823), ERRATA_N1_1207823
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR2_EL1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_11
+workaround_reset_end neoverse_n1, ERRATUM(1207823)
 
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Errata #1257314
- * This applies to revision <=r3p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1257314_wa
-	/* Compare x0 against revision r3p0 */
-	mov	x17, x30
-	bl	check_errata_1257314
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUACTLR3_EL1
-	orr	x1, x1, NEOVERSE_N1_CPUACTLR3_EL1_BIT_10
-	msr	NEOVERSE_N1_CPUACTLR3_EL1, x1
-1:
-	ret	x17
-endfunc errata_n1_1257314_wa
+check_erratum_ls neoverse_n1, ERRATUM(1207823), CPU_REV(2, 0)
 
-func check_errata_1257314
-	/* Applies to <=r3p0 */
-	mov	x1, #0x30
-	b	cpu_rev_var_ls
-endfunc check_errata_1257314
+workaround_reset_start neoverse_n1, ERRATUM(1220197), ERRATA_N1_1220197
+	sysreg_bit_set NEOVERSE_N1_CPUECTLR_EL1, NEOVERSE_N1_WS_THR_L2_MASK
+workaround_reset_end neoverse_n1, ERRATUM(1220197)
 
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Errata #1262606
- * This applies to revision <=r3p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1262606_wa
-	/* Compare x0 against revision r3p0 */
-	mov	x17, x30
-	bl	check_errata_1262606
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUACTLR_EL1
-	orr	x1, x1, NEOVERSE_N1_CPUACTLR_EL1_BIT_13
-	msr	NEOVERSE_N1_CPUACTLR_EL1, x1
-1:
-	ret	x17
-endfunc errata_n1_1262606_wa
+check_erratum_ls neoverse_n1, ERRATUM(1220197), CPU_REV(2, 0)
 
-func check_errata_1262606
-	/* Applies to <=r3p0 */
-	mov	x1, #0x30
-	b	cpu_rev_var_ls
-endfunc check_errata_1262606
+workaround_reset_start neoverse_n1, ERRATUM(1257314), ERRATA_N1_1257314
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR3_EL1, NEOVERSE_N1_CPUACTLR3_EL1_BIT_10
+workaround_reset_end neoverse_n1, ERRATUM(1257314)
 
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Errata #1262888
- * This applies to revision <=r3p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1262888_wa
-	/* Compare x0 against revision r3p0 */
-	mov	x17, x30
-	bl	check_errata_1262888
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUECTLR_EL1
-	orr	x1, x1, NEOVERSE_N1_CPUECTLR_EL1_MM_TLBPF_DIS_BIT
-	msr	NEOVERSE_N1_CPUECTLR_EL1, x1
-1:
-	ret	x17
-endfunc errata_n1_1262888_wa
+check_erratum_ls neoverse_n1, ERRATUM(1257314), CPU_REV(3, 0)
 
-func check_errata_1262888
-	/* Applies to <=r3p0 */
-	mov	x1, #0x30
-	b	cpu_rev_var_ls
-endfunc check_errata_1262888
+workaround_reset_start neoverse_n1, ERRATUM(1262606), ERRATA_N1_1262606
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR_EL1, NEOVERSE_N1_CPUACTLR_EL1_BIT_13
+workaround_reset_end neoverse_n1, ERRATUM(1262606)
 
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Errata #1275112
- * This applies to revision <=r3p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1275112_wa
-	/* Compare x0 against revision r3p0 */
-	mov	x17, x30
-	bl	check_errata_1275112
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUACTLR_EL1
-	orr	x1, x1, NEOVERSE_N1_CPUACTLR_EL1_BIT_13
-	msr	NEOVERSE_N1_CPUACTLR_EL1, x1
-1:
-	ret	x17
-endfunc errata_n1_1275112_wa
+check_erratum_ls neoverse_n1, ERRATUM(1262606), CPU_REV(3, 0)
 
-func check_errata_1275112
-	/* Applies to <=r3p0 */
-	mov	x1, #0x30
-	b	cpu_rev_var_ls
-endfunc check_errata_1275112
+workaround_reset_start neoverse_n1, ERRATUM(1262888), ERRATA_N1_1262888
+	sysreg_bit_set NEOVERSE_N1_CPUECTLR_EL1, NEOVERSE_N1_CPUECTLR_EL1_MM_TLBPF_DIS_BIT
+workaround_reset_end neoverse_n1, ERRATUM(1262888)
 
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Erratum 1315703.
- * This applies to revision <= r3p0 of Neoverse N1.
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1315703_wa
-	/* Compare x0 against revision r3p1 */
-	mov	x17, x30
-	bl	check_errata_1315703
-	cbz	x0, 1f
+check_erratum_ls neoverse_n1, ERRATUM(1262888), CPU_REV(3, 0)
 
-	mrs	x0, NEOVERSE_N1_CPUACTLR2_EL1
-	orr	x0, x0, #NEOVERSE_N1_CPUACTLR2_EL1_BIT_16
-	msr	NEOVERSE_N1_CPUACTLR2_EL1, x0
+workaround_reset_start neoverse_n1, ERRATUM(1275112), ERRATA_N1_1275112
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR_EL1, NEOVERSE_N1_CPUACTLR_EL1_BIT_13
+workaround_reset_end neoverse_n1, ERRATUM(1275112)
 
-1:
-	ret	x17
-endfunc errata_n1_1315703_wa
+check_erratum_ls neoverse_n1, ERRATUM(1275112), CPU_REV(3, 0)
 
-func check_errata_1315703
-	/* Applies to everything <= r3p0. */
-	mov	x1, #0x30
-	b	cpu_rev_var_ls
-endfunc check_errata_1315703
+workaround_reset_start neoverse_n1, ERRATUM(1315703), ERRATA_N1_1315703
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR2_EL1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_16
+workaround_reset_end neoverse_n1, ERRATUM(1315703)
 
-/* --------------------------------------------------
- * Errata Workaround for Neoverse N1 Erratum 1542419.
- * This applies to revisions r3p0 - r4p0 of Neoverse N1
- * Inputs:
- * x0: variant[4:7] and revision[0:3] of current cpu.
- * Shall clobber: x0-x17
- * --------------------------------------------------
- */
-func errata_n1_1542419_wa
-	/* Compare x0 against revision r3p0 and r4p0 */
-	mov	x17, x30
-	bl	check_errata_1542419
-	cbz	x0, 1f
+check_erratum_ls neoverse_n1, ERRATUM(1315703), CPU_REV(3, 0)
 
+workaround_reset_start neoverse_n1, ERRATUM(1542419), ERRATA_N1_1542419
 	/* Apply instruction patching sequence */
 	ldr	x0, =0x0
 	msr	CPUPSELR_EL3, x0
@@ -368,67 +123,17 @@
 	ldr	x0, =0x08000020007D
 	msr	CPUPCR_EL3, x0
 	isb
-1:
-	ret	x17
-endfunc errata_n1_1542419_wa
+workaround_reset_end neoverse_n1, ERRATUM(1542419)
 
-func check_errata_1542419
-	/* Applies to everything r3p0 - r4p0. */
-	mov	x1, #0x30
-	mov	x2, #0x40
-	b	cpu_rev_var_range
-endfunc check_errata_1542419
+check_erratum_range neoverse_n1, ERRATUM(1542419), CPU_REV(3, 0), CPU_REV(4, 0)
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Neoverse N1 Errata #1868343.
-	 * This applies to revision <= r4p0 of Neoverse N1.
-	 * This workaround is the same as the workaround for
-	 * errata 1262606 and 1275112 but applies to a wider
-	 * revision range.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0-x17
-	 * --------------------------------------------------
-	 */
-func errata_n1_1868343_wa
-	/*
-	 * Compare x0 against revision r4p0
-	 */
-	mov	x17, x30
-	bl	check_errata_1868343
-	cbz	x0, 1f
-	mrs	x1, NEOVERSE_N1_CPUACTLR_EL1
-	orr	x1, x1, NEOVERSE_N1_CPUACTLR_EL1_BIT_13
-	msr	NEOVERSE_N1_CPUACTLR_EL1, x1
-	isb
-1:
-	ret	x17
-endfunc errata_n1_1868343_wa
+workaround_reset_start neoverse_n1, ERRATUM(1868343), ERRATA_N1_1868343
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR_EL1, NEOVERSE_N1_CPUACTLR_EL1_BIT_13
+workaround_reset_end neoverse_n1, ERRATUM(1868343)
 
-func check_errata_1868343
-	/* Applies to everything <= r4p0 */
-	mov	x1, #0x40
-	b	cpu_rev_var_ls
-endfunc check_errata_1868343
+check_erratum_ls neoverse_n1, ERRATUM(1868343), CPU_REV(4, 0)
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Neoverse N1 Errata #1946160.
-	 * This applies to revisions r3p0, r3p1, r4p0, and
-	 * r4p1 of Neoverse N1. It also exists in r0p0, r1p0,
-	 * and r2p0 but there is no fix in these revisions.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0-x17
-	 * --------------------------------------------------
-	 */
-func errata_n1_1946160_wa
-	/*
-	 * Compare x0 against r3p0 - r4p1
-	 */
-	mov	x17, x30
-	bl	check_errata_1946160
-	cbz	x0, 1f
-
+workaround_reset_start neoverse_n1, ERRATUM(1946160), ERRATA_N1_1946160
 	mov	x0, #3
 	msr	S3_6_C15_C8_0, x0
 	ldr	x0, =0x10E3900002
@@ -437,7 +142,6 @@
 	msr	S3_6_C15_C8_3, x0
 	ldr	x0, =0x2001003FF
 	msr	S3_6_C15_C8_1, x0
-
 	mov	x0, #4
 	msr	S3_6_C15_C8_0, x0
 	ldr	x0, =0x10E3800082
@@ -446,7 +150,6 @@
 	msr	S3_6_C15_C8_3, x0
 	ldr	x0, =0x2001003FF
 	msr	S3_6_C15_C8_1, x0
-
 	mov	x0, #5
 	msr	S3_6_C15_C8_0, x0
 	ldr	x0, =0x10E3800200
@@ -455,147 +158,62 @@
 	msr	S3_6_C15_C8_3, x0
 	ldr	x0, =0x2001003FF
 	msr	S3_6_C15_C8_1, x0
-
 	isb
-1:
-	ret	x17
-endfunc errata_n1_1946160_wa
+workaround_reset_end neoverse_n1, ERRATUM(1946160)
 
-func check_errata_1946160
-	/* Applies to r3p0 - r4p1. */
-	mov	x1, #0x30
-	mov	x2, #0x41
-	b	cpu_rev_var_range
-endfunc check_errata_1946160
+check_erratum_range neoverse_n1, ERRATUM(1946160), CPU_REV(3, 0), CPU_REV(4, 1)
 
-	/* ----------------------------------------------------
-	 * Errata Workaround for Neoverse N1 Errata #2743102
-	 * This applies to revisions <= r4p1 and is still open.
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0-x17
-	 * ----------------------------------------------------
-	 */
-func errata_n1_2743102_wa
-	mov	x17, x30
-	bl	check_errata_2743102
-	cbz	x0, 1f
-
+workaround_runtime_start neoverse_n1, ERRATUM(2743102), ERRATA_N1_2743102
 	/* dsb before isb of power down sequence */
 	dsb	sy
-1:
-	ret	x17
-endfunc errata_n1_2743102_wa
+workaround_runtime_end neoverse_n1, ERRATUM(2743102)
 
-func check_errata_2743102
-	/* Applies to all revisions <= r4p1 */
-	mov	x1, #0x41
-	b	cpu_rev_var_ls
-endfunc check_errata_2743102
-
-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_n1_reset_func
-	mov	x19, x30
+check_erratum_ls neoverse_n1, ERRATUM(2743102), CPU_REV(4, 1)
 
-	bl neoverse_n1_disable_speculative_loads
-
-	/* Forces all cacheable atomic instructions to be near */
-	mrs	x0, NEOVERSE_N1_CPUACTLR2_EL1
-	orr	x0, x0, #NEOVERSE_N1_CPUACTLR2_EL1_BIT_2
-	msr	NEOVERSE_N1_CPUACTLR2_EL1, x0
-	isb
-
-	bl	cpu_get_rev_var
-	mov	x18, x0
-
-#if ERRATA_N1_1043202
-	mov	x0, x18
-	bl	errata_n1_1043202_wa
-#endif
-
-#if ERRATA_N1_1073348
-	mov	x0, x18
-	bl	errata_n1_1073348_wa
-#endif
-
-#if ERRATA_N1_1130799
-	mov	x0, x18
-	bl	errata_n1_1130799_wa
-#endif
-
-#if ERRATA_N1_1165347
-	mov	x0, x18
-	bl	errata_n1_1165347_wa
-#endif
-
-#if ERRATA_N1_1207823
-	mov	x0, x18
-	bl	errata_n1_1207823_wa
-#endif
-
-#if ERRATA_N1_1220197
-	mov	x0, x18
-	bl	errata_n1_1220197_wa
-#endif
-
-#if ERRATA_N1_1257314
-	mov	x0, x18
-	bl	errata_n1_1257314_wa
-#endif
-
-#if ERRATA_N1_1262606
-	mov	x0, x18
-	bl	errata_n1_1262606_wa
-#endif
+workaround_reset_start neoverse_n1, CVE(2022, 23960), WORKAROUND_CVE_2022_23960
+#if IMAGE_BL31
+	/*
+	 * The Neoverse-N1 generic vectors are overridden to apply errata
+	 * mitigation on exception entry from lower ELs.
+	 */
+	override_vector_table wa_cve_vbar_neoverse_n1
+#endif /* IMAGE_BL31 */
+workaround_reset_end neoverse_n1, CVE(2022, 23960)
 
-#if ERRATA_N1_1262888
-	mov	x0, x18
-	bl	errata_n1_1262888_wa
-#endif
+check_erratum_chosen neoverse_n1, CVE(2022, 23960), WORKAROUND_CVE_2022_23960
 
-#if ERRATA_N1_1275112
-	mov	x0, x18
-	bl	errata_n1_1275112_wa
-#endif
+/* --------------------------------------------------
+ * Disable speculative loads if Neoverse N1 supports
+ * SSBS.
+ *
+ * Shall clobber: x0.
+ * --------------------------------------------------
+ */
+func neoverse_n1_disable_speculative_loads
+	/* Check if the PE implements SSBS */
+	mrs	x0, id_aa64pfr1_el1
+	tst	x0, #(ID_AA64PFR1_EL1_SSBS_MASK << ID_AA64PFR1_EL1_SSBS_SHIFT)
+	b.eq	1f
 
-#if ERRATA_N1_1315703
-	mov	x0, x18
-	bl	errata_n1_1315703_wa
-#endif
+	/* Disable speculative loads */
+	msr	SSBS, xzr
 
-#if ERRATA_N1_1542419
-	mov	x0, x18
-	bl	errata_n1_1542419_wa
-#endif
+1:
+	ret
+endfunc neoverse_n1_disable_speculative_loads
 
-#if ERRATA_N1_1868343
-	mov	x0, x18
-	bl	errata_n1_1868343_wa
-#endif
+cpu_reset_func_start neoverse_n1
+	bl neoverse_n1_disable_speculative_loads
 
-#if ERRATA_N1_1946160
-	mov	x0, x18
-	bl	errata_n1_1946160_wa
-#endif
+	/* Forces all cacheable atomic instructions to be near */
+	sysreg_bit_set NEOVERSE_N1_CPUACTLR2_EL1, NEOVERSE_N1_CPUACTLR2_EL1_BIT_2
+	isb
 
 #if ENABLE_FEAT_AMU
 	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
-	mrs	x0, actlr_el3
-	orr	x0, x0, #NEOVERSE_N1_ACTLR_AMEN_BIT
-	msr	actlr_el3, x0
-
+	sysreg_bit_set actlr_el3, NEOVERSE_N1_ACTLR_AMEN_BIT
 	/* Make sure accesses from EL0/EL1 are not trapped to EL2 */
-	mrs	x0, actlr_el2
-	orr	x0, x0, #NEOVERSE_N1_ACTLR_AMEN_BIT
-	msr	actlr_el2, x0
-
+	sysreg_bit_set actlr_el2, NEOVERSE_N1_ACTLR_AMEN_BIT
 	/* Enable group0 counters */
 	mov	x0, #NEOVERSE_N1_AMU_GROUP0_MASK
 	msr	CPUAMCNTENSET_EL0, x0
@@ -603,27 +221,9 @@
 
 #if NEOVERSE_Nx_EXTERNAL_LLC
 	/* Some system may have External LLC, core needs to be made aware */
-	mrs     x0, NEOVERSE_N1_CPUECTLR_EL1
-	orr     x0, x0, NEOVERSE_N1_CPUECTLR_EL1_EXTLLC_BIT
-	msr     NEOVERSE_N1_CPUECTLR_EL1, x0
-#endif
-
-#if ERRATA_DSU_936184
-	bl	errata_dsu_936184_wa
+	sysreg_bit_set NEOVERSE_N1_CPUECTLR_EL1, NEOVERSE_N1_CPUECTLR_EL1_EXTLLC_BIT
 #endif
-
-#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
-	/*
-	 * The Neoverse-N1 generic vectors are overridden to apply errata
-         * mitigation on exception entry from lower ELs.
-	 */
-	adr	x0, wa_cve_vbar_neoverse_n1
-	msr	vbar_el3, x0
-#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
-
-	isb
-	ret	x19
-endfunc neoverse_n1_reset_func
+cpu_reset_func_end neoverse_n1
 
 	/* ---------------------------------------------
 	 * HW will do the cache maintenance while powering down
@@ -634,55 +234,15 @@
 	 * Enable CPU power down bit in power control register
 	 * ---------------------------------------------
 	 */
-	mrs	x0, NEOVERSE_N1_CPUPWRCTLR_EL1
-	orr	x0, x0, #NEOVERSE_N1_CORE_PWRDN_EN_MASK
-	msr	NEOVERSE_N1_CPUPWRCTLR_EL1, x0
-#if ERRATA_N1_2743102
-	mov	x15, x30
-	bl	cpu_get_rev_var
-	bl	errata_n1_2743102_wa
-	mov	x30, x15
-#endif /* ERRATA_N1_2743102 */
+	sysreg_bit_set NEOVERSE_N1_CPUPWRCTLR_EL1, NEOVERSE_N1_CORE_PWRDN_EN_MASK
+
+	apply_erratum neoverse_n1, ERRATUM(2743102), ERRATA_N1_2743102
+
 	isb
 	ret
 endfunc neoverse_n1_core_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Neoverse N1. Must follow AAPCS.
- */
-func neoverse_n1_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_N1_1043202, neoverse_n1, 1043202
-	report_errata ERRATA_N1_1073348, neoverse_n1, 1073348
-	report_errata ERRATA_N1_1130799, neoverse_n1, 1130799
-	report_errata ERRATA_N1_1165347, neoverse_n1, 1165347
-	report_errata ERRATA_N1_1207823, neoverse_n1, 1207823
-	report_errata ERRATA_N1_1220197, neoverse_n1, 1220197
-	report_errata ERRATA_N1_1257314, neoverse_n1, 1257314
-	report_errata ERRATA_N1_1262606, neoverse_n1, 1262606
-	report_errata ERRATA_N1_1262888, neoverse_n1, 1262888
-	report_errata ERRATA_N1_1275112, neoverse_n1, 1275112
-	report_errata ERRATA_N1_1315703, neoverse_n1, 1315703
-	report_errata ERRATA_N1_1542419, neoverse_n1, 1542419
-	report_errata ERRATA_N1_1868343, neoverse_n1, 1868343
-	report_errata ERRATA_N1_1946160, neoverse_n1, 1946160
-	report_errata ERRATA_N1_2743102, neoverse_n1, 2743102
-	report_errata ERRATA_DSU_936184, neoverse_n1, dsu_936184
-	report_errata WORKAROUND_CVE_2022_23960, neoverse_n1, cve_2022_23960
-
-	ldp	x8, x30, [sp], #16
-	ret
-endfunc neoverse_n1_errata_report
-#endif
+errata_report_shim neoverse_n1
 
 /*
  * Handle trap of EL0 IC IVAU instructions to EL3 by executing a TLB
diff --git a/lib/cpus/aarch64/neoverse_v2.S b/lib/cpus/aarch64/neoverse_v2.S
index 4ea887f..6e00e5e 100644
--- a/lib/cpus/aarch64/neoverse_v2.S
+++ b/lib/cpus/aarch64/neoverse_v2.S
@@ -38,6 +38,12 @@
 	mrs	x0, NEOVERSE_V2_CPUPWRCTLR_EL1
 	orr	x0, x0, #NEOVERSE_V2_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
 	msr	NEOVERSE_V2_CPUPWRCTLR_EL1, x0
+#if ERRATA_V2_2801372
+	mov	x15, x30
+	bl	cpu_get_rev_var
+	bl	errata_neoverse_v2_2801372_wa
+	mov	x30, x15
+#endif /* ERRATA_V2_2801372 */
 	isb
 	ret
 endfunc neoverse_v2_core_pwr_dwn
@@ -51,6 +57,30 @@
 	ret
 endfunc check_errata_cve_2022_23960
 
+	/* ----------------------------------------------------
+	 * Errata Workaround for Neoverse V2 Errata #2801372
+	 * This applies to revisions <= r0p1 and is fixed in r0p2.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * ----------------------------------------------------
+	 */
+func errata_neoverse_v2_2801372_wa
+	mov	x17, x30
+	bl	check_errata_2801372
+	cbz	x0, 1f
+
+	/* dsb before isb of power down sequence */
+	dsb	sy
+1:
+	ret	x17
+endfunc errata_neoverse_v2_2801372_wa
+
+func check_errata_2801372
+	/* Applies to all revisions <= r0p1 */
+	mov	x1, #0x01
+	b	cpu_rev_var_ls
+endfunc check_errata_2801372
+
 func neoverse_v2_reset_func
 	/* Disable speculative loads */
 	msr	SSBS, xzr
@@ -82,6 +112,7 @@
 	 * checking functions of each errata.
 	 */
 	report_errata WORKAROUND_CVE_2022_23960, neoverse_v2, cve_2022_23960
+	report_errata ERRATA_V2_2801372, neoverse_v2, 2801372
 
 	ldp	x8, x30, [sp], #16
 	ret
diff --git a/lib/cpus/aarch64/qemu_max.S b/lib/cpus/aarch64/qemu_max.S
index 8948fda..00963bc 100644
--- a/lib/cpus/aarch64/qemu_max.S
+++ b/lib/cpus/aarch64/qemu_max.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -47,14 +47,7 @@
 	b	dcsw_op_all
 endfunc qemu_max_cluster_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for QEMU "max". Must follow AAPCS.
- */
-func qemu_max_errata_report
-	ret
-endfunc qemu_max_errata_report
-#endif
+errata_report_shim qemu_max
 
 	/* ---------------------------------------------
 	 * This function provides cpu specific
diff --git a/lib/cpus/aarch64/rainier.S b/lib/cpus/aarch64/rainier.S
index 3b7b8b2..c770f54 100644
--- a/lib/cpus/aarch64/rainier.S
+++ b/lib/cpus/aarch64/rainier.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -41,78 +41,30 @@
 	ret
 endfunc rainier_disable_speculative_loads
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Neoverse N1 Errata #1868343.
-	 * This applies to revision <= r4p0 of Neoverse N1.
-	 * This workaround is the same as the workaround for
-	 * errata 1262606 and 1275112 but applies to a wider
-	 * revision range.
-	 * Rainier R0P0 is based on Neoverse N1 R4P0 so the
-	 * workaround checks for r0p0 version of Rainier CPU.
-	 * Inputs:
-	 * x0: variant[4:7] and revision[0:3] of current cpu.
-	 * Shall clobber: x0, x1 & x17
-	 * --------------------------------------------------
-	 */
-func errata_n1_1868343_wa
-	/*
-	 * Compare x0 against revision r4p0
-	 */
-	mov	x17, x30
-	bl	check_errata_1868343
-	cbz	x0, 1f
-	mrs	x1, RAINIER_CPUACTLR_EL1
-	orr	x1, x1, RAINIER_CPUACTLR_EL1_BIT_13
-	msr	RAINIER_CPUACTLR_EL1, x1
-	isb
-1:
-	ret	x17
-endfunc errata_n1_1868343_wa
-
-func check_errata_1868343
-	/* Applies to r0p0 of Rainier CPU */
-	mov	x1, #0x00
-	b	cpu_rev_var_ls
-endfunc check_errata_1868343
+	/* Rainier R0P0 is based on Neoverse N1 R4P0. */
+workaround_reset_start rainier, ERRATUM(1868343), ERRATA_N1_1868343
+	sysreg_bit_set RAINIER_CPUACTLR_EL1, RAINIER_CPUACTLR_EL1_BIT_13
+workaround_reset_end rainier, ERRATUM(1868343)
 
-func rainier_reset_func
-	mov	x19, x30
+check_erratum_ls rainier, ERRATUM(1868343), CPU_REV(0, 0)
 
+cpu_reset_func_start rainier
 	bl	rainier_disable_speculative_loads
-
 	/* Forces all cacheable atomic instructions to be near */
-	mrs	x0, RAINIER_CPUACTLR2_EL1
-	orr	x0, x0, #RAINIER_CPUACTLR2_EL1_BIT_2
-	msr	RAINIER_CPUACTLR2_EL1, x0
-	isb
-
-	bl	cpu_get_rev_var
-	mov	x18, x0
-
-#if ERRATA_N1_1868343
-	mov	x0, x18
-	bl	errata_n1_1868343_wa
-#endif
+	sysreg_bit_set RAINIER_CPUACTLR2_EL1, RAINIER_CPUACTLR2_EL1_BIT_2
 
 #if ENABLE_FEAT_AMU
 	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
-	mrs	x0, actlr_el3
-	orr	x0, x0, #RAINIER_ACTLR_AMEN_BIT
-	msr	actlr_el3, x0
+	sysreg_bit_set actlr_el3, RAINIER_ACTLR_AMEN_BIT
 
 	/* Make sure accesses from EL0/EL1 are not trapped to EL2 */
-	mrs	x0, actlr_el2
-	orr	x0, x0, #RAINIER_ACTLR_AMEN_BIT
-	msr	actlr_el2, x0
+	sysreg_bit_set actlr_el2, RAINIER_ACTLR_AMEN_BIT
 
 	/* Enable group0 counters */
 	mov	x0, #RAINIER_AMU_GROUP0_MASK
 	msr	CPUAMCNTENSET_EL0, x0
 #endif
-
-	isb
-	ret	x19
-endfunc rainier_reset_func
+cpu_reset_func_end rainier
 
 	/* ---------------------------------------------
 	 * HW will do the cache maintenance while powering down
@@ -123,33 +75,12 @@
 	 * Enable CPU power down bit in power control register
 	 * ---------------------------------------------
 	 */
-	mrs	x0, RAINIER_CPUPWRCTLR_EL1
-	orr	x0, x0, #RAINIER_CORE_PWRDN_EN_MASK
-	msr	RAINIER_CPUPWRCTLR_EL1, x0
+	 sysreg_bit_set RAINIER_CPUPWRCTLR_EL1, RAINIER_CORE_PWRDN_EN_MASK
 	isb
 	ret
 endfunc rainier_core_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Rainier. Must follow AAPCS.
- */
-func rainier_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_N1_1868343, rainier, 1868343
-
-	ldp	x8, x30, [sp], #16
-	ret
-endfunc rainier_errata_report
-#endif
+errata_report_shim rainier
 
 	/* ---------------------------------------------
 	 * This function provides Rainier specific
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index e16c8e4..4a80bce 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -782,6 +782,10 @@
 # erratum applies to revisions r0p0, rop1. Fixed in r0p2.
 CPU_FLAG_LIST += ERRATA_V2_2719103
 
+# Flag to apply erratum 2801372 workaround for all configurations.
+# This erratum applies to revisions r0p0, r0p1. Fixed in r0p2.
+CPU_FLAG_LIST += ERRATA_V2_2801372
+
 # Flag to apply erratum 2701951 workaround for non-arm interconnect ip.
 # This erratum applies to revisions r0p0, r1p0, and r1p1. Its is fixed in r1p2.
 CPU_FLAG_LIST += ERRATA_A715_2701951
diff --git a/lib/el3_runtime/aarch32/context_mgmt.c b/lib/el3_runtime/aarch32/context_mgmt.c
index 62e30fc..b60b8e0 100644
--- a/lib/el3_runtime/aarch32/context_mgmt.c
+++ b/lib/el3_runtime/aarch32/context_mgmt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -17,6 +17,7 @@
 #include <context.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/extensions/amu.h>
+#include <lib/extensions/pmuv3.h>
 #include <lib/extensions/sys_reg_trace.h>
 #include <lib/extensions/trf.h>
 #include <lib/utils.h>
@@ -141,13 +142,19 @@
 	}
 
 	if (is_feat_sys_reg_trace_supported()) {
-		sys_reg_trace_enable();
+		sys_reg_trace_init_el3();
 	}
 
 	if (is_feat_trf_supported()) {
-		trf_enable();
+		trf_init_el3();
 	}
-#endif
+
+	/*
+	 * Also applies to PMU < v3. The PMU is only disabled for EL3 and Secure
+	 * state execution. This does not affect lower NS ELs.
+	 */
+	pmuv3_init_el3();
+#endif /*  IMAGE_BL32 */
 }
 
 /*******************************************************************************
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 9922fb1..771fcdc 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -568,6 +568,8 @@
 	stp	x28, x29, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_X28]
 	mrs	x18, sp_el0
 	str	x18, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_SP_EL0]
+
+	/* PMUv3 is presumed to be always present */
 	mrs	x9, pmcr_el0
 	str	x9, [sp, #CTX_EL3STATE_OFFSET + CTX_PMCR_EL0]
 	/* Disable cycle counter when event counting is prohibited */
@@ -651,6 +653,8 @@
 	msr	APGAKeyLo_EL1, x8
 	msr	APGAKeyHi_EL1, x9
 #endif /* CTX_INCLUDE_PAUTH_REGS */
+
+	/* PMUv3 is presumed to be always present */
 	ldr	x0, [sp, #CTX_EL3STATE_OFFSET + CTX_PMCR_EL0]
 	msr	pmcr_el0, x0
 	ldp	x0, x1, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_X0]
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 3760b8f..9d717bb 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -24,6 +24,7 @@
 #include <lib/extensions/amu.h>
 #include <lib/extensions/brbe.h>
 #include <lib/extensions/mpam.h>
+#include <lib/extensions/pmuv3.h>
 #include <lib/extensions/sme.h>
 #include <lib/extensions/spe.h>
 #include <lib/extensions/sve.h>
@@ -37,6 +38,7 @@
 CASSERT(((TWED_DELAY & ~SCR_TWEDEL_MASK) == 0U), assert_twed_delay_value_check);
 #endif /* ENABLE_FEAT_TWED */
 
+static void manage_extensions_nonsecure(cpu_context_t *ctx);
 static void manage_extensions_secure(cpu_context_t *ctx);
 
 static void setup_el1_context(cpu_context_t *ctx, const struct entry_point_info *ep)
@@ -265,16 +267,6 @@
 	write_ctx_reg(get_el2_sysregs_ctx(ctx), CTX_ICC_SRE_EL2,
 			icc_sre_el2);
 
-	/*
-	 * Initialize MDCR_EL2.HPMN to its hardware reset value so we don't
-	 * throw anyone off who expects this to be sensible.
-	 * TODO: A similar thing happens in cm_prepare_el3_exit. They should be
-	 * unified with the proper PMU implementation
-	 */
-	u_register_t mdcr_el2 = ((read_pmcr_el0() >> PMCR_EL0_N_SHIFT) &
-			PMCR_EL0_N_MASK);
-	write_ctx_reg(get_el2_sysregs_ctx(ctx), CTX_MDCR_EL2, mdcr_el2);
-
 	if (is_feat_hcx_supported()) {
 		/*
 		 * Initialize register HCRX_EL2 with its init value.
@@ -288,6 +280,8 @@
 			HCRX_EL2_INIT_VAL);
 	}
 #endif /* CTX_INCLUDE_EL2_REGS */
+
+	manage_extensions_nonsecure(ctx);
 }
 
 /*******************************************************************************
@@ -502,21 +496,55 @@
 }
 
 /*******************************************************************************
- * Enable architecture extensions on first entry to Non-secure world.
- * When EL2 is implemented but unused `el2_unused` is non-zero, otherwise
- * it is zero.
+ * Enable architecture extensions for EL3 execution. This function only updates
+ * registers in-place which are expected to either never change or be
+ * overwritten by el3_exit.
  ******************************************************************************/
-static void manage_extensions_nonsecure(bool el2_unused, cpu_context_t *ctx)
-{
 #if IMAGE_BL31
+void cm_manage_extensions_el3(void)
+{
 	if (is_feat_spe_supported()) {
-		spe_enable(el2_unused);
+		spe_init_el3();
 	}
 
 	if (is_feat_amu_supported()) {
-		amu_enable(el2_unused, ctx);
+		amu_init_el3();
+	}
+
+	if (is_feat_sme_supported()) {
+		sme_init_el3();
 	}
 
+	if (is_feat_mpam_supported()) {
+		mpam_init_el3();
+	}
+
+	if (is_feat_trbe_supported()) {
+		trbe_init_el3();
+	}
+
+	if (is_feat_brbe_supported()) {
+		brbe_init_el3();
+	}
+
+	if (is_feat_trf_supported()) {
+		trf_init_el3();
+	}
+
+	pmuv3_init_el3();
+}
+#endif /* IMAGE_BL31 */
+
+/*******************************************************************************
+ * Enable architecture extensions on first entry to Non-secure world.
+ ******************************************************************************/
+static void manage_extensions_nonsecure(cpu_context_t *ctx)
+{
+#if IMAGE_BL31
+	if (is_feat_amu_supported()) {
+		amu_enable(ctx);
+	}
+
 	/* Enable SVE and FPU/SIMD */
 	if (is_feat_sve_supported()) {
 		sve_enable(ctx);
@@ -526,26 +554,73 @@
 		sme_enable(ctx);
 	}
 
-	if (is_feat_mpam_supported()) {
-		mpam_enable(el2_unused);
+	if (is_feat_sys_reg_trace_supported()) {
+		sys_reg_trace_enable(ctx);
 	}
 
-	if (is_feat_trbe_supported()) {
-		trbe_enable();
+	pmuv3_enable(ctx);
+#endif /* IMAGE_BL31 */
+}
+
+/* TODO: move to lib/extensions/pauth when it has been ported to FEAT_STATE */
+static __unused void enable_pauth_el2(void)
+{
+	u_register_t hcr_el2 = read_hcr_el2();
+	/*
+	 * For Armv8.3 pointer authentication feature, disable traps to EL2 when
+	 *  accessing key registers or using pointer authentication instructions
+	 *  from lower ELs.
+	 */
+	hcr_el2 |= (HCR_API_BIT | HCR_APK_BIT);
+
+	write_hcr_el2(hcr_el2);
+}
+
+/*******************************************************************************
+ * Enable architecture extensions in-place at EL2 on first entry to Non-secure
+ * world when EL2 is empty and unused.
+ ******************************************************************************/
+static void manage_extensions_nonsecure_el2_unused(void)
+{
+#if IMAGE_BL31
+	if (is_feat_spe_supported()) {
+		spe_init_el2_unused();
 	}
 
-	if (is_feat_brbe_supported()) {
-		brbe_enable();
+	if (is_feat_amu_supported()) {
+		amu_init_el2_unused();
+	}
+
+	if (is_feat_mpam_supported()) {
+		mpam_init_el2_unused();
+	}
+
+	if (is_feat_trbe_supported()) {
+		trbe_init_el2_unused();
 	}
 
 	if (is_feat_sys_reg_trace_supported()) {
-		sys_reg_trace_enable(ctx);
+		sys_reg_trace_init_el2_unused();
 	}
 
 	if (is_feat_trf_supported()) {
-		trf_enable();
+		trf_init_el2_unused();
 	}
-#endif
+
+	pmuv3_init_el2_unused();
+
+	if (is_feat_sve_supported()) {
+		sve_init_el2_unused();
+	}
+
+	if (is_feat_sme_supported()) {
+		sme_init_el2_unused();
+	}
+
+#if ENABLE_PAUTH
+	enable_pauth_el2();
+#endif /* ENABLE_PAUTH */
+#endif /* IMAGE_BL31 */
 }
 
 /*******************************************************************************
@@ -577,6 +652,7 @@
 		 * Enable SME, SVE, FPU/SIMD in secure context, secure manager
 		 * must ensure SME, SVE, and FPU/SIMD context properly managed.
 		 */
+			sme_init_el3();
 			sme_enable(ctx);
 		} else {
 		/*
@@ -586,6 +662,11 @@
 			sme_disable(ctx);
 		}
 	}
+
+	/* NS can access this but Secure shouldn't */
+	if (is_feat_sys_reg_trace_supported()) {
+		sys_reg_trace_disable(ctx);
+	}
 #endif /* IMAGE_BL31 */
 }
 
@@ -614,6 +695,109 @@
 	cm_setup_context(ctx, ep);
 }
 
+/* EL2 present but unused, need to disable safely. SCTLR_EL2 can be ignored */
+static __unused void init_nonsecure_el2_unused(cpu_context_t *ctx)
+{
+	u_register_t hcr_el2 = HCR_RESET_VAL;
+	u_register_t mdcr_el2;
+	u_register_t scr_el3;
+
+	scr_el3 = read_ctx_reg(get_el3state_ctx(ctx), CTX_SCR_EL3);
+
+	/* Set EL2 register width: Set HCR_EL2.RW to match SCR_EL3.RW */
+	if ((scr_el3 & SCR_RW_BIT) != 0U) {
+		hcr_el2 |= HCR_RW_BIT;
+	}
+
+	write_hcr_el2(hcr_el2);
+
+	/*
+	 * Initialise CPTR_EL2 setting all fields rather than relying on the hw.
+	 * All fields have architecturally UNKNOWN reset values.
+	 */
+	write_cptr_el2(CPTR_EL2_RESET_VAL);
+
+	/*
+	 * Initialise CNTHCTL_EL2. All fields are architecturally UNKNOWN on
+	 * reset and are set to zero except for field(s) listed below.
+	 *
+	 * CNTHCTL_EL2.EL1PTEN: Set to one to disable traps to Hyp mode of
+	 * Non-secure EL0 and EL1 accesses to the physical timer registers.
+	 *
+	 * CNTHCTL_EL2.EL1PCTEN: Set to one to disable traps to Hyp mode of
+	 * Non-secure EL0 and EL1 accesses to the physical counter registers.
+	 */
+	write_cnthctl_el2(CNTHCTL_RESET_VAL | EL1PCEN_BIT | EL1PCTEN_BIT);
+
+	/*
+	 * Initialise CNTVOFF_EL2 to zero as it resets to an architecturally
+	 * UNKNOWN value.
+	 */
+	write_cntvoff_el2(0);
+
+	/*
+	 * Set VPIDR_EL2 and VMPIDR_EL2 to match MIDR_EL1 and MPIDR_EL1
+	 * respectively.
+	 */
+	write_vpidr_el2(read_midr_el1());
+	write_vmpidr_el2(read_mpidr_el1());
+
+	/*
+	 * Initialise VTTBR_EL2. All fields are architecturally UNKNOWN on reset.
+	 *
+	 * VTTBR_EL2.VMID: Set to zero. Even though EL1&0 stage 2 address
+	 * translation is disabled, cache maintenance operations depend on the
+	 * VMID.
+	 *
+	 * VTTBR_EL2.BADDR: Set to zero as EL1&0 stage 2 address translation is
+	 * disabled.
+	 */
+	write_vttbr_el2(VTTBR_RESET_VAL &
+		     ~((VTTBR_VMID_MASK << VTTBR_VMID_SHIFT) |
+		       (VTTBR_BADDR_MASK << VTTBR_BADDR_SHIFT)));
+
+	/*
+	 * Initialise MDCR_EL2, setting all fields rather than relying on hw.
+	 * Some fields are architecturally UNKNOWN on reset.
+	 *
+	 * MDCR_EL2.TDRA: Set to zero so that Non-secure EL0 and EL1 System
+	 * register accesses to the Debug ROM registers are not trapped to EL2.
+	 *
+	 * MDCR_EL2.TDOSA: Set to zero so that Non-secure EL1 System register
+	 * accesses to the powerdown debug registers are not trapped to EL2.
+	 *
+	 * MDCR_EL2.TDA: Set to zero so that System register accesses to the
+	 * debug registers do not trap to EL2.
+	 *
+	 * MDCR_EL2.TDE: Set to zero so that debug exceptions are not routed to
+	 * EL2.
+	 */
+	mdcr_el2 = MDCR_EL2_RESET_VAL &
+		 ~(MDCR_EL2_TDRA_BIT | MDCR_EL2_TDOSA_BIT | MDCR_EL2_TDA_BIT |
+		   MDCR_EL2_TDE_BIT);
+
+	write_mdcr_el2(mdcr_el2);
+
+	/*
+	 * Initialise HSTR_EL2. All fields are architecturally UNKNOWN on reset.
+	 *
+	 * HSTR_EL2.T<n>: Set all these fields to zero so that Non-secure EL0 or
+	 * EL1 accesses to System registers do not trap to EL2.
+	 */
+	write_hstr_el2(HSTR_EL2_RESET_VAL & ~(HSTR_EL2_T_MASK));
+
+	/*
+	 * Initialise CNTHP_CTL_EL2. All fields are architecturally UNKNOWN on
+	 * reset.
+	 *
+	 * CNTHP_CTL_EL2:ENABLE: Set to zero to disable the EL2 physical timer
+	 * and prevent timer interrupts.
+	 */
+	write_cnthp_ctl_el2(CNTHP_CTL_RESET_VAL & ~(CNTHP_CTL_ENABLE_BIT));
+
+	manage_extensions_nonsecure_el2_unused();
+}
+
 /*******************************************************************************
  * Prepare the CPU system registers for first entry into realm, secure, or
  * normal world.
@@ -625,10 +809,8 @@
  ******************************************************************************/
 void cm_prepare_el3_exit(uint32_t security_state)
 {
-	u_register_t sctlr_elx, scr_el3, mdcr_el2;
+	u_register_t sctlr_elx, scr_el3;
 	cpu_context_t *ctx = cm_get_context(security_state);
-	bool el2_unused = false;
-	uint64_t hcr_el2 = 0U;
 
 	assert(ctx != NULL);
 
@@ -665,188 +847,8 @@
 #endif
 			write_sctlr_el2(sctlr_elx);
 		} else if (el2_implemented != EL_IMPL_NONE) {
-			el2_unused = true;
-
-			/*
-			 * EL2 present but unused, need to disable safely.
-			 * SCTLR_EL2 can be ignored in this case.
-			 *
-			 * Set EL2 register width appropriately: Set HCR_EL2
-			 * field to match SCR_EL3.RW.
-			 */
-			if ((scr_el3 & SCR_RW_BIT) != 0U)
-				hcr_el2 |= HCR_RW_BIT;
-
-			/*
-			 * For Armv8.3 pointer authentication feature, disable
-			 * traps to EL2 when accessing key registers or using
-			 * pointer authentication instructions from lower ELs.
-			 */
-			hcr_el2 |= (HCR_API_BIT | HCR_APK_BIT);
-
-			write_hcr_el2(hcr_el2);
-
-			/*
-			 * Initialise CPTR_EL2 setting all fields rather than
-			 * relying on the hw. All fields have architecturally
-			 * UNKNOWN reset values.
-			 *
-			 * CPTR_EL2.TCPAC: Set to zero so that Non-secure EL1
-			 *  accesses to the CPACR_EL1 or CPACR from both
-			 *  Execution states do not trap to EL2.
-			 *
-			 * CPTR_EL2.TTA: Set to zero so that Non-secure System
-			 *  register accesses to the trace registers from both
-			 *  Execution states do not trap to EL2.
-			 *  If PE trace unit System registers are not implemented
-			 *  then this bit is reserved, and must be set to zero.
-			 *
-			 * CPTR_EL2.TFP: Set to zero so that Non-secure accesses
-			 *  to SIMD and floating-point functionality from both
-			 *  Execution states do not trap to EL2.
-			 */
-			write_cptr_el2(CPTR_EL2_RESET_VAL &
-					~(CPTR_EL2_TCPAC_BIT | CPTR_EL2_TTA_BIT
-					| CPTR_EL2_TFP_BIT));
-
-			/*
-			 * Initialise CNTHCTL_EL2. All fields are
-			 * architecturally UNKNOWN on reset and are set to zero
-			 * except for field(s) listed below.
-			 *
-			 * CNTHCTL_EL2.EL1PTEN: Set to one to disable traps to
-			 *  Hyp mode of Non-secure EL0 and EL1 accesses to the
-			 *  physical timer registers.
-			 *
-			 * CNTHCTL_EL2.EL1PCTEN: Set to one to disable traps to
-			 *  Hyp mode of  Non-secure EL0 and EL1 accesses to the
-			 *  physical counter registers.
-			 */
-			write_cnthctl_el2(CNTHCTL_RESET_VAL |
-						EL1PCEN_BIT | EL1PCTEN_BIT);
-
-			/*
-			 * Initialise CNTVOFF_EL2 to zero as it resets to an
-			 * architecturally UNKNOWN value.
-			 */
-			write_cntvoff_el2(0);
-
-			/*
-			 * Set VPIDR_EL2 and VMPIDR_EL2 to match MIDR_EL1 and
-			 * MPIDR_EL1 respectively.
-			 */
-			write_vpidr_el2(read_midr_el1());
-			write_vmpidr_el2(read_mpidr_el1());
-
-			/*
-			 * Initialise VTTBR_EL2. All fields are architecturally
-			 * UNKNOWN on reset.
-			 *
-			 * VTTBR_EL2.VMID: Set to zero. Even though EL1&0 stage
-			 *  2 address translation is disabled, cache maintenance
-			 *  operations depend on the VMID.
-			 *
-			 * VTTBR_EL2.BADDR: Set to zero as EL1&0 stage 2 address
-			 *  translation is disabled.
-			 */
-			write_vttbr_el2(VTTBR_RESET_VAL &
-				~((VTTBR_VMID_MASK << VTTBR_VMID_SHIFT)
-				| (VTTBR_BADDR_MASK << VTTBR_BADDR_SHIFT)));
-
-			/*
-			 * Initialise MDCR_EL2, setting all fields rather than
-			 * relying on hw. Some fields are architecturally
-			 * UNKNOWN on reset.
-			 *
-			 * MDCR_EL2.HLP: Set to one so that event counter
-			 *  overflow, that is recorded in PMOVSCLR_EL0[0-30],
-			 *  occurs on the increment that changes
-			 *  PMEVCNTR<n>_EL0[63] from 1 to 0, when ARMv8.5-PMU is
-			 *  implemented. This bit is RES0 in versions of the
-			 *  architecture earlier than ARMv8.5, setting it to 1
-			 *  doesn't have any effect on them.
-			 *
-			 * MDCR_EL2.TTRF: Set to zero so that access to Trace
-			 *  Filter Control register TRFCR_EL1 at EL1 is not
-			 *  trapped to EL2. This bit is RES0 in versions of
-			 *  the architecture earlier than ARMv8.4.
-			 *
-			 * MDCR_EL2.HPMD: Set to one so that event counting is
-			 *  prohibited at EL2. This bit is RES0 in versions of
-			 *  the architecture earlier than ARMv8.1, setting it
-			 *  to 1 doesn't have any effect on them.
-			 *
-			 * MDCR_EL2.TPMS: Set to zero so that accesses to
-			 *  Statistical Profiling control registers from EL1
-			 *  do not trap to EL2. This bit is RES0 when SPE is
-			 *  not implemented.
-			 *
-			 * MDCR_EL2.TDRA: Set to zero so that Non-secure EL0 and
-			 *  EL1 System register accesses to the Debug ROM
-			 *  registers are not trapped to EL2.
-			 *
-			 * MDCR_EL2.TDOSA: Set to zero so that Non-secure EL1
-			 *  System register accesses to the powerdown debug
-			 *  registers are not trapped to EL2.
-			 *
-			 * MDCR_EL2.TDA: Set to zero so that System register
-			 *  accesses to the debug registers do not trap to EL2.
-			 *
-			 * MDCR_EL2.TDE: Set to zero so that debug exceptions
-			 *  are not routed to EL2.
-			 *
-			 * MDCR_EL2.HPME: Set to zero to disable EL2 Performance
-			 *  Monitors.
-			 *
-			 * MDCR_EL2.TPM: Set to zero so that Non-secure EL0 and
-			 *  EL1 accesses to all Performance Monitors registers
-			 *  are not trapped to EL2.
-			 *
-			 * MDCR_EL2.TPMCR: Set to zero so that Non-secure EL0
-			 *  and EL1 accesses to the PMCR_EL0 or PMCR are not
-			 *  trapped to EL2.
-			 *
-			 * MDCR_EL2.HPMN: Set to value of PMCR_EL0.N which is the
-			 *  architecturally-defined reset value.
-			 *
-			 * MDCR_EL2.E2TB: Set to zero so that the trace Buffer
-			 *  owning exception level is NS-EL1 and, tracing is
-			 *  prohibited at NS-EL2. These bits are RES0 when
-			 *  FEAT_TRBE is not implemented.
-			 */
-			mdcr_el2 = ((MDCR_EL2_RESET_VAL | MDCR_EL2_HLP |
-				     MDCR_EL2_HPMD) |
-				   ((read_pmcr_el0() & PMCR_EL0_N_BITS)
-				   >> PMCR_EL0_N_SHIFT)) &
-				   ~(MDCR_EL2_TTRF | MDCR_EL2_TPMS |
-				     MDCR_EL2_TDRA_BIT | MDCR_EL2_TDOSA_BIT |
-				     MDCR_EL2_TDA_BIT | MDCR_EL2_TDE_BIT |
-				     MDCR_EL2_HPME_BIT | MDCR_EL2_TPM_BIT |
-				     MDCR_EL2_TPMCR_BIT |
-				     MDCR_EL2_E2TB(MDCR_EL2_E2TB_EL1));
-
-			write_mdcr_el2(mdcr_el2);
-
-			/*
-			 * Initialise HSTR_EL2. All fields are architecturally
-			 * UNKNOWN on reset.
-			 *
-			 * HSTR_EL2.T<n>: Set all these fields to zero so that
-			 *  Non-secure EL0 or EL1 accesses to System registers
-			 *  do not trap to EL2.
-			 */
-			write_hstr_el2(HSTR_EL2_RESET_VAL & ~(HSTR_EL2_T_MASK));
-			/*
-			 * Initialise CNTHP_CTL_EL2. All fields are
-			 * architecturally UNKNOWN on reset.
-			 *
-			 * CNTHP_CTL_EL2:ENABLE: Set to zero to disable the EL2
-			 *  physical timer and prevent timer interrupts.
-			 */
-			write_cnthp_ctl_el2(CNTHP_CTL_RESET_VAL &
-						~(CNTHP_CTL_ENABLE_BIT));
+			init_nonsecure_el2_unused(ctx);
 		}
-		manage_extensions_nonsecure(el2_unused, ctx);
 	}
 
 	cm_el1_sysregs_context_restore(security_state);
@@ -1151,23 +1153,15 @@
 void cm_prepare_el3_exit_ns(void)
 {
 #if CTX_INCLUDE_EL2_REGS
+#if ENABLE_ASSERTIONS
 	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
+	u_register_t scr_el3 = read_ctx_reg(get_el3state_ctx(ctx), CTX_SCR_EL3);
 	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
-	 * instead of when setting up context.
-	 */
-	manage_extensions_nonsecure(0, ctx);
+#endif /* ENABLE_ASSERTIONS */
 
 	/*
 	 * Set the NS bit to be able to access the ICC_SRE_EL2
diff --git a/lib/extensions/amu/aarch64/amu.c b/lib/extensions/amu/aarch64/amu.c
index c650629..e0d3399 100644
--- a/lib/extensions/amu/aarch64/amu.c
+++ b/lib/extensions/amu/aarch64/amu.c
@@ -188,95 +188,66 @@
  * Enable counters. This function is meant to be invoked by the context
  * management library before exiting from EL3.
  */
-void amu_enable(bool el2_unused, cpu_context_t *ctx)
+void amu_enable(cpu_context_t *ctx)
 {
-	uint64_t amcfgr_el0_ncg;		/* Number of counter groups */
-	uint64_t amcgcr_el0_cg0nc;		/* Number of group 0 counters */
-
-	uint64_t amcntenset0_el0_px = 0x0;	/* Group 0 enable mask */
-	uint64_t amcntenset1_el0_px = 0x0;	/* Group 1 enable mask */
-
-	if (el2_unused) {
-		/*
-		 * CPTR_EL2.TAM: Set to zero so any accesses to the Activity
-		 * Monitor registers do not trap to EL2.
-		 */
-		write_cptr_el2_tam(0U);
-	}
-
 	/*
-	 * Retrieve and update the CPTR_EL3 value from the context mentioned
-	 * in 'ctx'. Set CPTR_EL3.TAM to zero so that any accesses to
-	 * the Activity Monitor registers do not trap to EL3.
+	 * Set CPTR_EL3.TAM to zero so that any accesses to the Activity Monitor
+	 * registers do not trap to EL3.
 	 */
 	ctx_write_cptr_el3_tam(ctx, 0U);
 
-	/*
-	 * Retrieve the number of architected counters. All of these counters
-	 * are enabled by default.
-	 */
-
-	amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
-	amcntenset0_el0_px = (UINT64_C(1) << (amcgcr_el0_cg0nc)) - 1U;
+	/* Initialize FEAT_AMUv1p1 features if present. */
+	if (is_feat_amuv1p1_supported()) {
+		/*
+		 * Set SCR_EL3.AMVOFFEN to one so that accesses to virtual
+		 * offset registers at EL2 do not trap to EL3
+		 */
+		ctx_write_scr_el3_amvoffen(ctx, 1U);
+	}
+}
 
-	assert(amcgcr_el0_cg0nc <= AMU_AMCGCR_CG0NC_MAX);
+void amu_init_el3(void)
+{
+	uint64_t group0_impl_ctr = read_amcgcr_el0_cg0nc();
+	uint64_t group0_en_mask = (1 << (group0_impl_ctr)) - 1U;
+	uint64_t num_ctr_groups = read_amcfgr_el0_ncg();
 
-	/*
-	 * The platform may opt to enable specific auxiliary counters. This can
-	 * be done via the common FCONF getter, or via the platform-implemented
-	 * function.
-	 */
+	/* Enable all architected counters by default */
+	write_amcntenset0_el0_px(group0_en_mask);
 
 #if ENABLE_AMU_AUXILIARY_COUNTERS
-	const struct amu_topology *topology;
+	if (num_ctr_groups > 0U) {
+		uint64_t amcntenset1_el0_px = 0x0; /* Group 1 enable mask */
+		const struct amu_topology *topology;
 
+		/*
+		 * The platform may opt to enable specific auxiliary counters.
+		 * This can be done via the common FCONF getter, or via the
+		 * platform-implemented function.
+		 */
 #if ENABLE_AMU_FCONF
-	topology = FCONF_GET_PROPERTY(amu, config, topology);
+		topology = FCONF_GET_PROPERTY(amu, config, topology);
 #else
-	topology = plat_amu_topology();
+		topology = plat_amu_topology();
 #endif /* ENABLE_AMU_FCONF */
 
-	if (topology != NULL) {
-		unsigned int core_pos = plat_my_core_pos();
+		if (topology != NULL) {
+			unsigned int core_pos = plat_my_core_pos();
 
-		amcntenset1_el0_px = topology->cores[core_pos].enable;
-	} else {
-		ERROR("AMU: failed to generate AMU topology\n");
-	}
-#endif /* ENABLE_AMU_AUXILIARY_COUNTERS */
-
-	/*
-	 * Enable the requested counters.
-	 */
-
-	write_amcntenset0_el0_px(amcntenset0_el0_px);
+			amcntenset1_el0_px = topology->cores[core_pos].enable;
+		} else {
+			ERROR("AMU: failed to generate AMU topology\n");
+		}
 
-	amcfgr_el0_ncg = read_amcfgr_el0_ncg();
-	if (amcfgr_el0_ncg > 0U) {
 		write_amcntenset1_el0_px(amcntenset1_el0_px);
-
-#if !ENABLE_AMU_AUXILIARY_COUNTERS
+	}
+#else /* ENABLE_AMU_AUXILIARY_COUNTERS */
+	if (num_ctr_groups > 0U) {
 		VERBOSE("AMU: auxiliary counters detected but support is disabled\n");
-#endif
 	}
+#endif /* ENABLE_AMU_AUXILIARY_COUNTERS */
 
-	/* Initialize FEAT_AMUv1p1 features if present. */
 	if (is_feat_amuv1p1_supported()) {
-		if (el2_unused) {
-			/*
-			 * Make sure virtual offsets are disabled if EL2 not
-			 * 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
 		/*
 		 * FEAT_AMUv1p1 adds a register field to restrict access to
@@ -297,6 +268,21 @@
 #endif
 }
 
+void amu_init_el2_unused(void)
+{
+	/*
+	 * CPTR_EL2.TAM: Set to zero so any accesses to the Activity Monitor
+	 *  registers do not trap to EL2.
+	 */
+	write_cptr_el2_tam(0U);
+
+	/* Initialize FEAT_AMUv1p1 features if present. */
+	if (is_feat_amuv1p1_supported()) {
+		/* Make sure virtual offsets are disabled if EL2 not used. */
+		write_hcr_el2_amvoffen(0U);
+	}
+}
+
 /* Read the group 0 counter identified by the given `idx`. */
 static uint64_t amu_group0_cnt_read(unsigned int idx)
 {
@@ -526,10 +512,10 @@
 
 	uint64_t hcr_el2_amvoffen = 0;	/* AMU virtual offsets enabled */
 
-	uint64_t amcfgr_el0_ncg;	/* Number of counter groups */
 	uint64_t amcgcr_el0_cg0nc;	/* Number of group 0 counters */
 
 #if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint64_t amcfgr_el0_ncg;	/* Number of counter groups */
 	uint64_t amcgcr_el0_cg1nc;	/* Number of group 1 counters */
 	uint64_t amcg1idr_el0_voff;	/* Auxiliary counters with virtual offsets */
 #endif
@@ -541,7 +527,6 @@
 	core_pos = plat_my_core_pos();
 	ctx = &amu_ctxs_[core_pos];
 
-	amcfgr_el0_ncg = read_amcfgr_el0_ncg();
 	amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
 
 	if (is_feat_amuv1p1_supported()) {
@@ -549,22 +534,12 @@
 	}
 
 #if ENABLE_AMU_AUXILIARY_COUNTERS
+	amcfgr_el0_ncg = read_amcfgr_el0_ncg();
 	amcgcr_el0_cg1nc = (amcfgr_el0_ncg > 0U) ? read_amcgcr_el0_cg1nc() : 0U;
 	amcg1idr_el0_voff = (hcr_el2_amvoffen != 0U) ? read_amcg1idr_el0_voff() : 0U;
 #endif
 
 	/*
-	 * Sanity check that all counters were disabled when the context was
-	 * previously saved.
-	 */
-
-	assert(read_amcntenset0_el0_px() == 0U);
-
-	if (amcfgr_el0_ncg > 0U) {
-		assert(read_amcntenset1_el0_px() == 0U);
-	}
-
-	/*
 	 * Restore the counter values from the local context.
 	 */
 
diff --git a/lib/extensions/brbe/brbe.c b/lib/extensions/brbe/brbe.c
index 329cf98..37bd834 100644
--- a/lib/extensions/brbe/brbe.c
+++ b/lib/extensions/brbe/brbe.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,8 +7,9 @@
 #include <arch.h>
 #include <arch_features.h>
 #include <arch_helpers.h>
+#include <lib/extensions/brbe.h>
 
-void brbe_enable(void)
+void brbe_init_el3(void)
 {
 	uint64_t val;
 
diff --git a/lib/extensions/mpam/mpam.c b/lib/extensions/mpam/mpam.c
index 62533fc..6462c97 100644
--- a/lib/extensions/mpam/mpam.c
+++ b/lib/extensions/mpam/mpam.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -11,7 +11,7 @@
 #include <arch_helpers.h>
 #include <lib/extensions/mpam.h>
 
-void mpam_enable(bool el2_unused)
+void mpam_init_el3(void)
 {
 	/*
 	 * Enable MPAM, and disable trapping to EL3 when lower ELs access their
@@ -19,15 +19,18 @@
 	 */
 	write_mpam3_el3(MPAM3_EL3_MPAMEN_BIT);
 
-	/*
-	 * If EL2 is implemented but unused, disable trapping to EL2 when lower
-	 * ELs access their own MPAM registers.
-	 */
-	if (el2_unused) {
-		write_mpam2_el2(0ULL);
+}
+
+/*
+ * If EL2 is implemented but unused, disable trapping to EL2 when lower ELs
+ * access their own MPAM registers.
+ */
+void mpam_init_el2_unused(void)
+{
+	write_mpam2_el2(0ULL);
 
-		if ((read_mpamidr_el1() & MPAMIDR_HAS_HCR_BIT) != 0U) {
-			write_mpamhcr_el2(0ULL);
-		}
+	if ((read_mpamidr_el1() & MPAMIDR_HAS_HCR_BIT) != 0U) {
+		write_mpamhcr_el2(0ULL);
 	}
+
 }
diff --git a/lib/extensions/mtpmu/aarch32/mtpmu.S b/lib/extensions/mtpmu/aarch32/mtpmu.S
deleted file mode 100644
index 834cee3..0000000
--- a/lib/extensions/mtpmu/aarch32/mtpmu.S
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-
-	.global	mtpmu_disable
-
-/* -------------------------------------------------------------
- * The functions in this file are called at entrypoint, before
- * the CPU has decided whether this is a cold or a warm boot.
- * Therefore there are no stack yet to rely on for a C function
- * call.
- * -------------------------------------------------------------
- */
-
-/*
- * bool mtpmu_supported(void)
- *
- * Return a boolean indicating whether FEAT_MTPMU is supported or not.
- *
- * Trash registers: r0.
- */
-func mtpmu_supported
-	ldcopr	r0, ID_DFR1
-	and	r0, r0, #(ID_DFR1_MTPMU_MASK >> ID_DFR1_MTPMU_SHIFT)
-	cmp	r0, #ID_DFR1_MTPMU_SUPPORTED
-	mov	r0, #0
-	addeq	r0, r0, #1
-	bx	lr
-endfunc mtpmu_supported
-
-/*
- * bool el_implemented(unsigned int el)
- *
- * Return a boolean indicating if the specified EL (2 or 3) is implemented.
- *
- * Trash registers: r0
- */
-func el_implemented
-	cmp	r0, #3
-	ldcopr	r0, ID_PFR1
-	lsreq	r0, r0, #ID_PFR1_SEC_SHIFT
-	lsrne	r0, r0, #ID_PFR1_VIRTEXT_SHIFT
-	/*
-	 * ID_PFR1_VIRTEXT_MASK is the same as ID_PFR1_SEC_MASK
-	 * so use any one of them
-	 */
-	and	r0, r0, #ID_PFR1_VIRTEXT_MASK
-	cmp	r0, #ID_PFR1_ELx_ENABLED
-	mov	r0, #0
-	addeq	r0, r0, #1
-	bx	lr
-endfunc el_implemented
-
-/*
- * void mtpmu_disable(void)
- *
- * Disable mtpmu feature if supported.
- *
- * Trash register: r0, r1, r2
- */
-func mtpmu_disable
-	mov	r2, lr
-	bl	mtpmu_supported
-	cmp	r0, #0
-	bxeq	r2	/* FEAT_MTPMU not supported */
-
-	/* FEAT_MTMPU Supported */
-	mov	r0, #3
-	bl	el_implemented
-	cmp	r0, #0
-	beq	1f
-
-	/* EL3 implemented */
-	ldcopr	r0, SDCR
-	ldr	r1, =SDCR_MTPME_BIT
-	bic	r0, r0, r1
-	stcopr	r0, SDCR
-
-	/*
-	 * If EL3 is implemented, HDCR.MTPME is implemented as Res0 and
-	 * FEAT_MTPMU is controlled only from EL3, so no need to perform
-	 * any operations for EL2.
-	 */
-	isb
-	bx	r2
-1:
-	/* EL3 not implemented */
-	mov	r0, #2
-	bl	el_implemented
-	cmp	r0, #0
-	bxeq	r2	/* No EL2 or EL3 implemented */
-
-	/* EL2 implemented */
-	ldcopr	r0, HDCR
-	ldr	r1, =HDCR_MTPME_BIT
-	orr	r0, r0, r1
-	stcopr	r0, HDCR
-	isb
-	bx	r2
-endfunc mtpmu_disable
diff --git a/lib/extensions/mtpmu/aarch64/mtpmu.S b/lib/extensions/mtpmu/aarch64/mtpmu.S
deleted file mode 100644
index 0a1d57b..0000000
--- a/lib/extensions/mtpmu/aarch64/mtpmu.S
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-
-	.global	mtpmu_disable
-
-/* -------------------------------------------------------------
- * The functions in this file are called at entrypoint, before
- * the CPU has decided whether this is a cold or a warm boot.
- * Therefore there are no stack yet to rely on for a C function
- * call.
- * -------------------------------------------------------------
- */
-
-/*
- * bool mtpmu_supported(void)
- *
- * Return a boolean indicating whether FEAT_MTPMU is supported or not.
- *
- * Trash registers: x0, x1
- */
-func mtpmu_supported
-	mrs	x0, id_aa64dfr0_el1
-	mov_imm	x1, ID_AA64DFR0_MTPMU_MASK
-	and	x0, x1, x0, LSR #ID_AA64DFR0_MTPMU_SHIFT
-	cmp	x0, ID_AA64DFR0_MTPMU_SUPPORTED
-	cset	x0, eq
-	ret
-endfunc mtpmu_supported
-
-/*
- * bool el_implemented(unsigned int el_shift)
- *
- * Return a boolean indicating if the specified EL is implemented.
- * The EL is represented as the bitmask shift on id_aa64pfr0_el1 register.
- *
- * Trash registers: x0, x1
- */
-func el_implemented
-	mrs	x1, id_aa64pfr0_el1
-	lsr	x1, x1, x0
-	cmp	x1, #ID_AA64PFR0_ELX_MASK
-	cset	x0, eq
-	ret
-endfunc el_implemented
-
-/*
- * void mtpmu_disable(void)
- *
- * Disable mtpmu feature if supported.
- *
- * Trash register: x0, x1, x30
- */
-func mtpmu_disable
-	mov	x10, x30
-	bl	mtpmu_supported
-	cbz	x0, exit_disable
-
-	/* FEAT_MTMPU Supported */
-	mov_imm	x0, ID_AA64PFR0_EL3_SHIFT
-	bl	el_implemented
-	cbz	x0, 1f
-
-	/* EL3 implemented */
-	mrs	x0, mdcr_el3
-	mov_imm x1, MDCR_MTPME_BIT
-	bic	x0, x0, x1
-	msr	mdcr_el3, x0
-
-	/*
-	 * If EL3 is implemented, MDCR_EL2.MTPME is implemented as Res0 and
-	 * FEAT_MTPMU is controlled only from EL3, so no need to perform
-	 * any operations for EL2.
-	 */
-	isb
-exit_disable:
-	ret	x10
-1:
-	/* EL3 not implemented */
-	mov_imm	x0, ID_AA64PFR0_EL2_SHIFT
-	bl	el_implemented
-	cbz	x0, exit_disable
-
-	/* EL2 implemented */
-	mrs	x0, mdcr_el2
-	mov_imm x1, MDCR_EL2_MTPME
-	bic	x0, x0, x1
-	msr	mdcr_el2, x0
-	isb
-	ret	x10
-endfunc mtpmu_disable
diff --git a/lib/extensions/pmuv3/aarch32/pmuv3.c b/lib/extensions/pmuv3/aarch32/pmuv3.c
new file mode 100644
index 0000000..effb7e0
--- /dev/null
+++ b/lib/extensions/pmuv3/aarch32/pmuv3.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <lib/extensions/pmuv3.h>
+
+static u_register_t mtpmu_disable_el3(u_register_t sdcr)
+{
+	if (!is_feat_mtpmu_supported()) {
+		return sdcr;
+	}
+
+	/*
+	 * SDCR.MTPME = 0
+	 * FEAT_MTPMU is disabled. The Effective value of PMEVTYPER<n>.MT is
+	 * zero.
+	 */
+	sdcr &= ~SDCR_MTPME_BIT;
+
+	return sdcr;
+}
+
+/*
+ * Applies to all PMU versions. Name is PMUv3 for compatibility with aarch64 and
+ * to not clash with platforms which reuse the PMU name
+ */
+void pmuv3_init_el3(void)
+{
+	u_register_t sdcr = read_sdcr();
+
+	/* ---------------------------------------------------------------------
+	 * Initialise SDCR, setting all the fields rather than relying on hw.
+	 *
+	 * SDCR.SCCD: Set to one so that cycle counting by PMCCNTR is prohibited
+	 *  in Secure state. This bit is RES0 in versions of the architecture
+	 *  earlier than ARMv8.5
+	 *
+	 * SDCR.SPME: Set to zero so that event counting is prohibited in Secure
+	 *  state (and explicitly EL3 with later revisions). If ARMv8.2 Debug is
+	 *  not implemented this bit does not have any effect on the counters
+	 *  unless there is support for the implementation defined
+	 *  authentication interface ExternalSecureNoninvasiveDebugEnabled().
+	 * ---------------------------------------------------------------------
+	 */
+	sdcr = (sdcr | SDCR_SCCD_BIT) & ~SDCR_SPME_BIT;
+	sdcr = mtpmu_disable_el3(sdcr);
+	write_sdcr(sdcr);
+
+	/* ---------------------------------------------------------------------
+	 * Initialise PMCR, setting all fields rather than relying
+	 * on hw. Some fields are architecturally UNKNOWN on reset.
+	 *
+	 * PMCR.DP: Set to one to prohibit cycle counting whilst in Secure mode.
+	 *
+	 * PMCR.X: Set to zero to disable export of events.
+	 *
+	 * PMCR.C: Set to one to reset PMCCNTR.
+	 *
+	 * PMCR.P: Set to one to reset each event counter PMEVCNTR<n> to zero.
+	 *
+	 * PMCR.E: Set to zero to disable cycle and event counters.
+	 * ---------------------------------------------------------------------
+	 */
+
+	write_pmcr(read_pmcr() | PMCR_DP_BIT | PMCR_C_BIT | PMCR_P_BIT |
+		 ~(PMCR_X_BIT | PMCR_E_BIT));
+}
diff --git a/lib/extensions/pmuv3/aarch64/pmuv3.c b/lib/extensions/pmuv3/aarch64/pmuv3.c
new file mode 100644
index 0000000..61fc47d
--- /dev/null
+++ b/lib/extensions/pmuv3/aarch64/pmuv3.c
@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <lib/extensions/pmuv3.h>
+
+static u_register_t init_mdcr_el2_hpmn(u_register_t mdcr_el2)
+{
+	/*
+	 * Initialize MDCR_EL2.HPMN to its hardware reset value so we don't
+	 * throw anyone off who expects this to be sensible.
+	 */
+	mdcr_el2 &= ~MDCR_EL2_HPMN_MASK;
+	mdcr_el2 |= ((read_pmcr_el0() >> PMCR_EL0_N_SHIFT) & PMCR_EL0_N_MASK);
+
+	return mdcr_el2;
+}
+
+void pmuv3_enable(cpu_context_t *ctx)
+{
+#if CTX_INCLUDE_EL2_REGS
+	u_register_t mdcr_el2;
+
+	mdcr_el2 = read_ctx_reg(get_el2_sysregs_ctx(ctx), CTX_MDCR_EL2);
+	mdcr_el2 = init_mdcr_el2_hpmn(mdcr_el2);
+	write_ctx_reg(get_el2_sysregs_ctx(ctx), CTX_MDCR_EL2, mdcr_el2);
+#endif /* CTX_INCLUDE_EL2_REGS */
+}
+
+static u_register_t mtpmu_disable_el3(u_register_t mdcr_el3)
+{
+	if (!is_feat_mtpmu_supported()) {
+		return mdcr_el3;
+	}
+
+	/*
+	 * MDCR_EL3.MTPME = 0
+	 * FEAT_MTPMU is disabled. The Effective value of PMEVTYPER<n>_EL0.MT is
+	 * zero.
+	 */
+	mdcr_el3 &= ~MDCR_MTPME_BIT;
+
+	return mdcr_el3;
+}
+
+void pmuv3_init_el3(void)
+{
+	u_register_t mdcr_el3 = read_mdcr_el3();
+
+	/* ---------------------------------------------------------------------
+	 * Initialise MDCR_EL3, setting all fields rather than relying on hw.
+	 * Some fields are architecturally UNKNOWN on reset.
+	 *
+	 * MDCR_EL3.MPMX: Set to zero to not affect event counters (when
+	 * SPME = 0).
+	 *
+	 * MDCR_EL3.MCCD: Set to one so that cycle counting by PMCCNTR_EL0 is
+	 *  prohibited in EL3. This bit is RES0 in versions of the
+	 *  architecture with FEAT_PMUv3p7 not implemented.
+	 *
+	 * MDCR_EL3.SCCD: Set to one so that cycle counting by PMCCNTR_EL0 is
+	 *  prohibited in Secure state. This bit is RES0 in versions of the
+	 *  architecture with FEAT_PMUv3p5 not implemented.
+	 *
+	 * MDCR_EL3.SPME: Set to zero so that event counting is prohibited in
+	 *  Secure state (and explicitly EL3 with later revisions). If ARMv8.2
+	 *  Debug is not implemented this bit does not have any effect on the
+	 *  counters unless there is support for the implementation defined
+	 *  authentication interface ExternalSecureNoninvasiveDebugEnabled().
+	 *
+	 * The SPME/MPMX combination is a little tricky. Below is a small
+	 * summary if another combination is ever needed:
+	 * SPME | MPMX | secure world |   EL3
+	 * -------------------------------------
+	 *   0  |  0   |    disabled  | disabled
+	 *   1  |  0   |    enabled   | enabled
+	 *   0  |  1   |    enabled   | disabled
+	 *   1  |  1   |    enabled   | disabled only for counters 0 to
+	 *                              MDCR_EL2.HPMN - 1. Enabled for the rest
+	 *
+	 * MDCR_EL3.TPM: Set to zero so that EL0, EL1, and EL2 System register
+	 *  accesses to all Performance Monitors registers do not trap to EL3.
+	 */
+	mdcr_el3 = (mdcr_el3 | MDCR_SCCD_BIT | MDCR_MCCD_BIT) &
+		  ~(MDCR_MPMX_BIT | MDCR_SPME_BIT | MDCR_TPM_BIT);
+	mdcr_el3 = mtpmu_disable_el3(mdcr_el3);
+	write_mdcr_el3(mdcr_el3);
+
+	/* ---------------------------------------------------------------------
+	 * Initialise PMCR_EL0 setting all fields rather than relying
+	 * on hw. Some fields are architecturally UNKNOWN on reset.
+	 *
+	 * PMCR_EL0.DP: Set to one so that the cycle counter,
+	 *  PMCCNTR_EL0 does not count when event counting is prohibited.
+	 *  Necessary on PMUv3 <= p7 where MDCR_EL3.{SCCD,MCCD} are not
+	 *  available
+	 *
+	 * PMCR_EL0.X: Set to zero to disable export of events.
+	 *
+	 * PMCR_EL0.C: Set to one to reset PMCCNTR_EL0 to zero.
+	 *
+	 * PMCR_EL0.P: Set to one to reset each event counter PMEVCNTR<n>_EL0 to
+	 *  zero.
+	 *
+	 * PMCR_EL0.E: Set to zero to disable cycle and event counters.
+	 * ---------------------------------------------------------------------
+	 */
+	write_pmcr_el0((read_pmcr_el0() | PMCR_EL0_DP_BIT | PMCR_EL0_C_BIT |
+			PMCR_EL0_P_BIT) & ~(PMCR_EL0_X_BIT | PMCR_EL0_E_BIT));
+}
+
+static u_register_t mtpmu_disable_el2(u_register_t mdcr_el2)
+{
+	if (!is_feat_mtpmu_supported()) {
+		return mdcr_el2;
+	}
+
+	/*
+	 * MDCR_EL2.MTPME = 0
+	 * FEAT_MTPMU is disabled. The Effective value of PMEVTYPER<n>_EL0.MT is
+	 * zero.
+	 */
+	mdcr_el2 &= ~MDCR_EL2_MTPME;
+
+	return mdcr_el2;
+}
+
+void pmuv3_init_el2_unused(void)
+{
+	u_register_t mdcr_el2 = read_mdcr_el2();
+
+	/*
+	 * Initialise MDCR_EL2, setting all fields rather than
+	 * relying on hw. Some fields are architecturally
+	 * UNKNOWN on reset.
+	 *
+	 * MDCR_EL2.HLP: Set to one so that event counter overflow, that is
+	 *  recorded in PMOVSCLR_EL0[0-30], occurs on the increment that changes
+	 *  PMEVCNTR<n>_EL0[63] from 1 to 0, when ARMv8.5-PMU is implemented.
+	 *  This bit is RES0 in versions of the architecture earlier than
+	 *  ARMv8.5, setting it to 1 doesn't have any effect on them.
+	 *
+	 * MDCR_EL2.HCCD: Set to one to prohibit cycle counting at EL2. This bit
+	 *  is RES0 in versions of the architecture with FEAT_PMUv3p5 not
+	 *  implemented.
+	 *
+	 * MDCR_EL2.HPMD: Set to one so that event counting is
+	 *  prohibited at EL2 for counter n < MDCR_EL2.HPMN. This bit  is RES0
+	 *  in versions of the architecture with FEAT_PMUv3p1 not implemented.
+	 *
+	 * MDCR_EL2.HPME: Set to zero to disable event counters for counters
+	 *  n >= MDCR_EL2.HPMN.
+	 *
+	 * MDCR_EL2.TPM: Set to zero so that Non-secure EL0 and
+	 *  EL1 accesses to all Performance Monitors registers
+	 *  are not trapped to EL2.
+	 *
+	 * MDCR_EL2.TPMCR: Set to zero so that Non-secure EL0
+	 *  and EL1 accesses to the PMCR_EL0 or PMCR are not
+	 *  trapped to EL2.
+	 */
+	mdcr_el2 = (mdcr_el2 | MDCR_EL2_HLP_BIT | MDCR_EL2_HPMD_BIT |
+		    MDCR_EL2_HCCD_BIT) &
+		  ~(MDCR_EL2_HPME_BIT | MDCR_EL2_TPM_BIT | MDCR_EL2_TPMCR_BIT);
+	mdcr_el2 = init_mdcr_el2_hpmn(mdcr_el2);
+	mdcr_el2 = mtpmu_disable_el2(mdcr_el2);
+	write_mdcr_el2(mdcr_el2);
+}
diff --git a/lib/extensions/sme/sme.c b/lib/extensions/sme/sme.c
index 3423dba..d705b64 100644
--- a/lib/extensions/sme/sme.c
+++ b/lib/extensions/sme/sme.c
@@ -17,7 +17,6 @@
 void sme_enable(cpu_context_t *context)
 {
 	u_register_t reg;
-	u_register_t cptr_el3;
 	el3_state_t *state;
 
 	/* Get the context state. */
@@ -32,9 +31,14 @@
 	reg = read_ctx_reg(state, CTX_SCR_EL3);
 	reg |= SCR_ENTP2_BIT;
 	write_ctx_reg(state, CTX_SCR_EL3, reg);
+}
 
-	/* Set CPTR_EL3.ESM bit so we can write SMCR_EL3 without trapping. */
-	cptr_el3 = read_cptr_el3();
+void sme_init_el3(void)
+{
+	u_register_t cptr_el3 = read_cptr_el3();
+	u_register_t smcr_el3;
+
+	/* Set CPTR_EL3.ESM bit so we can access SMCR_EL3 without trapping. */
 	write_cptr_el3(cptr_el3 | ESM_BIT);
 	isb();
 
@@ -43,11 +47,10 @@
 	 * to be the least restrictive, then lower ELs can restrict as needed
 	 * using SMCR_EL2 and SMCR_EL1.
 	 */
-	reg = SMCR_ELX_LEN_MAX;
-
+	smcr_el3 = SMCR_ELX_LEN_MAX;
 	if (read_feat_sme_fa64_id_field() != 0U) {
 		VERBOSE("[SME] FA64 enabled\n");
-		reg |= SMCR_ELX_FA64_BIT;
+		smcr_el3 |= SMCR_ELX_FA64_BIT;
 	}
 
 	/*
@@ -58,15 +61,24 @@
 	 */
 	if (is_feat_sme2_supported()) {
 		VERBOSE("SME2 enabled\n");
-		reg |= SMCR_ELX_EZT0_BIT;
+		smcr_el3 |= SMCR_ELX_EZT0_BIT;
 	}
-	write_smcr_el3(reg);
+	write_smcr_el3(smcr_el3);
 
 	/* Reset CPTR_EL3 value. */
 	write_cptr_el3(cptr_el3);
 	isb();
 }
 
+void sme_init_el2_unused(void)
+{
+	/*
+	 * CPTR_EL2.TCPAC: Set to zero so that Non-secure EL1 accesses to the
+	 *  CPACR_EL1 or CPACR from both Execution states do not trap to EL2.
+	 */
+	write_cptr_el2(read_cptr_el2() & ~CPTR_EL2_TCPAC_BIT);
+}
+
 void sme_disable(cpu_context_t *context)
 {
 	u_register_t reg;
diff --git a/lib/extensions/spe/spe.c b/lib/extensions/spe/spe.c
index b1fe39f..2c25a9d 100644
--- a/lib/extensions/spe/spe.c
+++ b/lib/extensions/spe/spe.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -21,30 +21,18 @@
 	__asm__ volatile("hint #17");
 }
 
-void spe_enable(bool el2_unused)
+void spe_init_el3(void)
 {
 	uint64_t v;
 
-	if (el2_unused) {
-		/*
-		 * MDCR_EL2.TPMS (ARM v8.2): Do not trap statistical
-		 * profiling controls to EL2.
-		 *
-		 * MDCR_EL2.E2PB (ARM v8.2): SPE enabled in Non-secure
-		 * state. Accesses to profiling buffer controls at
-		 * Non-secure EL1 are not trapped to EL2.
-		 */
-		v = read_mdcr_el2();
-		v &= ~MDCR_EL2_TPMS;
-		v |= MDCR_EL2_E2PB(MDCR_EL2_E2PB_EL1);
-		write_mdcr_el2(v);
-	}
-
 	/*
-	 * MDCR_EL2.NSPB (ARM v8.2): SPE enabled in Non-secure state
+	 * MDCR_EL3.NSPB (ARM v8.2): SPE enabled in Non-secure state
 	 * and disabled in secure state. Accesses to SPE registers at
 	 * S-EL1 generate trap exceptions to EL3.
 	 *
+	 * MDCR_EL3.NSPBE: Profiling Buffer uses Non-secure Virtual Addresses.
+	 * When FEAT_RME is not implemented, this field is RES0.
+	 *
 	 * MDCR_EL3.EnPMSN (ARM v8.7): Do not trap access to PMSNEVFR_EL1
 	 * register at NS-EL1 or NS-EL2 to EL3 if FEAT_SPEv1p2 is implemented.
 	 * Setting this bit to 1 doesn't have any effect on it when
@@ -52,9 +40,28 @@
 	 */
 	v = read_mdcr_el3();
 	v |= MDCR_NSPB(MDCR_NSPB_EL1) | MDCR_EnPMSN_BIT;
+	v &= ~(MDCR_NSPBE_BIT);
 	write_mdcr_el3(v);
 }
 
+void spe_init_el2_unused(void)
+{
+	uint64_t v;
+
+	/*
+	 * MDCR_EL2.TPMS (ARM v8.2): Do not trap statistical
+	 * profiling controls to EL2.
+	 *
+	 * MDCR_EL2.E2PB (ARM v8.2): SPE enabled in Non-secure
+	 * state. Accesses to profiling buffer controls at
+	 * Non-secure EL1 are not trapped to EL2.
+	 */
+	v = read_mdcr_el2();
+	v &= ~MDCR_EL2_TPMS;
+	v |= MDCR_EL2_E2PB(MDCR_EL2_E2PB_EL1);
+	write_mdcr_el2(v);
+}
+
 void spe_disable(void)
 {
 	uint64_t v;
diff --git a/lib/extensions/sve/sve.c b/lib/extensions/sve/sve.c
index f551ca7..eb4ac8d 100644
--- a/lib/extensions/sve/sve.c
+++ b/lib/extensions/sve/sve.c
@@ -37,6 +37,16 @@
 		(ZCR_EL3_LEN_MASK & CONVERT_SVE_LENGTH(SVE_VECTOR_LEN)));
 }
 
+void sve_init_el2_unused(void)
+{
+	/*
+	 * CPTR_EL2.TFP: Set to zero so that Non-secure accesses to Advanced
+	 *  SIMD and floating-point functionality from both Execution states do
+	 *  not trap to EL2.
+	 */
+	write_cptr_el2(read_cptr_el2() & ~CPTR_EL2_TFP_BIT);
+}
+
 void sve_disable(cpu_context_t *context)
 {
 	u_register_t reg;
diff --git a/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
index b3f44b7..6da504e 100644
--- a/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
+++ b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,7 +10,7 @@
 #include <arch_helpers.h>
 #include <lib/extensions/sys_reg_trace.h>
 
-void sys_reg_trace_enable(void)
+void sys_reg_trace_init_el3(void)
 {
 	uint32_t val;
 
diff --git a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
index e61cb90..1349566 100644
--- a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
+++ b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -12,15 +12,36 @@
 
 void sys_reg_trace_enable(cpu_context_t *ctx)
 {
-	uint64_t val;
+	/*
+	 * CPTR_EL3.TTA: Set to zero so that System register accesses to the
+	 *  trace registers do not trap to EL3.
+	 */
+	uint64_t val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
+
+	val &= ~(TTA_BIT);
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, val);
+}
 
-	/* Retrieve CPTR_EL3 value from the given context 'ctx',
-	 * and update CPTR_EL3.TTA bit to 0.
-	 * This function is called while switching context to NS to
-	 * allow system trace register access to NS-EL2 and NS-EL1
-	 * when NS-EL2 is implemented but not used.
+void sys_reg_trace_disable(cpu_context_t *ctx)
+{
+	/*
+	 * CPTR_EL3.TTA: Set to one so that System register accesses to the
+	 *  trace registers trap to EL3, unless it is trapped by CPACR.TRCDIS,
+	 *  CPACR_EL1.TTA, or CPTR_EL2.TTA
 	 */
-	val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
-	val &= ~TTA_BIT;
+	uint64_t val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
+
+	val |= TTA_BIT;
 	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, val);
 }
+
+void sys_reg_trace_init_el2_unused(void)
+{
+	/*
+	 * CPTR_EL2.TTA: Set to zero so that Non-secure System register accesses
+	 *  to the trace registers from both Execution states do not trap to
+	 *  EL2. If PE trace unit System registers are not implemented then this
+	 *  bit is reserved, and must be set to zero.
+	 */
+	write_cptr_el2(read_cptr_el2() & ~CPTR_EL2_TTA_BIT);
+}
diff --git a/lib/extensions/trbe/trbe.c b/lib/extensions/trbe/trbe.c
index fa139ca..d4fbdfb 100644
--- a/lib/extensions/trbe/trbe.c
+++ b/lib/extensions/trbe/trbe.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,21 +19,37 @@
 	__asm__ volatile("hint #18");
 }
 
-void trbe_enable(void)
+void trbe_init_el3(void)
 {
-	uint64_t val;
+	u_register_t val;
 
 	/*
+	 * MDCR_EL3.NSTBE = 0b0
+	 *  Trace Buffer owning Security state is Non-secure state. If FEAT_RME
+	 *  is not implemented, this field is RES0.
+	 *
 	 * MDCR_EL3.NSTB = 0b11
-	 * Allow access of trace buffer control registers from NS-EL1
-	 * and NS-EL2, tracing is prohibited in Secure and Realm state
-	 * (if implemented).
+	 *  Allow access of trace buffer control registers from NS-EL1 and
+	 *  NS-EL2, tracing is prohibited in Secure and Realm state (if
+	 *  implemented).
 	 */
 	val = read_mdcr_el3();
 	val |= MDCR_NSTB(MDCR_NSTB_EL1);
+	val &= ~(MDCR_NSTBE_BIT);
 	write_mdcr_el3(val);
 }
 
+void trbe_init_el2_unused(void)
+{
+	/*
+	 * MDCR_EL2.E2TB: Set to zero so that the trace Buffer
+	 *  owning exception level is NS-EL1 and, tracing is
+	 *  prohibited at NS-EL2. These bits are RES0 when
+	 *  FEAT_TRBE is not implemented.
+	 */
+	write_mdcr_el2(read_mdcr_el2() & ~MDCR_EL2_E2TB(MDCR_EL2_E2TB_EL1));
+}
+
 static void *trbe_drain_trace_buffers_hook(const void *arg __unused)
 {
 	if (is_feat_trbe_supported()) {
diff --git a/lib/extensions/trf/aarch32/trf.c b/lib/extensions/trf/aarch32/trf.c
index 0c63efa..321c089 100644
--- a/lib/extensions/trf/aarch32/trf.c
+++ b/lib/extensions/trf/aarch32/trf.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,15 +10,15 @@
 #include <arch_helpers.h>
 #include <lib/extensions/trf.h>
 
-void trf_enable(void)
+void trf_init_el3(void)
 {
 	uint32_t val;
 
 	/*
 	 * Allow access of trace filter control registers from
-	 * non-monitor mode
+	 * non-monitor mode.
 	 */
 	val = read_sdcr();
-	val &= ~SDCR_TTRF_BIT;
+	val &= ~(SDCR_TTRF_BIT);
 	write_sdcr(val);
 }
diff --git a/lib/extensions/trf/aarch64/trf.c b/lib/extensions/trf/aarch64/trf.c
index 941692b..83fbf85 100644
--- a/lib/extensions/trf/aarch64/trf.c
+++ b/lib/extensions/trf/aarch64/trf.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,16 +9,32 @@
 #include <arch_helpers.h>
 #include <lib/extensions/trf.h>
 
-void trf_enable(void)
+void trf_init_el3(void)
 {
-	uint64_t val;
+	u_register_t val;
 
 	/*
+	 * MDCR_EL3.STE = b0
+	 * Trace prohibited in Secure state unless overridden by the
+	 * IMPLEMENTATION DEFINED authentication interface.
+	 *
 	 * MDCR_EL3.TTRF = b0
 	 * Allow access of trace filter control registers from NS-EL2
 	 * and NS-EL1 when NS-EL2 is implemented but not used
 	 */
 	val = read_mdcr_el3();
-	val &= ~MDCR_TTRF_BIT;
+	val &= ~(MDCR_STE_BIT | MDCR_TTRF_BIT);
 	write_mdcr_el3(val);
 }
+
+void trf_init_el2_unused(void)
+{
+	/*
+	 * MDCR_EL2.TTRF: Set to zero so that access to Trace
+	 *  Filter Control register TRFCR_EL1 at EL1 is not
+	 *  trapped to EL2. This bit is RES0 in versions of
+	 *  the architecture earlier than ARMv8.4.
+	 *
+	 */
+	write_mdcr_el2(read_mdcr_el2() & ~MDCR_EL2_TTRF);
+}
diff --git a/lib/libc/libc.mk b/lib/libc/libc.mk
index b75d09c..5eb8b52 100644
--- a/lib/libc/libc.mk
+++ b/lib/libc/libc.mk
@@ -11,6 +11,7 @@
 			memchr.c			\
 			memcmp.c			\
 			memcpy.c			\
+			memcpy_s.c			\
 			memmove.c			\
 			memrchr.c			\
 			memset.c			\
diff --git a/lib/libc/memcpy_s.c b/lib/libc/memcpy_s.c
new file mode 100644
index 0000000..01e88b0
--- /dev/null
+++ b/lib/libc/memcpy_s.c
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2013-2023, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stddef.h>
+#include <string.h>
+
+int memcpy_s(void *dst, size_t dsize, void *src, size_t ssize)
+{
+	unsigned int *s = (unsigned int *)src;
+	unsigned int *d = (unsigned int *)dst;
+
+	/*
+	 * Check source and destination size is NULL
+	 */
+	if ((dst == NULL) || (src == NULL)) {
+		return -ENOMEM;
+	}
+
+	/*
+	 * Check source and destination size validity
+	 */
+	if ((dsize == 0) || (ssize == 0)) {
+		return -ERANGE;
+	}
+
+	/*
+	 * Check both source and destination size range
+	 */
+	if ((ssize > dsize) || (dsize > ssize)) {
+		return -EINVAL;
+	}
+
+	/*
+	 * Check both source and destination address overlapping
+	 * When (s > d < s + ssize)
+	 * Or (d > s < d + dsize)
+	 */
+
+	if (d > s) {
+		if ((d) < (s + ssize)) {
+			return -EOPNOTSUPP;
+		}
+	}
+
+	if (s > d) {
+		if ((s) < (d + dsize)) {
+			return -EOPNOTSUPP;
+		}
+	}
+
+	/*
+	 * Start copy process when there is no error
+	 */
+	while (ssize--) {
+		d[ssize] = s[ssize];
+	}
+
+	return 0;
+}
diff --git a/lib/libc/printf.c b/lib/libc/printf.c
index faccfdf..6931a7e 100644
--- a/lib/libc/printf.c
+++ b/lib/libc/printf.c
@@ -36,7 +36,7 @@
 }
 
 static int unsigned_num_print(unsigned long long int unum, unsigned int radix,
-			      char padc, int padn)
+			      char padc, int padn, bool uppercase)
 {
 	/* Just need enough space to store 64 bit decimal integer */
 	char num_buf[20];
@@ -51,10 +51,13 @@
 
 	do {
 		rem = unum % radix;
-		if (rem < 0xa)
+		if (rem < 0xa) {
 			num_buf[i] = '0' + rem;
-		else
+		} else if (uppercase) {
+			num_buf[i] = 'A' + (rem - 0xa);
+		} else {
 			num_buf[i] = 'a' + (rem - 0xa);
+		}
 		i++;
 		unum /= radix;
 	} while (unum > 0U);
@@ -105,8 +108,10 @@
 	char padc = '\0'; /* Padding character */
 	int padn; /* Number of characters to pad */
 	int count = 0; /* Number of printed characters */
+	bool uppercase; /* Print characters in uppercase */
 
 	while (*fmt != '\0') {
+		uppercase = false;
 		l_count = 0;
 		padn = 0;
 
@@ -129,7 +134,7 @@
 					unum = (unsigned long long int)num;
 
 				count += unsigned_num_print(unum, 10,
-							    padc, padn);
+							    padc, padn, uppercase);
 				break;
 			case 'c':
 				(void)putchar(va_arg(args, int));
@@ -147,12 +152,15 @@
 				}
 
 				count += unsigned_num_print(unum, 16,
-							    padc, padn);
+							    padc, padn, uppercase);
 				break;
+			case 'X':
+				uppercase = true;
+				// fall through
 			case 'x':
 				unum = get_unum_va_args(args, l_count);
 				count += unsigned_num_print(unum, 16,
-							    padc, padn);
+							    padc, padn, uppercase);
 				break;
 			case 'z':
 				if (sizeof(size_t) == 8U)
@@ -167,7 +175,7 @@
 			case 'u':
 				unum = get_unum_va_args(args, l_count);
 				count += unsigned_num_print(unum, 10,
-							    padc, padn);
+							    padc, padn, uppercase);
 				break;
 			case '0':
 				padc = '0';
diff --git a/lib/psa/rss_platform.c b/lib/psa/rss_platform.c
index 359f894..7d90bfc 100644
--- a/lib/psa/rss_platform.c
+++ b/lib/psa/rss_platform.c
@@ -5,10 +5,9 @@
  *
  */
 
-#include <stdint.h>
-
 #include <psa/client.h>
 #include <psa_manifest/sid.h>
+#include <rss_crypto_defs.h>
 #include <rss_platform_api.h>
 
 psa_status_t
@@ -41,3 +40,30 @@
 			RSS_PLATFORM_API_ID_NV_READ,
 			in_vec, 1, out_vec, 1);
 }
+
+psa_status_t
+rss_platform_key_read(enum rss_key_id_builtin_t key, uint8_t *data,
+		size_t data_size, size_t *data_length)
+{
+	psa_status_t status;
+
+	struct rss_crypto_pack_iovec iov = {
+		.function_id = RSS_CRYPTO_EXPORT_PUBLIC_KEY_SID,
+		.key_id = key,
+	};
+
+	psa_invec in_vec[] = {
+		{.base = &iov, .len = sizeof(struct rss_crypto_pack_iovec)},
+	};
+	psa_outvec out_vec[] = {
+		{.base = data, .len = data_size}
+	};
+
+	status = psa_call(RSS_CRYPTO_HANDLE, PSA_IPC_CALL,
+			in_vec, IOVEC_LEN(in_vec),
+			out_vec, IOVEC_LEN(out_vec));
+
+	*data_length = out_vec[0].len;
+
+	return status;
+}
diff --git a/lib/psci/aarch64/runtime_errata.S b/lib/psci/aarch64/runtime_errata.S
index 8d46691..89e3e12 100644
--- a/lib/psci/aarch64/runtime_errata.S
+++ b/lib/psci/aarch64/runtime_errata.S
@@ -20,7 +20,7 @@
 	mov	x18, x0
 
 #if ERRATA_A510_2684597
-	bl errata_cortex_a510_2684597_wa
+	bl erratum_cortex_a510_2684597_wa
 #endif
 
 	ret	x19
diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c
index bfc09cc..70bf77e 100644
--- a/lib/psci/psci_common.c
+++ b/lib/psci/psci_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -985,6 +985,9 @@
 	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
 	psci_power_state_t state_info = { {PSCI_LOCAL_STATE_RUN} };
 
+	/* Init registers that never change for the lifetime of TF-A */
+	cm_manage_extensions_el3();
+
 	/*
 	 * Verify that we have been explicitly turned ON or resumed from
 	 * suspend.
@@ -1034,6 +1037,13 @@
 		psci_cpu_suspend_finish(cpu_idx, &state_info);
 
 	/*
+	 * Generic management: Now we just need to retrieve the
+	 * information that we had stashed away during the cpu_on
+	 * call to set this cpu on its way.
+	 */
+	cm_prepare_el3_exit_ns();
+
+	/*
 	 * Set the requested and target state of this CPU and all the higher
 	 * power domains which are ancestors of this CPU to run.
 	 */
diff --git a/lib/psci/psci_on.c b/lib/psci/psci_on.c
index 76eb50c..1f619ae 100644
--- a/lib/psci/psci_on.c
+++ b/lib/psci/psci_on.c
@@ -225,11 +225,4 @@
 	/* Populate the mpidr field within the cpu node array */
 	/* This needs to be done only once */
 	psci_cpu_pd_nodes[cpu_idx].mpidr = read_mpidr() & MPIDR_AFFINITY_MASK;
-
-	/*
-	 * Generic management: Now we just need to retrieve the
-	 * information that we had stashed away during the cpu_on
-	 * call to set this cpu on its way.
-	 */
-	cm_prepare_el3_exit_ns();
 }
diff --git a/lib/psci/psci_suspend.c b/lib/psci/psci_suspend.c
index d93e60d..cb12b83 100644
--- a/lib/psci/psci_suspend.c
+++ b/lib/psci/psci_suspend.c
@@ -367,11 +367,4 @@
 	psci_set_suspend_pwrlvl(PSCI_INVALID_PWR_LVL);
 
 	PUBLISH_EVENT(psci_suspend_pwrdown_finish);
-
-	/*
-	 * Generic management: Now we just need to retrieve the
-	 * information that we had stashed away during the suspend
-	 * call to set this cpu on its way.
-	 */
-	cm_prepare_el3_exit_ns();
 }
diff --git a/lib/xlat_tables_v2/aarch64/xlat_tables_arch.c b/lib/xlat_tables_v2/aarch64/xlat_tables_arch.c
index c847a9e..bb6a35c 100644
--- a/lib/xlat_tables_v2/aarch64/xlat_tables_arch.c
+++ b/lib/xlat_tables_v2/aarch64/xlat_tables_arch.c
@@ -27,16 +27,14 @@
 	if (size == PAGE_SIZE_4KB) {
 		tgranx = read_id_aa64mmfr0_el0_tgran4_field();
 		/* MSB of TGRAN4 field will be '1' for unsupported feature */
-		return ((tgranx >= ID_AA64MMFR0_EL1_TGRAN4_SUPPORTED) &&
-			(tgranx < 8U));
+		return (tgranx < 8U);
 	} else if (size == PAGE_SIZE_16KB) {
 		tgranx = read_id_aa64mmfr0_el0_tgran16_field();
 		return (tgranx >= ID_AA64MMFR0_EL1_TGRAN16_SUPPORTED);
 	} else if (size == PAGE_SIZE_64KB) {
 		tgranx = read_id_aa64mmfr0_el0_tgran64_field();
 		/* MSB of TGRAN64 field will be '1' for unsupported feature */
-		return ((tgranx >= ID_AA64MMFR0_EL1_TGRAN64_SUPPORTED) &&
-			(tgranx < 8U));
+		return (tgranx < 8U);
 	} else {
 		return false;
 	}
diff --git a/make_helpers/armv7-a-cpus.mk b/make_helpers/armv7-a-cpus.mk
index eec85cc..a8e9d50 100644
--- a/make_helpers/armv7-a-cpus.mk
+++ b/make_helpers/armv7-a-cpus.mk
@@ -15,9 +15,9 @@
 # armClang requires -march=armv7-a for all ARMv7 Cortex-A. To comply with
 # all, just drop -march and supply only -mcpu.
 
-# Platform can override march32-directive through MARCH32_DIRECTIVE
-ifdef MARCH32_DIRECTIVE
-march32-directive		:= $(MARCH32_DIRECTIVE)
+# Platform can override march-directive through MARCH_DIRECTIVE
+ifdef MARCH_DIRECTIVE
+march-directive		:= $(MARCH_DIRECTIVE)
 else
 march32-set-${ARM_CORTEX_A5}	:= -mcpu=cortex-a5
 march32-set-${ARM_CORTEX_A7}	:= -mcpu=cortex-a7
@@ -29,7 +29,7 @@
 
 # default to -march=armv7-a as target directive
 march32-set-yes			?= -march=armv7-a
-march32-directive		:= ${march32-set-yes} ${march32-neon-yes}
+march-directive		:= ${march32-set-yes} ${march32-neon-yes}
 endif
 
 # Platform may override these extension support directives:
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index f9077eb..a065039 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -191,6 +191,9 @@
 # Flag to enable access to Guarded Control Stack (FEAT_GCS)
 ENABLE_FEAT_GCS			:= 0
 
+# Flag to enable NoTagAccess memory region attribute for stage 2 of translation.
+ENABLE_FEAT_MTE_PERM		:= 0
+
 # By default BL31 encryption disabled
 ENCRYPT_BL31			:= 0
 
diff --git a/make_helpers/march.mk b/make_helpers/march.mk
new file mode 100644
index 0000000..2417709
--- /dev/null
+++ b/make_helpers/march.mk
@@ -0,0 +1,85 @@
+#
+# Copyright (c) 2023, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# This build helper makefile is used to determine a suitable march build
+# option to be used, based on platform provided ARM_ARCH_MAJOR/MINOR
+# data and compiler supported march version.
+
+# Set the compiler's target architecture profile based on
+# ARM_ARCH_MAJOR and ARM_ARCH_MINOR options.
+
+
+# This is used to collect available march values from compiler.
+# Example on a gcc-12.2 arm64 toolchain it will look like:
+# [...]
+#./aarch64-none-elf-gcc --target-help -march=foo
+# cc1: error: unknown value 'foo' for '-march'
+# cc1: note: valid arguments are: armv8-a armv8.1-a armv8.2-a armv8.3-a armv8.4-a armv8.5-a
+# armv8.6-a armv8.7-a armv8.8-a armv8-r armv9-a
+# [...]
+#
+GCC_MARCH_OUTPUT := $(shell $(CC) -march=foo -Q --help=target -v 2>&1)
+
+# This function is used to find the best march value supported by the given compiler.
+# We try to use `GCC_MARCH_OUTPUT` which has verbose message with supported march values we filter that
+# to find armvX.Y-a or armvX-a values, then filter the best supported arch based on ARM_ARCH_MAJOR.
+#
+# Example on a gcc-12.2 arm64 toolchain this will return armv9-a if platform requested for armv9.2-a
+# Similarly on gcc 11.3 it would return armv8.6-a if platform requested armv8.8-a
+define major_best_avail_march
+$(1) := $(lastword $(filter armv$(ARM_ARCH_MAJOR)% ,$(filter armv%-a, $(GCC_MARCH_OUTPUT))))
+endef
+
+# This function is used to just return latest march value supported by the given compiler.
+#
+# Example: this would return armv8.6-a on a gcc-11.3 when platform requested for armv9.0-a
+#
+# Thus this function should be used in conjunction with major_best_avail_march, when best match
+# is not found it should be ok to try with lastest known supported march value from the
+# compiler.
+define latest_match_march
+$(1) := $(lastword $(filter armv%-a, $(GCC_MARCH_OUTPUT)))
+endef
+
+ifdef MARCH_DIRECTIVE
+    march-directive		:= $(MARCH_DIRECTIVE)
+else
+
+ifeq (${ARM_ARCH_MINOR},0)
+    provided-march = armv${ARM_ARCH_MAJOR}-a
+else
+    provided-march = armv${ARM_ARCH_MAJOR}.${ARM_ARCH_MINOR}-a
+endif
+
+ifeq ($(findstring clang,$(notdir $(CC))),)
+
+# We expect from Platform to provide a correct Major/Minor value but expecting something
+# from compiler with unsupported march means we shouldn't fail without trying anything,
+# so here we try to find best supported march value and use that for compilation.
+# If we don't support current march version from gcc compiler, try with lower arch based on
+# availability. In TF-A there is no hard rule for need of higher version march for basic
+# functionality, denying a build on only this fact doesn't look correct, so try with best
+# or latest march values from whats available from compiler.
+ifeq (,$(findstring $(provided-march), $(GCC_MARCH_OUTPUT)))
+    $(eval $(call major_best_avail_march, available-march))
+
+ifeq (, $(available-march))
+    $(eval $(call latest_match_march, available-march))
+endif
+
+# If we fail to come up with any available-march value so far, don't update
+# provided-march and thus allow the build to fail using the provided-march
+# which is derived based on arch_major and arch_minor values.
+ifneq (,$(available-march))
+    provided-march := ${available-march}
+endif
+
+endif # provided-march supported
+endif # not clang
+
+march-directive := -march=${provided-march}
+
+endif # MARCH_DIRECTIVE
diff --git a/plat/arm/board/arm_fpga/platform.mk b/plat/arm/board/arm_fpga/platform.mk
index f88eaa8..c31697e 100644
--- a/plat/arm/board/arm_fpga/platform.mk
+++ b/plat/arm/board/arm_fpga/platform.mk
@@ -58,18 +58,17 @@
 				lib/cpus/aarch64/cortex_a73.S
 else
 # AArch64-only cores
-	FPGA_CPU_LIBS	+=lib/cpus/aarch64/cortex_a510.S		\
-				lib/cpus/aarch64/cortex_a710.S				\
-				lib/cpus/aarch64/cortex_a715.S				\
-				lib/cpus/aarch64/cortex_x3.S 				\
+	FPGA_CPU_LIBS	+=	lib/cpus/aarch64/cortex_a510.S			\
+				lib/cpus/aarch64/cortex_a520.S			\
+				lib/cpus/aarch64/cortex_a715.S			\
+				lib/cpus/aarch64/cortex_a720.S			\
+				lib/cpus/aarch64/cortex_x3.S 			\
+				lib/cpus/aarch64/cortex_x4.S			\
 				lib/cpus/aarch64/neoverse_n_common.S		\
-				lib/cpus/aarch64/neoverse_n1.S				\
-				lib/cpus/aarch64/neoverse_n2.S				\
-				lib/cpus/aarch64/neoverse_v1.S				\
-				lib/cpus/aarch64/cortex_hayes.S				\
-				lib/cpus/aarch64/cortex_hunter.S			\
-				lib/cpus/aarch64/cortex_hunter_elp_arm.S	\
-				lib/cpus/aarch64/cortex_chaberton.S			\
+				lib/cpus/aarch64/neoverse_n1.S			\
+				lib/cpus/aarch64/neoverse_n2.S			\
+				lib/cpus/aarch64/neoverse_v1.S			\
+				lib/cpus/aarch64/cortex_chaberton.S		\
 				lib/cpus/aarch64/cortex_blackhawk.S
 
 # AArch64/AArch32 cores
diff --git a/plat/arm/board/fvp/fvp_bl1_measured_boot.c b/plat/arm/board/fvp/fvp_bl1_measured_boot.c
index 72fdfef..b8431c5 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-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -54,7 +54,7 @@
 	event_log_init(event_log, event_log + sizeof(event_log));
 	event_log_write_header();
 
-	rss_measured_boot_init();
+	rss_measured_boot_init(fvp_rss_mboot_metadata);
 }
 
 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 e6b9192..564118e 100644
--- a/plat/arm/board/fvp/fvp_bl2_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
@@ -117,7 +117,7 @@
 
 	event_log_init((uint8_t *)event_log_start, event_log_finish);
 
-	rss_measured_boot_init();
+	rss_measured_boot_init(fvp_rss_mboot_metadata);
 }
 
 int plat_mboot_measure_critical_data(unsigned int critical_data_id,
diff --git a/plat/arm/board/fvp/fvp_common_measured_boot.c b/plat/arm/board/fvp/fvp_common_measured_boot.c
index b5b8f10..7419e5e 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-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -16,11 +16,6 @@
 extern event_log_metadata_t fvp_event_log_metadata[];
 extern struct rss_mboot_metadata fvp_rss_mboot_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;
@@ -38,7 +33,8 @@
 	}
 
 	/* Calculate image hash and record data in RSS */
-	err = rss_mboot_measure_and_record(image_data->image_base,
+	err = rss_mboot_measure_and_record(fvp_rss_mboot_metadata,
+					   image_data->image_base,
 					   image_data->image_size,
 					   image_id);
 	if (err != 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
index b9e4f86..72dfa30 100644
--- a/plat/arm/board/fvp/fvp_el3_spmc_logical_sp.c
+++ b/plat/arm/board/fvp/fvp_el3_spmc_logical_sp.c
@@ -27,6 +27,7 @@
 					  void *handle, uint64_t flags)
 {
 	uint64_t ret;
+	uint32_t src_dst;
 
 	/* Determine if we have a 64 or 32 direct request. */
 	if (smc_fid == FFA_MSG_SEND_DIRECT_REQ_SMC32) {
@@ -36,18 +37,22 @@
 	} 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");
+	INFO("LSP: Received Direct Request from %s world (0x%x)\n",
+	     secure_origin ? "Secure" : "Normal", ffa_endpoint_source(x1));
 
+	/* Populate the source and destination IDs. */
+	src_dst = (uint32_t) LP_PARTITION_ID << FFA_DIRECT_MSG_SOURCE_SHIFT |
+		  ffa_endpoint_source(x1) << FFA_DIRECT_MSG_DESTINATION_SHIFT;
 	/*
 	 * 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);
+	SMC_RET8(handle, ret, src_dst, 0, x4, 0, 0, 0, 0);
 }
 
 /* Register logical partition  */
diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c
index b8c97f8..534a175 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -393,6 +393,10 @@
 
 	for (i = ARM_PWR_LVL0; i <= PLAT_MAX_PWR_LVL; i++)
 		req_state->pwr_domain_state[i] = ARM_LOCAL_STATE_OFF;
+
+#if PSCI_OS_INIT_MODE
+	req_state->last_at_pwrlvl = PLAT_MAX_PWR_LVL;
+#endif
 }
 #endif
 
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index 9e72ba0..df04617 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -43,7 +43,7 @@
  */
 #define PLAT_ARM_CLUSTER_COUNT		U(FVP_CLUSTER_COUNT)
 
-#define PLAT_ARM_TRUSTED_SRAM_SIZE	UL(0x00040000)	/* 256 KB */
+#define PLAT_ARM_TRUSTED_SRAM_SIZE	(FVP_TRUSTED_SRAM_SIZE * UL(1024))
 
 #define PLAT_ARM_TRUSTED_ROM_BASE	UL(0x00000000)
 #define PLAT_ARM_TRUSTED_ROM_SIZE	UL(0x04000000)	/* 64 MB */
@@ -205,14 +205,18 @@
 #endif
 
 /*
- * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
- * little space for growth.
+ * Set the maximum size of BL2 to be close to half of the Trusted SRAM.
+ * Maximum size of BL2 increases as Trusted SRAM size increases.
  */
 #if CRYPTO_SUPPORT
 #if (TF_MBEDTLS_KEY_ALG_ID == TF_MBEDTLS_RSA_AND_ECDSA) || COT_DESC_IN_DTB
-# define PLAT_ARM_MAX_BL2_SIZE	(UL(0x1E000) - FVP_BL2_ROMLIB_OPTIMIZATION)
+# define PLAT_ARM_MAX_BL2_SIZE	((PLAT_ARM_TRUSTED_SRAM_SIZE / 2) - \
+				 (2 * PAGE_SIZE) - \
+				 FVP_BL2_ROMLIB_OPTIMIZATION)
 #else
-# define PLAT_ARM_MAX_BL2_SIZE	(UL(0x1D000) - FVP_BL2_ROMLIB_OPTIMIZATION)
+# define PLAT_ARM_MAX_BL2_SIZE	((PLAT_ARM_TRUSTED_SRAM_SIZE / 2) - \
+				 (3 * PAGE_SIZE) - \
+				 FVP_BL2_ROMLIB_OPTIMIZATION)
 #endif
 #elif ARM_BL31_IN_DRAM
 /* When ARM_BL31_IN_DRAM is set, BL2 can use almost all of Trusted SRAM. */
@@ -230,9 +234,12 @@
 /*
  * 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
+ * BL2 and BL1-RW.
+ * Size of the BL31 PROGBITS increases as the SRAM size increases.
  */
-#define PLAT_ARM_MAX_BL31_SIZE		(UL(0x3D000) - ARM_L0_GPT_SIZE)
+#define PLAT_ARM_MAX_BL31_SIZE		(PLAT_ARM_TRUSTED_SRAM_SIZE - \
+					 ARM_SHARED_RAM_SIZE - \
+					 ARM_FW_CONFIGS_SIZE - ARM_L0_GPT_SIZE)
 #endif /* RESET_TO_BL31 */
 
 #ifndef __aarch64__
diff --git a/plat/arm/board/fvp/jmptbl.i b/plat/arm/board/fvp/jmptbl.i
index 927ffef..dc8032f 100644
--- a/plat/arm/board/fvp/jmptbl.i
+++ b/plat/arm/board/fvp/jmptbl.i
@@ -43,6 +43,7 @@
 mbedtls mbedtls_asn1_get_bitstring_null
 mbedtls mbedtls_asn1_get_bool
 mbedtls mbedtls_asn1_get_int
+mbedtls mbedtls_asn1_get_len
 mbedtls mbedtls_asn1_get_tag
 mbedtls mbedtls_free
 mbedtls mbedtls_md
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 0433b61..e790f92 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -24,6 +24,10 @@
 
 FVP_DT_PREFIX		:= fvp-base-gicv3-psci
 
+# Size (in kilobytes) of the Trusted SRAM region to  utilize when building for
+# the FVP platform. This option defaults to 256.
+FVP_TRUSTED_SRAM_SIZE	:= 256
+
 # This is a very trickly TEMPORARY fix. Enabling ALL features exceeds BL31's
 # progbits limit. We need a way to build all useful configurations while waiting
 # on the fvp to increase its SRAM size. The problem is twofild:
@@ -76,6 +80,7 @@
 ENABLE_FEAT_CSV2_2		:= 2
 ENABLE_FEAT_DIT			:= 2
 ENABLE_FEAT_PAN			:= 2
+ENABLE_FEAT_MTE_PERM		:= 2
 ENABLE_FEAT_VHE			:= 2
 CTX_INCLUDE_NEVE_REGS		:= 2
 ENABLE_FEAT_SEL2		:= 2
@@ -104,6 +109,9 @@
 # Pass FVP_GICR_REGION_PROTECTION to the build system.
 $(eval $(call add_define,FVP_GICR_REGION_PROTECTION))
 
+# Pass FVP_TRUSTED_SRAM_SIZE to the build system.
+$(eval $(call add_define,FVP_TRUSTED_SRAM_SIZE))
+
 # Sanity check the cluster count and if FVP_CLUSTER_COUNT <= 2,
 # choose the CCI driver , else the CCN driver
 ifeq ($(FVP_CLUSTER_COUNT), 0)
@@ -197,6 +205,7 @@
 					lib/cpus/aarch64/cortex_a76ae.S		\
 					lib/cpus/aarch64/cortex_a77.S		\
 					lib/cpus/aarch64/cortex_a78.S		\
+					lib/cpus/aarch64/cortex_a78_ae.S	\
 					lib/cpus/aarch64/cortex_a78c.S		\
 					lib/cpus/aarch64/cortex_a710.S		\
 					lib/cpus/aarch64/neoverse_n_common.S	\
@@ -213,7 +222,8 @@
 
 else
 FVP_CPU_LIBS		+=	lib/cpus/aarch32/cortex_a32.S			\
-				lib/cpus/aarch32/cortex_a57.S
+				lib/cpus/aarch32/cortex_a57.S			\
+				lib/cpus/aarch32/cortex_a53.S
 endif
 
 BL1_SOURCES		+=	drivers/arm/smmu/smmu_v3.c			\
diff --git a/plat/arm/board/juno/jmptbl.i b/plat/arm/board/juno/jmptbl.i
index 8932aa0..327a345 100644
--- a/plat/arm/board/juno/jmptbl.i
+++ b/plat/arm/board/juno/jmptbl.i
@@ -41,6 +41,7 @@
 mbedtls mbedtls_asn1_get_bitstring_null
 mbedtls mbedtls_asn1_get_bool
 mbedtls mbedtls_asn1_get_int
+mbedtls mbedtls_asn1_get_len
 mbedtls mbedtls_asn1_get_tag
 mbedtls mbedtls_free
 mbedtls mbedtls_md
diff --git a/plat/arm/board/morello/include/platform_def.h b/plat/arm/board/morello/include/platform_def.h
index 76e63aa..993aa46 100644
--- a/plat/arm/board/morello/include/platform_def.h
+++ b/plat/arm/board/morello/include/platform_def.h
@@ -163,7 +163,59 @@
 #define PLAT_MHUV2_BASE				PLAT_CSS_MHU_BASE
 #define PLAT_MAX_PWR_LVL			U(2)
 
-#define PLAT_ARM_G1S_IRQ_PROPS(grp)		CSS_G1S_IRQ_PROPS(grp)
+/* Interrupt handling constants */
+#define MORELLO_IRQ_SEC_UART			U(87)
+#define MORELLO_IRQ_DISPLAY_TCU_EVENT_Q		U(107)
+#define MORELLO_IRQ_DISPLAY_TCU_CMD_SYNC	U(111)
+#define MORELLO_IRQ_DISPLAY_TCU_GLOBAL		U(113)
+#define MORELLO_IRQ_MMU_TCU1_EVENT_Q		U(257)
+#define MORELLO_IRQ_MMU_TCU1_CMD_SYNC		U(258)
+#define MORELLO_IRQ_MMU_TCU1_GLOBAL		U(259)
+#define MORELLO_IRQ_MMU_TCU2_EVENT_Q		U(264)
+#define MORELLO_IRQ_MMU_TCU2_CMD_SYNC		U(265)
+#define MORELLO_IRQ_MMU_TCU2_GLOBAL		U(266)
+#define MORELLO_IRQ_CLUSTER0_MHU		U(349)
+#define MORELLO_IRQ_CLUSTER1_MHU		U(351)
+#define MORELLO_IRQ_P0_REFCLK			U(412)
+#define MORELLO_IRQ_P1_REFCLK			U(413)
+
+#define PLAT_ARM_G1S_IRQ_PROPS(grp) \
+	ARM_G1S_IRQ_PROPS(grp), \
+	INTR_PROP_DESC(CSS_IRQ_MHU, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(CSS_IRQ_TZ_WDOG, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(CSS_IRQ_SEC_SYS_TIMER, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_SEC_UART, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_DISPLAY_TCU_EVENT_Q, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_DISPLAY_TCU_CMD_SYNC, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_DISPLAY_TCU_GLOBAL, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_MMU_TCU1_EVENT_Q, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_MMU_TCU1_CMD_SYNC, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_MMU_TCU1_GLOBAL, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_MMU_TCU2_EVENT_Q, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_MMU_TCU2_CMD_SYNC, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_MMU_TCU2_GLOBAL, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_CLUSTER0_MHU, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_CLUSTER1_MHU, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_P0_REFCLK, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(MORELLO_IRQ_P1_REFCLK, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL)
+
 #define PLAT_ARM_G0_IRQ_PROPS(grp)		ARM_G0_IRQ_PROPS(grp)
 
 #define MORELLO_DEVICE_BASE			ULL(0x08000000)
diff --git a/plat/arm/board/n1sdp/include/platform_def.h b/plat/arm/board/n1sdp/include/platform_def.h
index b3799a7..74d0c91 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-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -190,11 +190,47 @@
 #define PLAT_CSS_MHU_BASE			0x45000000
 #define PLAT_MAX_PWR_LVL			2
 
-#define PLAT_ARM_G1S_IRQS			ARM_G1S_IRQS,			\
-						CSS_IRQ_MHU
-#define PLAT_ARM_G0_IRQS			ARM_G0_IRQS
+/* Interrupt handling constants */
+#define N1SDP_IRQ_MMU_TCU1_EVENT_Q_SEC		U(257)
+#define N1SDP_IRQ_MMU_TCU1_CMD_SYNC_SEC		U(258)
+#define N1SDP_IRQ_MMU_TCU1_GLOBAL		U(259)
+#define N1SDP_IRQ_MMU_TCU2_EVENT_Q_SEC		U(264)
+#define N1SDP_IRQ_MMU_TCU2_CMD_SYNC_SEC		U(265)
+#define N1SDP_IRQ_MMU_TCU2_GLOBAL		U(266)
+#define N1SDP_IRQ_CLUSTER0_MHU			U(349)
+#define N1SDP_IRQ_CLUSTER1_MHU			U(351)
+#define N1SDP_IRQ_P0_REFCLK			U(412)
+#define N1SDP_IRQ_P1_REFCLK			U(413)
+
+#define PLAT_ARM_G1S_IRQ_PROPS(grp) \
+	ARM_G1S_IRQ_PROPS(grp), \
+	INTR_PROP_DESC(CSS_IRQ_MHU, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(CSS_IRQ_TZ_WDOG, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(CSS_IRQ_SEC_SYS_TIMER, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_MMU_TCU1_EVENT_Q_SEC, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_MMU_TCU1_CMD_SYNC_SEC, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_MMU_TCU1_GLOBAL, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_MMU_TCU2_EVENT_Q_SEC, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_MMU_TCU2_CMD_SYNC_SEC, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_MMU_TCU2_GLOBAL, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_CLUSTER0_MHU, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_CLUSTER1_MHU, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_P0_REFCLK, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(N1SDP_IRQ_P1_REFCLK, \
+			GIC_HIGHEST_SEC_PRIORITY, grp, GIC_INTR_CFG_LEVEL)
 
-#define PLAT_ARM_G1S_IRQ_PROPS(grp)		CSS_G1S_IRQ_PROPS(grp)
 #define PLAT_ARM_G0_IRQ_PROPS(grp)		ARM_G0_IRQ_PROPS(grp)
 
 
diff --git a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
index 2b9ed25..430aab6 100644
--- a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
+++ b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
@@ -52,8 +52,8 @@
 		PLAT_ARM_GICD_BASE >> 16
 	},
 	.spi_ids = {
-		{PLAT_ARM_GICD_BASE, 32, 479},
-		{PLAT_ARM_GICD_BASE, 512, 959}
+		{PLAT_ARM_GICD_BASE, 32, 511},
+		{PLAT_ARM_GICD_BASE, 512, 991}
 	}
 };
 
diff --git a/plat/arm/board/rdn2/include/platform_def.h b/plat/arm/board/rdn2/include/platform_def.h
index 8e63de5..ff1a437 100644
--- a/plat/arm/board/rdn2/include/platform_def.h
+++ b/plat/arm/board/rdn2/include/platform_def.h
@@ -8,7 +8,7 @@
 #define PLATFORM_DEF_H
 
 #include <lib/utils_def.h>
-
+#include <sgi_sdei.h>
 #include <sgi_soc_platform_def_v2.h>
 
 #if (CSS_SGI_PLATFORM_VARIANT == 1)
diff --git a/plat/arm/board/rdn2/include/rdn2_ras.h b/plat/arm/board/rdn2/include/rdn2_ras.h
new file mode 100644
index 0000000..1d9af60
--- /dev/null
+++ b/plat/arm/board/rdn2/include/rdn2_ras.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2023, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RDN2_RAS_H
+#define RDN2_RAS_H
+
+#include <sgi_ras.h>
+
+extern struct plat_sgi_ras_config ras_config;
+
+#endif /* RDN2_RAS_H */
diff --git a/plat/arm/board/rdn2/platform.mk b/plat/arm/board/rdn2/platform.mk
index ca55036..9ebca23 100644
--- a/plat/arm/board/rdn2/platform.mk
+++ b/plat/arm/board/rdn2/platform.mk
@@ -69,6 +69,12 @@
 BL31_CFLAGS		+=	-DPLAT_XLAT_TABLES_DYNAMIC
 endif
 
+ifeq (${RAS_FFH_SUPPORT},1)
+BL31_SOURCES		+=	${RDN2_BASE}/rdn2_ras.c			\
+				${CSS_ENT_BASE}/ras/sgi_ras_common.c	\
+				${CSS_ENT_BASE}/ras/sgi_ras_sram.c
+endif
+
 # Add the FDT_SOURCES and options for Dynamic Config
 FDT_SOURCES		+=	${RDN2_BASE}/fdts/${PLAT}_fw_config.dts	\
 				${RDN2_BASE}/fdts/${PLAT}_tb_fw_config.dts
diff --git a/plat/arm/board/rdn2/rdn2_plat.c b/plat/arm/board/rdn2/rdn2_plat.c
index a5564ce..e8a967e 100644
--- a/plat/arm/board/rdn2/rdn2_plat.c
+++ b/plat/arm/board/rdn2/rdn2_plat.c
@@ -8,6 +8,7 @@
 #include <drivers/arm/gic600_multichip.h>
 #include <plat/arm/common/plat_arm.h>
 #include <plat/common/platform.h>
+#include <rdn2_ras.h>
 #include <sgi_soc_platform_def_v2.h>
 #include <sgi_plat.h>
 
@@ -134,5 +135,9 @@
 #endif
 
 	sgi_bl31_common_platform_setup();
+
+#if RAS_FFH_SUPPORT
+	sgi_ras_platform_setup(&ras_config);
+#endif
 }
 #endif /* IMAGE_BL31 */
diff --git a/plat/arm/board/rdn2/rdn2_ras.c b/plat/arm/board/rdn2/rdn2_ras.c
new file mode 100644
index 0000000..c1e5ab6
--- /dev/null
+++ b/plat/arm/board/rdn2/rdn2_ras.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2023, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+#include <sgi_ras.h>
+#include <sgi_sdei.h>
+
+struct sgi_ras_ev_map plat_ras_map[] = {
+	/* Non Secure base RAM ECC CE interrupt */
+	{SGI_SDEI_DS_EVENT_0, NS_RAM_ECC_CE_INT, SGI_RAS_INTR_TYPE_SPI},
+
+	/* Non Secure base RAM ECC UE interrupt */
+	{SGI_SDEI_DS_EVENT_0, NS_RAM_ECC_UE_INT, SGI_RAS_INTR_TYPE_SPI},
+};
+
+/* RAS error record list definition, used by the common RAS framework. */
+struct err_record_info plat_err_records[] = {
+	/* Base element RAM Non-secure error record. */
+	ERR_RECORD_MEMMAP_V1(SOC_NS_RAM_ERR_REC_BASE, 4, NULL,
+				&sgi_ras_sram_intr_handler, 0),
+};
+
+/* RAS error interrupt list definition, used by the common RAS framework. */
+struct ras_interrupt plat_ras_interrupts[] = {
+	{
+		.intr_number = NS_RAM_ECC_CE_INT,
+		.err_record = &plat_err_records[0],
+	}, {
+		.intr_number = NS_RAM_ECC_UE_INT,
+		.err_record = &plat_err_records[0],
+	},
+};
+
+/* Registers the RAS error record list with common RAS framework. */
+REGISTER_ERR_RECORD_INFO(plat_err_records);
+/* Registers the RAS error interrupt info list with common RAS framework. */
+REGISTER_RAS_INTERRUPTS(plat_ras_interrupts);
+
+/* Platform RAS handling config data definition */
+struct plat_sgi_ras_config ras_config = {
+	plat_ras_map,
+	ARRAY_SIZE(plat_ras_map)
+};
diff --git a/plat/arm/board/rdn2/rdn2_security.c b/plat/arm/board/rdn2/rdn2_security.c
index dff6a19..b836a7f 100644
--- a/plat/arm/board/rdn2/rdn2_security.c
+++ b/plat/arm/board/rdn2/rdn2_security.c
@@ -8,8 +8,16 @@
 #include <plat/arm/common/plat_arm.h>
 #include <platform_def.h>
 
+#define RDN2_TZC_CPER_REGION					\
+	{CSS_SGI_SP_CPER_BUF_BASE, (CSS_SGI_SP_CPER_BUF_BASE +	\
+	CSS_SGI_SP_CPER_BUF_SIZE) - 1, TZC_REGION_S_NONE,	\
+	PLAT_ARM_TZC_NS_DEV_ACCESS}
+
 static const arm_tzc_regions_info_t tzc_regions[] = {
 	ARM_TZC_REGIONS_DEF,
+#if RAS_FFH_SUPPORT
+	RDN2_TZC_CPER_REGION,
+#endif
 	{}
 };
 
diff --git a/plat/arm/board/tc/include/tc_plat.h b/plat/arm/board/tc/include/tc_plat.h
index 117fbb4..a6b2b0d 100644
--- a/plat/arm/board/tc/include/tc_plat.h
+++ b/plat/arm/board/tc/include/tc_plat.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021-2023, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,10 @@
 #ifndef TC_PLAT_H
 #define TC_PLAT_H
 
+#ifdef PLATFORM_TEST_ROTPK
+#include <rss_crypto_defs.h>
+#endif
+
 void tc_bl31_common_platform_setup(void);
 
 #ifdef PLATFORM_TEST_TFM_TESTSUITE
@@ -17,4 +21,13 @@
 int nv_counter_test(void);
 #endif
 
+#ifdef PLATFORM_TEST_ROTPK
+struct key_id_info {
+	enum rss_key_id_builtin_t key_id;
+	const char *key_id_name;
+};
+
+int rotpk_test(void);
+#endif
+
 #endif /* TC_PLAT_H */
diff --git a/plat/arm/board/tc/nv_counter_test.c b/plat/arm/board/tc/nv_counter_test.c
index f9e001e..179ec4b 100644
--- a/plat/arm/board/tc/nv_counter_test.c
+++ b/plat/arm/board/tc/nv_counter_test.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2023, ARM Limited. All rights reserved.
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -22,29 +22,29 @@
 
 	status = rss_comms_init(PLAT_RSS_AP_SND_MHU_BASE, PLAT_RSS_AP_RCV_MHU_BASE);
 	if (status != PSA_SUCCESS) {
-		printf("Failed to initialize RSS communication channel\n");
+		printf("Failed to initialize RSS communication channel - psa_status = %d\n", status);
 		return -1;
 	}
 
 	for (id = 0; id < 3; id++) {
 		status = rss_platform_nv_counter_read(id, sizeof(old_val), (uint8_t *)&old_val);
 		if (status != PSA_SUCCESS) {
-			printf("Failed during first id=(%d) rss_platform_nv_counter_read\n",
-				       id);
+			printf("Failed during first id=(%d) rss_platform_nv_counter_read - psa_status = %d\n",
+				       id, status);
 			return -1;
 		}
 
 		status = rss_platform_nv_counter_increment(id);
 		if (status != PSA_SUCCESS) {
-			printf("Failed during id=(%d) rss_platform_nv_counter_increment\n",
-					id);
+			printf("Failed during id=(%d) rss_platform_nv_counter_increment - psa_status = %d\n",
+					id, status);
 			return -1;
 		}
 
 		status = rss_platform_nv_counter_read(id, sizeof(new_val), (uint8_t *)&new_val);
 		if (status != PSA_SUCCESS) {
-			printf("Failed during second id=(%d) rss_platform_nv_counter_read\n",
-					id);
+			printf("Failed during second id=(%d) rss_platform_nv_counter_read - psa_status = %d\n",
+					id, status);
 			return -1;
 		}
 
diff --git a/plat/arm/board/tc/platform.mk b/plat/arm/board/tc/platform.mk
index c29537c..8ca33ca 100644
--- a/plat/arm/board/tc/platform.mk
+++ b/plat/arm/board/tc/platform.mk
@@ -6,8 +6,7 @@
 include common/fdt_wrappers.mk
 
 ifeq ($(TARGET_PLATFORM), 0)
-$(warning Platform ${PLAT}$(TARGET_PLATFORM) is deprecated. \
-Some of the features might not work as expected)
+	$(error Platform ${PLAT}$(TARGET_PLATFORM) is deprecated.)
 endif
 
 ifeq ($(shell expr $(TARGET_PLATFORM) \<= 2), 0)
@@ -70,13 +69,6 @@
 
 PLAT_INCLUDES		+=	-I${TC_BASE}/include/
 
-# CPU libraries for TARGET_PLATFORM=0
-ifeq (${TARGET_PLATFORM}, 0)
-TC_CPU_SOURCES	+=	lib/cpus/aarch64/cortex_a510.S	\
-			lib/cpus/aarch64/cortex_a710.S	\
-			lib/cpus/aarch64/cortex_x2.S
-endif
-
 # CPU libraries for TARGET_PLATFORM=1
 ifeq (${TARGET_PLATFORM}, 1)
 TC_CPU_SOURCES	+=	lib/cpus/aarch64/cortex_a510.S \
@@ -86,9 +78,9 @@
 
 # CPU libraries for TARGET_PLATFORM=2
 ifeq (${TARGET_PLATFORM}, 2)
-TC_CPU_SOURCES	+=	lib/cpus/aarch64/cortex_hayes.S \
-			lib/cpus/aarch64/cortex_hunter.S \
-			lib/cpus/aarch64/cortex_hunter_elp_arm.S
+TC_CPU_SOURCES	+=	lib/cpus/aarch64/cortex_a520.S \
+			lib/cpus/aarch64/cortex_a720.S \
+			lib/cpus/aarch64/cortex_x4.S
 endif
 
 INTERCONNECT_SOURCES	:=	${TC_BASE}/tc_interconnect.c
@@ -197,31 +189,11 @@
 endif
 
 ifneq (${PLATFORM_TEST},)
-    $(eval $(call add_define,PLATFORM_TESTS))
-
-    ifeq (${PLATFORM_TEST},rss-nv-counters)
-        include drivers/arm/rss/rss_comms.mk
-
-        # Test code.
-        BL31_SOURCES	+=	plat/arm/board/tc/nv_counter_test.c
-
-        # Code under testing.
-        BL31_SOURCES	+=	lib/psa/rss_platform.c \
-				drivers/arm/rss/rss_comms.c \
-				${RSS_COMMS_SOURCES}
-
-        PLAT_INCLUDES	+=	-Iinclude/lib/psa
-
-        $(eval $(call add_define,PLATFORM_TEST_NV_COUNTERS))
-    else ifeq (${PLATFORM_TEST},tfm-testsuite)
-        # Add this include as first, before arm_common.mk. This is necessary
-        # because arm_common.mk builds Mbed TLS, and platform_test.mk can
-        # change the list of Mbed TLS files that are to be compiled
-        # (LIBMBEDTLS_SRCS).
-        include plat/arm/board/tc/platform_test.mk
-    else
-        $(error "Unsupported PLATFORM_TEST value")
-    endif
+    # Add this include as first, before arm_common.mk. This is necessary
+    # because arm_common.mk builds Mbed TLS, and platform_test.mk can
+    # change the list of Mbed TLS files that are to be compiled
+    # (LIBMBEDTLS_SRCS).
+    include plat/arm/board/tc/platform_test.mk
 endif
 
 
diff --git a/plat/arm/board/tc/platform_test.mk b/plat/arm/board/tc/platform_test.mk
index e974855..2fd5ea0 100644
--- a/plat/arm/board/tc/platform_test.mk
+++ b/plat/arm/board/tc/platform_test.mk
@@ -3,7 +3,37 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
-ifeq (${PLATFORM_TEST},tfm-testsuite)
+$(eval $(call add_define,PLATFORM_TESTS))
+
+ifeq (${PLATFORM_TEST},rss-nv-counters)
+    include drivers/arm/rss/rss_comms.mk
+
+    # Test code.
+    BL31_SOURCES	+=	plat/arm/board/tc/nv_counter_test.c
+
+    # Code under testing.
+    BL31_SOURCES	+=	lib/psa/rss_platform.c \
+				drivers/arm/rss/rss_comms.c \
+				${RSS_COMMS_SOURCES}
+
+    PLAT_INCLUDES	+=	-Iinclude/lib/psa
+
+    $(eval $(call add_define,PLATFORM_TEST_NV_COUNTERS))
+else ifeq (${PLATFORM_TEST},rss-rotpk)
+    include drivers/arm/rss/rss_comms.mk
+
+    # Test code.
+    BL31_SOURCES	+=	plat/arm/board/tc/rotpk_test.c
+
+    # Code under testing.
+    BL31_SOURCES	+=	lib/psa/rss_platform.c \
+				drivers/arm/rss/rss_comms.c \
+				${RSS_COMMS_SOURCES}
+
+    PLAT_INCLUDES	+=	-Iinclude/lib/psa
+
+    $(eval $(call add_define,PLATFORM_TEST_ROTPK))
+else ifeq (${PLATFORM_TEST},tfm-testsuite)
 
     # The variables need to be set to compile the platform test:
     ifeq (${TF_M_TESTS_PATH},)
@@ -77,4 +107,6 @@
     $(eval $(call add_define,DELEG_ATTEST_DUMP_TOKEN_AND_KEY))
 
     $(eval $(call add_define,PLATFORM_TEST_TFM_TESTSUITE))
+else
+    $(error "Unsupported PLATFORM_TEST value")
 endif
diff --git a/plat/arm/board/tc/region_defs.h b/plat/arm/board/tc/region_defs.h
index d3dfd13..50ec2f1 100644
--- a/plat/arm/board/tc/region_defs.h
+++ b/plat/arm/board/tc/region_defs.h
@@ -7,6 +7,6 @@
 #ifndef REGION_DEFS_H
 #define REGION_DEFS_H
 
-#define PSA_INITIAL_ATTEST_TOKEN_MAX_SIZE   0x800
+#define PSA_INITIAL_ATTEST_MAX_TOKEN_SIZE   0x800
 
 #endif /* REGION_DEFS_H */
diff --git a/plat/arm/board/tc/rotpk_test.c b/plat/arm/board/tc/rotpk_test.c
new file mode 100644
index 0000000..ed56c31
--- /dev/null
+++ b/plat/arm/board/tc/rotpk_test.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+
+#include <drivers/arm/rss_comms.h>
+#include <plat/common/platform.h>
+#include <rss_platform_api.h>
+#include <tc_plat.h>
+
+static void print_hex(const char *key_id_name, size_t key_size, const uint8_t *key_buf)
+{
+	printf("%s = ", key_id_name);
+	for (int i = 0; i < key_size; i++) {
+		printf("%02x", key_buf[i]);
+	}
+	printf("\n\n");
+}
+
+int rotpk_test(void)
+{
+	psa_status_t status;
+	uint8_t key_buf[128];
+	size_t key_size;
+
+	struct key_id_info key_ids[3] = {
+	       {.key_id = RSS_BUILTIN_KEY_ID_HOST_S_ROTPK,  .key_id_name = "Secure-ROTPK"},
+	       {.key_id = RSS_BUILTIN_KEY_ID_HOST_NS_ROTPK,  .key_id_name = "NS-ROTPK"},
+	       {.key_id = RSS_BUILTIN_KEY_ID_HOST_CCA_ROTPK,  .key_id_name = "CCA-ROTPK"}
+	};
+
+	status = rss_comms_init(PLAT_RSS_AP_SND_MHU_BASE, PLAT_RSS_AP_RCV_MHU_BASE);
+	if (status != PSA_SUCCESS) {
+		printf("Failed to initialize RSS communication channel - psa_status = %d\n", status);
+		return -1;
+	}
+
+	for (int i = 0; i < ARRAY_SIZE(key_ids); i++) {
+		status = rss_platform_key_read(key_ids[i].key_id, key_buf,
+			       sizeof(key_buf), &key_size);
+		if (status != PSA_SUCCESS) {
+			printf("Failed to retrieve %s - psa_status = %d\n", key_ids[i].key_id_name, status);
+			return -1;
+		}
+		print_hex(key_ids[i].key_id_name, key_size, key_buf);
+	}
+
+	printf("Passed rotpk_test\n");
+
+	return 0;
+}
diff --git a/plat/arm/board/tc/rss_ap_tests.c b/plat/arm/board/tc/rss_ap_tests.c
index 8c40271..ea90ac3 100644
--- a/plat/arm/board/tc/rss_ap_tests.c
+++ b/plat/arm/board/tc/rss_ap_tests.c
@@ -56,7 +56,7 @@
 	return 0;
 }
 
-void run_platform_tests(void)
+int run_platform_tests(void)
 {
 	size_t i;
 	int ret;
diff --git a/plat/arm/board/tc/tc_bl1_measured_boot.c b/plat/arm/board/tc/tc_bl1_measured_boot.c
index 0d29c51..6d4bb07 100644
--- a/plat/arm/board/tc/tc_bl1_measured_boot.c
+++ b/plat/arm/board/tc/tc_bl1_measured_boot.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -46,7 +46,7 @@
 	(void)rss_comms_init(PLAT_RSS_AP_SND_MHU_BASE,
 			     PLAT_RSS_AP_RCV_MHU_BASE);
 
-	rss_measured_boot_init();
+	rss_measured_boot_init(tc_rss_mboot_metadata);
 }
 
 void bl1_plat_mboot_finish(void)
diff --git a/plat/arm/board/tc/tc_bl2_measured_boot.c b/plat/arm/board/tc/tc_bl2_measured_boot.c
index 7ea2c2e..9039853 100644
--- a/plat/arm/board/tc/tc_bl2_measured_boot.c
+++ b/plat/arm/board/tc/tc_bl2_measured_boot.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -45,7 +45,7 @@
 	(void)rss_comms_init(PLAT_RSS_AP_SND_MHU_BASE,
 			     PLAT_RSS_AP_RCV_MHU_BASE);
 
-	rss_measured_boot_init();
+	rss_measured_boot_init(tc_rss_mboot_metadata);
 }
 
 void bl2_plat_mboot_finish(void)
diff --git a/plat/arm/board/tc/tc_bl31_setup.c b/plat/arm/board/tc/tc_bl31_setup.c
index ca3a032..ff7809d 100644
--- a/plat/arm/board/tc/tc_bl31_setup.c
+++ b/plat/arm/board/tc/tc_bl31_setup.c
@@ -59,6 +59,8 @@
 
 #ifdef PLATFORM_TEST_NV_COUNTERS
 	tests_failed = nv_counter_test();
+#elif PLATFORM_TEST_ROTPK
+	tests_failed = rotpk_test();
 #elif PLATFORM_TEST_TFM_TESTSUITE
 	tests_failed = run_platform_tests();
 #endif
@@ -120,12 +122,9 @@
 {
 	/* Trusted Watchdog timer is the only source of Group0 interrupt now. */
 	if (intid == SBSA_SECURE_WDOG_INTID) {
-		INFO("Watchdog restarted\n");
 		/* Refresh the timer. */
 		plat_arm_secure_wdt_refresh();
 
-		/* Deactivate the corresponding interrupt. */
-		plat_ic_end_of_interrupt(intid);
 		return 0;
 	}
 
diff --git a/plat/arm/board/tc/tc_common_measured_boot.c b/plat/arm/board/tc/tc_common_measured_boot.c
index fe71899..eddcc81 100644
--- a/plat/arm/board/tc/tc_common_measured_boot.c
+++ b/plat/arm/board/tc/tc_common_measured_boot.c
@@ -1,6 +1,5 @@
-
 /*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,17 +12,13 @@
 
 extern struct rss_mboot_metadata tc_rss_mboot_metadata[];
 
-struct rss_mboot_metadata *plat_rss_mboot_get_metadata(void)
-{
-	return tc_rss_mboot_metadata;
-}
-
 int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data)
 {
 	int err;
 
 	/* Calculate image hash and record data in RSS */
-	err = rss_mboot_measure_and_record(image_data->image_base,
+	err = rss_mboot_measure_and_record(tc_rss_mboot_metadata,
+					   image_data->image_base,
 					   image_data->image_size,
 					   image_id);
 	if (err != 0) {
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 c6cf0e6..b9c785f 100644
--- a/plat/arm/css/sgi/include/sgi_base_platform_def.h
+++ b/plat/arm/css/sgi/include/sgi_base_platform_def.h
@@ -204,7 +204,11 @@
 					SOC_CSS_DEVICE_SIZE,	\
 					MT_DEVICE | MT_RW | MT_SECURE | MT_USER)
 
+#if RAS_FFH_SUPPORT
 #define PLAT_SP_PRI				PLAT_RAS_PRI
+#else
+#define PLAT_SP_PRI				0x10
+#endif
 
 #if SPM_MM && RAS_FFH_SUPPORT
 /*
diff --git a/plat/arm/css/sgi/include/sgi_ras.h b/plat/arm/css/sgi/include/sgi_ras.h
index e69a684..24bbfa7 100644
--- a/plat/arm/css/sgi/include/sgi_ras.h
+++ b/plat/arm/css/sgi/include/sgi_ras.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,15 +7,57 @@
 #ifndef SGI_RAS_H
 #define SGI_RAS_H
 
+#include <lib/extensions/ras.h>
+#include <plat/common/platform.h>
+
+/*
+ * Interrupt type supported.
+ * - SGI_RAS_INTR_TYPE_SPI: Denotes a SPI interrupt
+ * - SGI_RAS_INTR_TYPE_PPI: Denotes a PPI interrupt
+ */
+#define SGI_RAS_INTR_TYPE_SPI 0
+#define SGI_RAS_INTR_TYPE_PPI 1
+
 /*
- * Mapping the RAS interrupt with SDEI event number and the event
- * id used with Standalone MM code
+ * MM Communicate information structure. Required to generate MM Communicate
+ * payload to be shared with Standalone MM.
  */
+typedef struct mm_communicate_header {
+	struct efi_guid	header_guid;
+	size_t		message_len;
+	uint8_t		data[1];
+} mm_communicate_header_t;
+
+/* RAS error info data structure. */
 struct sgi_ras_ev_map {
 	int sdei_ev_num;	/* SDEI Event number */
 	int intr;		/* Physical intr number */
+	int intr_type;          /* Interrupt Type (SPI or PPI)*/
+};
+
+/* RAS config data structure. Must be defined by each platform. */
+struct plat_sgi_ras_config {
+	struct sgi_ras_ev_map *ev_map;
+	int ev_map_size;
 };
 
-int sgi_ras_intr_handler_setup(void);
+/*
+ * Find event map for a given interrupt number. On success, returns pointer
+ * to the event map. On error, returns NULL.
+ */
+struct sgi_ras_ev_map *sgi_find_ras_event_map_by_intr(uint32_t intr_num);
+
+/*
+ * Initialization function for the framework.
+ *
+ * Registers RAS config provided by the platform and then configures and
+ * enables interrupt for each registered error. On success, return 0.
+ */
+int sgi_ras_platform_setup(struct plat_sgi_ras_config *config);
+
+/* Base element RAM RAS interrupt handler function. */
+int sgi_ras_sram_intr_handler(const struct err_record_info *err_rec,
+				int probe_data,
+				const struct err_handler_data *const data);
 
 #endif /* SGI_RAS_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 acf31eb..d659ae5 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
@@ -58,6 +58,11 @@
 #define END_KEY_BASE			(SOC_KEYS_BASE + 0x0044)
 #define END_KEY_SIZE			U(32)
 
+/* Base Element RAM error definitions */
+#define SOC_NS_RAM_ERR_REC_BASE		UL(0x2A4C0000)
+#define NS_RAM_ECC_CE_INT		U(87)
+#define NS_RAM_ECC_UE_INT		U(88)
+
 #define SOC_PLATFORM_PERIPH_MAP_DEVICE	MAP_REGION_FLAT(			\
 						SOC_PLATFORM_PERIPH_BASE, 	\
 						SOC_PLATFORM_PERIPH_SIZE, 	\
diff --git a/plat/arm/css/sgi/ras/sgi_ras_common.c b/plat/arm/css/sgi/ras/sgi_ras_common.c
new file mode 100644
index 0000000..9789670
--- /dev/null
+++ b/plat/arm/css/sgi/ras/sgi_ras_common.c
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2018-2023, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <bl31/interrupt_mgmt.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+
+#include <sgi_ras.h>
+
+static struct plat_sgi_ras_config *sgi_ras_config;
+
+/*
+ * Find event map for a given interrupt number. On success, returns pointer to
+ * the event map. On error, returns NULL.
+ */
+struct sgi_ras_ev_map *sgi_find_ras_event_map_by_intr(uint32_t intr_num)
+{
+	struct sgi_ras_ev_map *map;
+	int size;
+	int i;
+
+	if (sgi_ras_config == NULL) {
+		ERROR("RAS config is NULL\n");
+		return NULL;
+	}
+
+	map = sgi_ras_config->ev_map;
+	size = sgi_ras_config->ev_map_size;
+
+	for (i = 0; i < size; i++) {
+		if (map->intr == intr_num)
+			return map;
+
+		map++;
+	}
+
+	return NULL;
+}
+
+/*
+ * Programs GIC registers and configures interrupt ID's as Group0 EL3
+ * interrupts. Current support is to register PPI and SPI interrupts.
+ */
+static void sgi_ras_intr_configure(int intr, int intr_type)
+{
+	plat_ic_set_interrupt_type(intr, INTR_TYPE_EL3);
+	plat_ic_set_interrupt_priority(intr, PLAT_RAS_PRI);
+	plat_ic_clear_interrupt_pending(intr);
+
+	/* Routing mode option available only for SPI interrupts */
+	if (intr_type == SGI_RAS_INTR_TYPE_SPI) {
+		plat_ic_set_spi_routing(intr, INTR_ROUTING_MODE_ANY,
+					(u_register_t)read_mpidr_el1());
+	}
+	plat_ic_enable_interrupt(intr);
+}
+
+/*
+ * Initialization function for the framework.
+ *
+ * Registers RAS config provided by the platform and then configures and
+ * enables interrupt for each registered error. On success, return 0.
+ */
+int sgi_ras_platform_setup(struct plat_sgi_ras_config *config)
+{
+	struct sgi_ras_ev_map *map;
+	int size;
+	int i;
+
+	/* Check if parameter is valid. */
+	if (config == NULL) {
+		ERROR("SGI: Failed to register RAS config\n");
+		return -1;
+	}
+
+	/*
+	 * Maintain a reference to the platform RAS config data for later
+	 * use.
+	 */
+	sgi_ras_config = config;
+
+	map = sgi_ras_config->ev_map;
+	size = sgi_ras_config->ev_map_size;
+
+	for (i = 0; i < size; i++) {
+		sgi_ras_intr_configure(map->intr, map->intr_type);
+		map++;
+	}
+
+	INFO("SGI: Platform RAS setup successful\n");
+
+	return 0;
+}
diff --git a/plat/arm/css/sgi/ras/sgi_ras_sram.c b/plat/arm/css/sgi/ras/sgi_ras_sram.c
new file mode 100644
index 0000000..b100700
--- /dev/null
+++ b/plat/arm/css/sgi/ras/sgi_ras_sram.c
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2023, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <bl31/interrupt_mgmt.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <plat/common/platform.h>
+#include <services/sdei.h>
+#include <services/spm_mm_svc.h>
+
+#include <platform_def.h>
+#include <sgi_ras.h>
+
+/* Base Element RAM Error Record offsets. */
+#define ERRSTATUS	U(0)
+#define ERRCODE		U(8)
+#define ERRADDR		U(12)
+
+/*
+ * Base Element RAM error information data structure communicated as part of MM
+ * Communication data payload.
+ */
+typedef struct sgi_sram_err_info {
+	uint32_t err_status;
+	uint32_t err_code;
+	uint32_t err_addr;
+} sgi_sram_err_info_t;
+
+/*
+ * MM Communicate message header GUID to indicate the payload is intended for
+ * base element RAM MM driver.
+ */
+struct efi_guid sram_ecc_event_guid = {
+	0x7312db4f, 0xd0c4, 0x4fb5,
+	{ 0x81, 0x2c, 0xb7, 0x4b, 0xc6, 0xc4, 0xa9, 0x38 }
+};
+
+/* Base element RAM RAS error interrupt handler */
+int sgi_ras_sram_intr_handler(const struct err_record_info *err_rec,
+				int probe_data,
+				const struct err_handler_data *const data)
+{
+	struct sgi_ras_ev_map *ras_map;
+	mm_communicate_header_t *header;
+	sgi_sram_err_info_t sram_info;
+	uintptr_t base_addr;
+	uint32_t clear_status, intr;
+	int ret;
+
+	cm_el1_sysregs_context_save(NON_SECURE);
+	intr = data->interrupt;
+
+	INFO("SGI: Base element RAM interrupt [%d] handler\n", intr);
+
+	/* Determine error record base address to read. */
+	base_addr = 0;
+	if (intr == NS_RAM_ECC_CE_INT || intr == NS_RAM_ECC_UE_INT) {
+		base_addr = SOC_NS_RAM_ERR_REC_BASE;
+	}
+	sram_info.err_status = mmio_read_32(base_addr + ERRSTATUS);
+	sram_info.err_code = mmio_read_32(base_addr + ERRCODE);
+	sram_info.err_addr = mmio_read_32(base_addr + ERRADDR);
+
+	/* Clear the interrupt. */
+	clear_status = mmio_read_32(base_addr + ERRSTATUS);
+	mmio_write_32((base_addr + ERRSTATUS), clear_status);
+
+	/*
+	 * Prepare the MM Communication buffer to pass the base element RAM
+	 * error information to Secure Partition.
+	 */
+	header = (void *)PLAT_SPM_BUF_BASE;
+	memset(header, 0, sizeof(*header));
+	memcpy(&header->data, &sram_info, sizeof(sram_info));
+	header->message_len = sizeof(sram_info);
+	memcpy(&header->header_guid, (void *)&sram_ecc_event_guid,
+		sizeof(struct efi_guid));
+
+	spm_mm_sp_call(MM_COMMUNICATE_AARCH64, (uint64_t)header, 0,
+			plat_my_core_pos());
+
+	plat_ic_end_of_interrupt(intr);
+
+	/*
+	 * Find if this is a RAS interrupt. There must be an event against
+	 * this interrupt
+	 */
+	ras_map = sgi_find_ras_event_map_by_intr(intr);
+	if (ras_map == NULL) {
+		ERROR("SGI: RAS error info for interrupt id: %d not found\n",
+			intr);
+		return -1;
+	}
+
+	/* Dispatch the event to the SDEI client */
+	ret = sdei_dispatch_event(ras_map->sdei_ev_num);
+	if (ret != 0) {
+		/*
+		 * sdei_dispatch_event() may return failing result in some
+		 * cases, for example kernel may not have registered a handler
+		 * or RAS event may happen early during boot. We restore the NS
+		 * context when sdei_dispatch_event() returns failing result.
+		 */
+		ERROR("SDEI dispatch failed: %d", ret);
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+	}
+
+	return ret;
+}
diff --git a/plat/arm/css/sgi/sgi-common.mk b/plat/arm/css/sgi/sgi-common.mk
index 6d17bc2..358316c 100644
--- a/plat/arm/css/sgi/sgi-common.mk
+++ b/plat/arm/css/sgi/sgi-common.mk
@@ -54,10 +54,6 @@
 				${CSS_ENT_BASE}/sgi_bl31_setup.c	\
 				${CSS_ENT_BASE}/sgi_topology.c
 
-ifeq (${RAS_FFH_SUPPORT},1)
-BL31_SOURCES		+=	${CSS_ENT_BASE}/sgi_ras.c
-endif
-
 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.")
diff --git a/plat/arm/css/sgi/sgi_bl31_setup.c b/plat/arm/css/sgi/sgi_bl31_setup.c
index 9c8d163..7aa7b34 100644
--- a/plat/arm/css/sgi/sgi_bl31_setup.c
+++ b/plat/arm/css/sgi/sgi_bl31_setup.c
@@ -106,10 +106,6 @@
 {
 	arm_bl31_platform_setup();
 
-#if RAS_FFH_SUPPORT
-	sgi_ras_intr_handler_setup();
-#endif
-
 	/* Configure the warm reboot SGI for primary core */
 	css_setup_cpu_pwr_down_intr();
 
diff --git a/plat/arm/css/sgi/sgi_plat_v2.c b/plat/arm/css/sgi/sgi_plat_v2.c
index 8d38108..8de0efe 100644
--- a/plat/arm/css/sgi/sgi_plat_v2.c
+++ b/plat/arm/css/sgi/sgi_plat_v2.c
@@ -87,6 +87,9 @@
 	SOC_PLATFORM_PERIPH_MAP_DEVICE_USER,
 	ARM_SP_IMAGE_MMAP,
 	ARM_SP_IMAGE_NS_BUF_MMAP,
+#if RAS_FFH_SUPPORT
+	CSS_SGI_SP_CPER_BUF_MMAP,
+#endif
 	ARM_SP_IMAGE_RW_MMAP,
 	ARM_SPM_BUF_EL0_MMAP,
 	{0}
diff --git a/plat/arm/css/sgi/sgi_ras.c b/plat/arm/css/sgi/sgi_ras.c
deleted file mode 100644
index 4f03ac4..0000000
--- a/plat/arm/css/sgi/sgi_ras.c
+++ /dev/null
@@ -1,194 +0,0 @@
-/*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <string.h>
-
-#include <bl31/interrupt_mgmt.h>
-#include <lib/el3_runtime/context_mgmt.h>
-#include <lib/extensions/ras.h>
-#include <plat/arm/common/arm_spm_def.h>
-#include <plat/common/platform.h>
-#include <services/sdei.h>
-#include <services/spm_mm_svc.h>
-
-#include <sgi_ras.h>
-
-static int sgi_ras_intr_handler(const struct err_record_info *err_rec,
-				int probe_data,
-				const struct err_handler_data *const data);
-typedef struct mm_communicate_header {
-	struct efi_guid	header_guid;
-	size_t		message_len;
-	uint8_t		data[8];
-} mm_communicate_header_t;
-
-/*
- * GUID to indicate that the MM communication message is intended for DMC-620
- * MM driver.
- */
-const struct efi_guid dmc620_ecc_event_guid = {
-	0x5ef0afd5, 0xe01a, 0x4c30,
-	{0x86, 0x19, 0x45, 0x46, 0x26, 0x91, 0x80, 0x98}
-};
-
-struct sgi_ras_ev_map sgi575_ras_map[] = {
-
-	/* DMC 0 error ECC error interrupt*/
-	{SGI_SDEI_DS_EVENT_0, 35},
-
-	/* DMC 1 error ECC error interrupt*/
-	{SGI_SDEI_DS_EVENT_1, 39},
-};
-
-#define SGI575_RAS_MAP_SIZE	ARRAY_SIZE(sgi575_ras_map)
-
-struct err_record_info sgi_err_records[] = {
-	{
-		/* DMC 0 error record info */
-		.handler = &sgi_ras_intr_handler,
-		.aux_data = (void *)0,
-	}, {
-		/* DMC 1 error record info */
-		.handler = &sgi_ras_intr_handler,
-		.aux_data = (void *)1,
-	},
-};
-
-struct ras_interrupt sgi_ras_interrupts[] = {
-	{
-		.intr_number = 35,
-		.err_record = &sgi_err_records[0],
-	}, {
-		.intr_number = 39,
-		.err_record = &sgi_err_records[1],
-	}
-};
-
-REGISTER_ERR_RECORD_INFO(sgi_err_records);
-REGISTER_RAS_INTERRUPTS(sgi_ras_interrupts);
-
-static struct sgi_ras_ev_map *plat_sgi_get_ras_ev_map(void)
-{
-	return sgi575_ras_map;
-}
-
-static int plat_sgi_get_ras_ev_map_size(void)
-{
-	return SGI575_RAS_MAP_SIZE;
-}
-
-/*
- * Find event mapping for a given interrupt number: On success, returns pointer
- * to the event mapping. On error, returns NULL.
- */
-static struct sgi_ras_ev_map *find_ras_event_map_by_intr(uint32_t intr_num)
-{
-	struct sgi_ras_ev_map *map = plat_sgi_get_ras_ev_map();
-	int i;
-	int size = plat_sgi_get_ras_ev_map_size();
-
-	for (i = 0; i < size; i++) {
-		if (map->intr == intr_num)
-			return map;
-
-		map++;
-	}
-
-	return NULL;
-}
-
-static void sgi_ras_intr_configure(int intr)
-{
-	plat_ic_set_interrupt_type(intr, INTR_TYPE_EL3);
-	plat_ic_set_interrupt_priority(intr, PLAT_RAS_PRI);
-	plat_ic_clear_interrupt_pending(intr);
-	plat_ic_set_spi_routing(intr, INTR_ROUTING_MODE_ANY,
-				(u_register_t)read_mpidr_el1());
-	plat_ic_enable_interrupt(intr);
-}
-
-static int sgi_ras_intr_handler(const struct err_record_info *err_rec,
-				int probe_data,
-				const struct err_handler_data *const data)
-{
-	struct sgi_ras_ev_map *ras_map;
-	mm_communicate_header_t *header;
-	uint32_t intr;
-	int ret;
-
-	cm_el1_sysregs_context_save(NON_SECURE);
-	intr = data->interrupt;
-
-	/*
-	 * Find if this is a RAS interrupt. There must be an event against
-	 * this interrupt
-	 */
-	ras_map = find_ras_event_map_by_intr(intr);
-	assert(ras_map != NULL);
-
-	/*
-	 * Populate the MM_COMMUNICATE payload to share the
-	 * event info with StandaloneMM code. This allows us to use
-	 * MM_COMMUNICATE as a common entry mechanism into S-EL0. The
-	 * header data will be parsed in StandaloneMM to process the
-	 * corresponding event.
-	 *
-	 * TBD - Currently, the buffer allocated by SPM for communication
-	 * between EL3 and S-EL0 is being used(PLAT_SPM_BUF_BASE). But this
-	 * should happen via a dynamic mem allocation, which should be
-	 * managed by SPM -- the individual platforms then call the mem
-	 * alloc api to get memory for the payload.
-	 */
-	header = (void *) PLAT_SPM_BUF_BASE;
-	memset(header, 0, sizeof(*header));
-	memcpy(&header->data, &err_rec->aux_data, sizeof(err_rec->aux_data));
-	header->message_len = sizeof(err_rec->aux_data);
-	memcpy(&header->header_guid, (void *) &dmc620_ecc_event_guid,
-			sizeof(const struct efi_guid));
-
-	spm_mm_sp_call(MM_COMMUNICATE_AARCH64, (uint64_t)header, 0,
-		       plat_my_core_pos());
-
-	/*
-	 * Do an EOI of the RAS interrupt. This allows the
-	 * sdei event to be dispatched at the SDEI event's
-	 * priority.
-	 */
-	plat_ic_end_of_interrupt(intr);
-
-	/* Dispatch the event to the SDEI client */
-	ret = sdei_dispatch_event(ras_map->sdei_ev_num);
-	if (ret != 0) {
-		/*
-		 * sdei_dispatch_event() may return failing result in some cases,
-		 * for example kernel may not have registered a handler or RAS event
-		 * may happen early during boot. We restore the NS context when
-		 * sdei_dispatch_event() returns failing result.
-		 */
-		ERROR("SDEI dispatch failed: %d", ret);
-		cm_el1_sysregs_context_restore(NON_SECURE);
-		cm_set_next_eret_context(NON_SECURE);
-	}
-
-	return ret;
-}
-
-int sgi_ras_intr_handler_setup(void)
-{
-	int i;
-	struct sgi_ras_ev_map *map = plat_sgi_get_ras_ev_map();
-	int size = plat_sgi_get_ras_ev_map_size();
-
-	for (i = 0; i < size; i++) {
-		sgi_ras_intr_configure(map->intr);
-		map++;
-	}
-
-	INFO("SGI: RAS Interrupt Handler successfully registered\n");
-
-	return 0;
-}
diff --git a/plat/brcm/board/stingray/platform.mk b/plat/brcm/board/stingray/platform.mk
index aa2fe86..67413c6 100644
--- a/plat/brcm/board/stingray/platform.mk
+++ b/plat/brcm/board/stingray/platform.mk
@@ -210,7 +210,8 @@
 
 BL2_SOURCES		+=	plat/${SOC_DIR}/driver/ihost_pll_config.c \
 				plat/${SOC_DIR}/src/bl2_setup.c \
-				plat/${SOC_DIR}/driver/swreg.c
+				plat/${SOC_DIR}/driver/swreg.c \
+				lib/cpus/aarch64/cpu_helpers.S
 
 ifeq (${USE_DDR},yes)
 PLAT_INCLUDES		+=	-Iplat/${SOC_DIR}/driver/ddr/soc/include
diff --git a/plat/common/aarch64/plat_ehf.c b/plat/common/aarch64/plat_ehf.c
index 4ec69b1..be0fac1 100644
--- a/plat/common/aarch64/plat_ehf.c
+++ b/plat/common/aarch64/plat_ehf.c
@@ -24,9 +24,17 @@
 	/* Normal priority SDEI */
 	EHF_PRI_DESC(PLAT_PRI_BITS, PLAT_SDEI_NORMAL_PRI),
 #endif
+
 #if SPM_MM
+#if RAS_FFH_SUPPORT
+#if (PLAT_SP_PRI != PLAT_RAS_PRI)
 	EHF_PRI_DESC(PLAT_PRI_BITS, PLAT_SP_PRI),
 #endif
+#else
+	EHF_PRI_DESC(PLAT_PRI_BITS, PLAT_SP_PRI),
+#endif
+#endif
+
 	/* Platform specific exceptions description */
 #ifdef PLAT_EHF_DESC
 	PLAT_EHF_DESC,
diff --git a/plat/imx/imx8m/imx8m_ccm.c b/plat/imx/imx8m/imx8m_ccm.c
new file mode 100644
index 0000000..10a00c9
--- /dev/null
+++ b/plat/imx/imx8m/imx8m_ccm.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2023, Pengutronix. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#define UCR1    		0x80
+#define UCR1_UARTEN		BIT(0)
+#define DOMAIN0_RUNNING(d)	(((d) & 0x3) != 0)
+
+static struct imx_uart {
+	unsigned int ccm_reg;
+	unsigned int uart_base;
+} imx8m_uart_info[] = {
+	{	/* UART 1 */
+		.ccm_reg = 0x4490,
+		.uart_base = 0x30860000,
+	}, {	/* UART 2 */
+		.ccm_reg = 0x44a0,
+		.uart_base = 0x30890000,
+	}, {	/* UART 3 */
+		.ccm_reg = 0x44b0,
+		.uart_base = 0x30880000,
+	}, {	/* UART 4 */
+		.ccm_reg = 0x44c0,
+		.uart_base = 0x30a60000,
+	}
+};
+
+unsigned int imx8m_uart_get_base(void)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx8m_uart_info); i++) {
+		uint32_t val;
+
+		/*
+		 * At least check that the clock-gate is ungated before we
+		 * access the UART register.
+		 */
+		val = mmio_read_32(IMX_CCM_BASE + imx8m_uart_info[i].ccm_reg);
+		if (DOMAIN0_RUNNING(val)) {
+			val = mmio_read_32(imx8m_uart_info[i].uart_base + UCR1);
+			if (val & UCR1_UARTEN) {
+				return imx8m_uart_info[i].uart_base;
+			}
+		}
+	}
+
+	/*
+	 * We should return an error and inform the user but we can't do it
+	 * this early.
+	 */
+	return 0;
+}
diff --git a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
index 67bfd36..8e66361 100644
--- a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
@@ -27,6 +27,7 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_ccm.h>
 #include <imx8m_csu.h>
 #include <plat_imx8.h>
 
@@ -130,6 +131,7 @@
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 		u_register_t arg2, u_register_t arg3)
 {
+	unsigned int console_base = 0U;
 	static console_t console;
 	int i;
 
@@ -144,7 +146,14 @@
 
 	imx_csu_init(csu_cfg);
 
-	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
+#if IMX_BOOT_UART_BASE
+	console_base = IMX_BOOT_UART_BASE;
+#endif
+	if (console_base == 0U) {
+		console_base = imx8m_uart_get_base();
+	}
+
+	console_imx_uart_register(console_base, IMX_BOOT_UART_CLK_IN_HZ,
 		IMX_CONSOLE_BAUDRATE, &console);
 	/* This console is only used for boot stage */
 	console_set_scope(&console, CONSOLE_FLAG_BOOT);
diff --git a/plat/imx/imx8m/imx8mm/platform.mk b/plat/imx/imx8m/imx8mm/platform.mk
index 24582f8..2462517 100644
--- a/plat/imx/imx8m/imx8mm/platform.mk
+++ b/plat/imx/imx8m/imx8mm/platform.mk
@@ -37,6 +37,7 @@
 				plat/imx/imx8m/imx_rdc.c			\
 				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
+				plat/imx/imx8m/imx8m_ccm.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c	\
 				plat/imx/imx8m/imx8mm/imx8mm_psci.c		\
diff --git a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
index eff198d..aeb1cbf 100644
--- a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
@@ -25,6 +25,7 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_ccm.h>
 #include <imx8m_csu.h>
 #include <platform_def.h>
 #include <plat_imx8.h>
@@ -121,6 +122,7 @@
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 		u_register_t arg2, u_register_t arg3)
 {
+	unsigned int console_base = 0U;
 	static console_t console;
 	unsigned int val;
 	int i;
@@ -141,7 +143,14 @@
 	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
 	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
 
-	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
+#if IMX_BOOT_UART_BASE
+	console_base = IMX_BOOT_UART_BASE;
+#endif
+	if (console_base == 0U) {
+		console_base = imx8m_uart_get_base();
+	}
+
+	console_imx_uart_register(console_base, IMX_BOOT_UART_CLK_IN_HZ,
 		IMX_CONSOLE_BAUDRATE, &console);
 	/* This console is only used for boot stage */
 	console_set_scope(&console, CONSOLE_FLAG_BOOT);
diff --git a/plat/imx/imx8m/imx8mn/platform.mk b/plat/imx/imx8m/imx8mn/platform.mk
index 1c0ad4f..4651610 100644
--- a/plat/imx/imx8m/imx8mn/platform.mk
+++ b/plat/imx/imx8m/imx8mn/platform.mk
@@ -31,6 +31,7 @@
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
+				plat/imx/imx8m/imx8m_ccm.c			\
 				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c	\
diff --git a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
index 4c31fa2..e25668b 100644
--- a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
@@ -25,6 +25,7 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_ccm.h>
 #include <imx8m_csu.h>
 #include <platform_def.h>
 #include <plat_imx8.h>
@@ -117,6 +118,7 @@
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 		u_register_t arg2, u_register_t arg3)
 {
+	unsigned int console_base = 0U;
 	static console_t console;
 	unsigned int val;
 	unsigned int i;
@@ -137,7 +139,14 @@
 	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
 	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
 
-	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
+#if IMX_BOOT_UART_BASE
+	console_base = IMX_BOOT_UART_BASE;
+#endif
+	if (console_base == 0U) {
+		console_base = imx8m_uart_get_base();
+	}
+
+	console_imx_uart_register(console_base, IMX_BOOT_UART_CLK_IN_HZ,
 		IMX_CONSOLE_BAUDRATE, &console);
 	/* This console is only used for boot stage */
 	console_set_scope(&console, CONSOLE_FLAG_BOOT);
diff --git a/plat/imx/imx8m/imx8mp/platform.mk b/plat/imx/imx8m/imx8mp/platform.mk
index 1102316..21836d9 100644
--- a/plat/imx/imx8m/imx8mp/platform.mk
+++ b/plat/imx/imx8m/imx8mp/platform.mk
@@ -32,6 +32,7 @@
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
+				plat/imx/imx8m/imx8m_ccm.c			\
 				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c	\
diff --git a/plat/imx/imx8m/include/imx8m_ccm.h b/plat/imx/imx8m/include/imx8m_ccm.h
new file mode 100644
index 0000000..acbd135
--- /dev/null
+++ b/plat/imx/imx8m/include/imx8m_ccm.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2023, Pengutronix. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX8M_CCM_H
+#define IMX8M_CCM_H
+
+unsigned int imx8m_uart_get_base(void);
+
+#endif /* IMX8M_CCM_H */
diff --git a/plat/imx/imx93/aarch64/plat_helpers.S b/plat/imx/imx93/aarch64/plat_helpers.S
new file mode 100644
index 0000000..9987a53
--- /dev/null
+++ b/plat/imx/imx93/aarch64/plat_helpers.S
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2022-2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include <cortex_a55.h>
+
+#include <platform_def.h>
+
+	.globl	plat_is_my_cpu_primary
+	.globl	plat_my_core_pos
+	.globl	plat_calc_core_pos
+	.globl	platform_mem_init
+
+	/* ------------------------------------------------------
+	 * Helper macro that reads the part number of the current
+	 * CPU and jumps to the given label if it matches the CPU
+	 * MIDR provided.
+	 *
+	 * Clobbers x0.
+	 * ------------------------------------------------------
+	 */
+	.macro  jump_if_cpu_midr _cpu_midr, _label
+
+	mrs	x0, midr_el1
+	ubfx	x0, x0, MIDR_PN_SHIFT, #12
+	cmp     w0, #((\_cpu_midr >> MIDR_PN_SHIFT) & MIDR_PN_MASK)
+	b.eq	\_label
+
+	.endm
+
+	/* ----------------------------------------------
+	 * unsigned int plat_is_my_cpu_primary(void);
+	 * This function checks if this is the primary CPU
+	 * ----------------------------------------------
+	 */
+func plat_is_my_cpu_primary
+	mrs	x0, mpidr_el1
+	mov_imm x1, MPIDR_AFFINITY_MASK
+	and	x0, x0, x1
+	cmp	x0, #PLAT_PRIMARY_CPU
+	cset	x0, eq
+	ret
+endfunc plat_is_my_cpu_primary
+
+	/* ----------------------------------------------
+	 * unsigned int plat_my_core_pos(void)
+	 * This function uses the plat_calc_core_pos()
+	 * to get the index of the calling CPU.
+	 * ----------------------------------------------
+	 */
+func plat_my_core_pos
+	mrs	x0, mpidr_el1
+	mov	x1, #MPIDR_AFFLVL_MASK
+	and	x0, x1, x0, lsr #MPIDR_AFF1_SHIFT
+	ret
+endfunc plat_my_core_pos
+
+	/*
+	 * unsigned int plat_calc_core_pos(uint64_t mpidr)
+	 * helper function to calculate the core position.
+	 * With this function.
+	 */
+func plat_calc_core_pos
+	mov	x1, #MPIDR_AFFLVL_MASK
+	and	x0, x1, x0, lsr #MPIDR_AFF1_SHIFT
+	ret
+endfunc plat_calc_core_pos
+
+func platform_mem_init
+	ret
+endfunc platform_mem_init
diff --git a/plat/imx/imx93/imx93_bl31_setup.c b/plat/imx/imx93/imx93_bl31_setup.c
new file mode 100644
index 0000000..8458f6c
--- /dev/null
+++ b/plat/imx/imx93/imx93_bl31_setup.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright 2022-2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <context.h>
+#include <drivers/console.h>
+#include <drivers/generic_delay_timer.h>
+#include <drivers/nxp/trdc/imx_trdc.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
+
+#include <imx8_lpuart.h>
+#include <plat_imx8.h>
+#include <platform_def.h>
+
+#define MAP_BL31_TOTAL										   \
+	MAP_REGION_FLAT(BL31_BASE, BL31_LIMIT - BL31_BASE, MT_MEMORY | MT_RW | MT_SECURE)
+#define MAP_BL31_RO										   \
+	MAP_REGION_FLAT(BL_CODE_BASE, BL_CODE_END - BL_CODE_BASE, MT_MEMORY | MT_RO | MT_SECURE)
+
+static const mmap_region_t imx_mmap[] = {
+	AIPS1_MAP, AIPS2_MAP, AIPS4_MAP, GIC_MAP,
+	TRDC_A_MAP, TRDC_W_MAP, TRDC_M_MAP,
+	TRDC_N_MAP,
+	{0},
+};
+
+static entry_point_info_t bl32_image_ep_info;
+static entry_point_info_t bl33_image_ep_info;
+
+/* get SPSR for BL33 entry */
+static uint32_t get_spsr_for_bl33_entry(void)
+{
+	unsigned long el_status;
+	unsigned long mode;
+	uint32_t spsr;
+
+	/* figure out what mode we enter the non-secure world */
+	el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
+	el_status &= ID_AA64PFR0_ELX_MASK;
+
+	mode = (el_status) ? MODE_EL2 : MODE_EL1;
+
+	spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	return spsr;
+}
+
+void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
+		u_register_t arg2, u_register_t arg3)
+{
+	static console_t console;
+
+	console_lpuart_register(IMX_LPUART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
+		     IMX_CONSOLE_BAUDRATE, &console);
+
+	/* This console is only used for boot stage */
+	console_set_scope(&console, CONSOLE_FLAG_BOOT);
+
+	/*
+	 * tell BL3-1 where the non-secure software image is located
+	 * and the entry state information.
+	 */
+	bl33_image_ep_info.pc = PLAT_NS_IMAGE_OFFSET;
+	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
+	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
+
+#if defined(SPD_opteed)
+	/* 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);
+	bl32_image_ep_info.pc = BL32_BASE;
+	bl32_image_ep_info.spsr = 0;
+
+	/* Pass TEE base and size to bl33 */
+	bl33_image_ep_info.args.arg1 = BL32_BASE;
+	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+	/* 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
+}
+
+void bl31_plat_arch_setup(void)
+{
+	/* no coherence memory support on i.MX9 */
+	const mmap_region_t bl_regions[] = {
+		MAP_BL31_TOTAL,
+		MAP_BL31_RO,
+	};
+
+	/* Assign all the GPIO pins to non-secure world by default */
+	mmio_write_32(GPIO2_BASE + 0x10, 0xffffffff);
+	mmio_write_32(GPIO2_BASE + 0x14, 0x3);
+	mmio_write_32(GPIO2_BASE + 0x18, 0xffffffff);
+	mmio_write_32(GPIO2_BASE + 0x1c, 0x3);
+
+	mmio_write_32(GPIO3_BASE + 0x10, 0xffffffff);
+	mmio_write_32(GPIO3_BASE + 0x14, 0x3);
+	mmio_write_32(GPIO3_BASE + 0x18, 0xffffffff);
+	mmio_write_32(GPIO3_BASE + 0x1c, 0x3);
+
+	mmio_write_32(GPIO4_BASE + 0x10, 0xffffffff);
+	mmio_write_32(GPIO4_BASE + 0x14, 0x3);
+	mmio_write_32(GPIO4_BASE + 0x18, 0xffffffff);
+	mmio_write_32(GPIO4_BASE + 0x1c, 0x3);
+
+	mmio_write_32(GPIO1_BASE + 0x10, 0xffffffff);
+	mmio_write_32(GPIO1_BASE + 0x14, 0x3);
+	mmio_write_32(GPIO1_BASE + 0x18, 0xffffffff);
+	mmio_write_32(GPIO1_BASE + 0x1c, 0x3);
+
+	setup_page_tables(bl_regions, imx_mmap);
+	enable_mmu_el3(0);
+
+	/* trdc must be initialized */
+	trdc_config();
+}
+
+void bl31_platform_setup(void)
+{
+	generic_delay_timer_init();
+
+	plat_gic_driver_init();
+	plat_gic_init();
+}
+
+void bl31_plat_runtime_setup(void)
+{
+	console_switch_state(CONSOLE_FLAG_RUNTIME);
+}
+
+entry_point_info_t *bl31_plat_get_next_image_ep_info(unsigned int type)
+{
+	if (type == NON_SECURE) {
+		return &bl33_image_ep_info;
+	}
+
+	if (type == SECURE) {
+		return &bl32_image_ep_info;
+	}
+
+	return NULL;
+}
+
+unsigned int plat_get_syscnt_freq2(void)
+{
+	return COUNTER_FREQUENCY;
+}
diff --git a/plat/imx/imx93/imx93_psci.c b/plat/imx/imx93/imx93_psci.c
new file mode 100644
index 0000000..5e1fa95
--- /dev/null
+++ b/plat/imx/imx93/imx93_psci.c
@@ -0,0 +1,253 @@
+/*
+ * Copyright 2022-2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/psci/psci.h>
+
+#include <plat_imx8.h>
+#include <pwr_ctrl.h>
+
+#define CORE_PWR_STATE(state) ((state)->pwr_domain_state[MPIDR_AFFLVL0])
+#define CLUSTER_PWR_STATE(state) ((state)->pwr_domain_state[MPIDR_AFFLVL1])
+#define SYSTEM_PWR_STATE(state) ((state)->pwr_domain_state[PLAT_MAX_PWR_LVL])
+
+/* platform secure warm boot entry */
+static uintptr_t secure_entrypoint;
+
+static bool boot_stage = true;
+
+int imx_validate_ns_entrypoint(uintptr_t ns_entrypoint)
+{
+	/* The non-secure entrypoint should be in RAM space */
+	if (ns_entrypoint < PLAT_NS_IMAGE_OFFSET) {
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	return PSCI_E_SUCCESS;
+}
+
+int imx_validate_power_state(unsigned int power_state,
+			 psci_power_state_t *req_state)
+{
+	int pwr_lvl = psci_get_pstate_pwrlvl(power_state);
+	int pwr_type = psci_get_pstate_type(power_state);
+	int state_id = psci_get_pstate_id(power_state);
+
+	if (pwr_lvl > PLAT_MAX_PWR_LVL) {
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	if (pwr_type == PSTATE_TYPE_STANDBY) {
+		CORE_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
+		CLUSTER_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
+	}
+
+	if (pwr_type == PSTATE_TYPE_POWERDOWN && state_id == 0x33) {
+		CORE_PWR_STATE(req_state) = PLAT_MAX_OFF_STATE;
+		CLUSTER_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
+	}
+
+	return PSCI_E_SUCCESS;
+}
+
+void imx_set_cpu_boot_entry(unsigned int core_id, uint64_t boot_entry)
+{
+	/* set the cpu core reset entry: BLK_CTRL_S */
+	mmio_write_32(BLK_CTRL_S_BASE + CA55_RVBADDR0_L + core_id * 8, boot_entry >> 2);
+}
+
+int imx_pwr_domain_on(u_register_t mpidr)
+{
+	unsigned int core_id;
+
+	core_id = MPIDR_AFFLVL1_VAL(mpidr);
+
+	imx_set_cpu_boot_entry(core_id, secure_entrypoint);
+
+	/*
+	 * When the core is first time boot up, the core is already ON after SoC POR,
+	 * So 'SW_WAKEUP' can not work, so need to toggle core's reset then release
+	 * the core from cpu_wait.
+	 */
+	if (boot_stage) {
+		/* assert CPU core SW reset */
+		mmio_clrbits_32(SRC_SLICE(SRC_A55C0 + core_id) + 0x24, BIT(2) | BIT(0));
+		/* deassert CPU core SW reset */
+		mmio_setbits_32(SRC_SLICE(SRC_A55C0 + core_id) + 0x24, BIT(2) | BIT(0));
+		/* release the cpuwait to kick the cpu */
+		mmio_clrbits_32(BLK_CTRL_S_BASE + CA55_CPUWAIT, BIT(core_id));
+	} else {
+		/* assert the CMC MISC SW WAKEUP BIT to kick the offline core */
+		gpc_assert_sw_wakeup(CPU_A55C0 + core_id);
+	}
+
+	return PSCI_E_SUCCESS;
+}
+
+void imx_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int core_id = MPIDR_AFFLVL1_VAL(mpidr);
+
+	plat_gic_pcpu_init();
+	plat_gic_cpuif_enable();
+
+	/* below config is ok both for boot & hotplug */
+	/* clear the CPU power mode */
+	gpc_set_cpu_mode(CPU_A55C0 + core_id, CM_MODE_RUN);
+	/* clear the SW wakeup */
+	gpc_deassert_sw_wakeup(CPU_A55C0 + core_id);
+	/* switch to GIC wakeup source */
+	gpc_select_wakeup_gic(CPU_A55C0 + core_id);
+
+	if (boot_stage) {
+		/* SRC config */
+		/* config the MEM LPM */
+		src_mem_lpm_en(SRC_A55P0_MEM + core_id, MEM_OFF);
+		/* LPM config to only ON in run mode to its domain */
+		src_mix_set_lpm(SRC_A55C0 + core_id, core_id, CM_MODE_WAIT);
+		/* white list config, only enable its own domain */
+		src_authen_config(SRC_A55C0 + core_id, 1 << core_id, 0x1);
+
+		boot_stage = false;
+	}
+}
+
+void imx_pwr_domain_off(const psci_power_state_t *target_state)
+{
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int core_id = MPIDR_AFFLVL1_VAL(mpidr);
+	unsigned int i;
+
+	plat_gic_cpuif_disable();
+	write_clusterpwrdn(DSU_CLUSTER_PWR_OFF);
+
+	/*
+	 * mask all the GPC IRQ wakeup to make sure no IRQ can wakeup this core
+	 * as we need to use SW_WAKEUP for hotplug purpose
+	 */
+	for (i = 0U; i < IMR_NUM; i++) {
+		gpc_set_irq_mask(CPU_A55C0 + core_id, i, 0xffffffff);
+	}
+	/* switch to GPC wakeup source */
+	gpc_select_wakeup_raw_irq(CPU_A55C0 + core_id);
+	/* config the target mode to suspend */
+	gpc_set_cpu_mode(CPU_A55C0 + core_id, CM_MODE_SUSPEND);
+}
+
+void imx_pwr_domain_suspend(const psci_power_state_t *target_state)
+{
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int core_id = MPIDR_AFFLVL1_VAL(mpidr);
+
+	/* do cpu level config */
+	if (is_local_state_off(CORE_PWR_STATE(target_state))) {
+		plat_gic_cpuif_disable();
+		imx_set_cpu_boot_entry(core_id, secure_entrypoint);
+		/* config the target mode to WAIT */
+		gpc_set_cpu_mode(CPU_A55C0 + core_id, CM_MODE_WAIT);
+	}
+
+	/* do cluster level config */
+	if (!is_local_state_run(CLUSTER_PWR_STATE(target_state))) {
+		/* config the A55 cluster target mode to WAIT */
+		gpc_set_cpu_mode(CPU_A55_PLAT, CM_MODE_WAIT);
+
+		/* config DSU for cluster power down with L3 MEM RET */
+		if (is_local_state_retn(CLUSTER_PWR_STATE(target_state))) {
+			write_clusterpwrdn(DSU_CLUSTER_PWR_OFF | BIT(1));
+		}
+	}
+}
+
+void imx_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
+{
+	uint64_t mpidr = read_mpidr_el1();
+	unsigned int core_id = MPIDR_AFFLVL1_VAL(mpidr);
+
+	/* cluster level */
+	if (!is_local_state_run(CLUSTER_PWR_STATE(target_state))) {
+		/* set the cluster's target mode to RUN */
+		gpc_set_cpu_mode(CPU_A55_PLAT, CM_MODE_RUN);
+	}
+
+	/* do core level */
+	if (is_local_state_off(CORE_PWR_STATE(target_state))) {
+		/* set A55 CORE's power mode to RUN */
+		gpc_set_cpu_mode(CPU_A55C0 + core_id, CM_MODE_RUN);
+		plat_gic_cpuif_enable();
+	}
+}
+
+void imx_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	unsigned int i;
+
+	for (i = IMX_PWR_LVL0; i <= PLAT_MAX_PWR_LVL; i++) {
+		req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
+	}
+
+	SYSTEM_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
+	CLUSTER_PWR_STATE(req_state) = PLAT_MAX_RET_STATE;
+}
+
+void __dead2 imx_system_reset(void)
+{
+	mmio_write_32(WDOG3_BASE + WDOG_CNT, 0xd928c520);
+	while ((mmio_read_32(WDOG3_BASE + WDOG_CS) & WDOG_CS_ULK) == 0U) {
+		;
+	}
+
+	mmio_write_32(WDOG3_BASE + WDOG_TOVAL, 0x10);
+	mmio_write_32(WDOG3_BASE + WDOG_CS, 0x21e3);
+
+	while (1) {
+		wfi();
+	}
+}
+
+void __dead2 imx_system_off(void)
+{
+	mmio_setbits_32(BBNSM_BASE + BBNSM_CTRL, BBNSM_DP_EN | BBNSM_TOSP);
+
+	while (1) {
+		wfi();
+	}
+}
+
+static const plat_psci_ops_t imx_plat_psci_ops = {
+	.validate_ns_entrypoint = imx_validate_ns_entrypoint,
+	.validate_power_state = imx_validate_power_state,
+	.pwr_domain_on = imx_pwr_domain_on,
+	.pwr_domain_off = imx_pwr_domain_off,
+	.pwr_domain_on_finish = imx_pwr_domain_on_finish,
+	.pwr_domain_suspend = imx_pwr_domain_suspend,
+	.pwr_domain_suspend_finish = imx_pwr_domain_suspend_finish,
+	.get_sys_suspend_power_state = imx_get_sys_suspend_power_state,
+	.system_reset = imx_system_reset,
+	.system_off = imx_system_off,
+};
+
+/* export the platform specific psci ops */
+int plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			const plat_psci_ops_t **psci_ops)
+{
+	/* sec_entrypoint is used for warm reset */
+	secure_entrypoint = sec_entrypoint;
+	imx_set_cpu_boot_entry(0, sec_entrypoint);
+
+	pwr_sys_init();
+
+	*psci_ops = &imx_plat_psci_ops;
+
+	return 0;
+}
diff --git a/plat/imx/imx93/include/platform_def.h b/plat/imx/imx93/include/platform_def.h
new file mode 100644
index 0000000..7efbf1c
--- /dev/null
+++ b/plat/imx/imx93/include/platform_def.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright 2022-2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef PLATFORM_DEF_H
+#define PLATFORM_DEF_H
+
+#include <lib/utils_def.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+
+#define PLATFORM_LINKER_FORMAT		"elf64-littleaarch64"
+#define PLATFORM_LINKER_ARCH		aarch64
+
+#define PLATFORM_STACK_SIZE		0xB00
+#define CACHE_WRITEBACK_GRANULE		64
+
+#define PLAT_PRIMARY_CPU		U(0x0)
+#define PLATFORM_MAX_CPU_PER_CLUSTER	U(2)
+#define PLATFORM_CLUSTER_COUNT		U(1)
+#define PLATFORM_CLUSTER0_CORE_COUNT	U(2)
+#define PLATFORM_CORE_COUNT		U(2)
+
+#define IMX_PWR_LVL0			MPIDR_AFFLVL0
+
+#define PWR_DOMAIN_AT_MAX_LVL		U(1)
+#define PLAT_MAX_PWR_LVL		U(2)
+#define PLAT_MAX_OFF_STATE		U(4)
+#define PLAT_MAX_RET_STATE		U(2)
+
+#define BL31_BASE			U(0x204E0000)
+#define BL31_LIMIT			U(0x20520000)
+
+/* non-secure uboot base */
+/* TODO */
+#define PLAT_NS_IMAGE_OFFSET		U(0x80200000)
+#define BL32_FDT_OVERLAY_ADDR           (PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
+/* GICv4 base address */
+#define PLAT_GICD_BASE			U(0x48000000)
+#define PLAT_GICR_BASE			U(0x48040000)
+
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(ULL(1) << 32)
+#define PLAT_PHY_ADDR_SPACE_SIZE	(ULL(1) << 32)
+
+#define MAX_XLAT_TABLES			8
+#define MAX_MMAP_REGIONS		16
+
+#define IMX_LPUART_BASE			U(0x44380000)
+#define IMX_BOOT_UART_CLK_IN_HZ		U(24000000) /* Select 24MHz oscillator */
+#define IMX_CONSOLE_BAUDRATE		115200
+
+#define AIPSx_SIZE			U(0x800000)
+#define AIPS1_BASE			U(0x44000000)
+#define AIPS2_BASE			U(0x42000000)
+#define AIPS3_BASE			U(0x42800000)
+#define AIPS4_BASE			U(0x49000000)
+#define GPIO1_BASE			U(0x47400000)
+#define GPIO2_BASE			U(0x43810000)
+#define GPIO3_BASE			U(0x43820000)
+#define GPIO4_BASE			U(0x43830000)
+
+#define TRDC_A_BASE			U(0x44270000)
+#define TRDC_W_BASE			U(0x42460000)
+#define TRDC_M_BASE			U(0x42810000)
+#define TRDC_N_BASE			U(0x49010000)
+#define TRDC_x_SISE			U(0x20000)
+
+#define WDOG3_BASE			U(0x42490000)
+#define WDOG_CS				U(0x0)
+#define WDOG_CS_ULK			BIT(11)
+#define WDOG_CNT			U(0x4)
+#define WDOG_TOVAL			U(0x8)
+
+#define BBNSM_BASE			U(0x44440000)
+#define BBNSM_CTRL			U(0x8)
+#define BBNSM_DP_EN			BIT(24)
+#define BBNSM_TOSP			BIT(25)
+
+#define SRC_BASE			U(0x44460000)
+#define GPC_BASE			U(0x44470000)
+#define BLK_CTRL_S_BASE			U(0x444F0000)
+#define S400_MU_BASE			U(0x47520000)
+
+/* system memory map define */
+#define AIPS2_MAP	MAP_REGION_FLAT(AIPS2_BASE, AIPSx_SIZE, MT_DEVICE | MT_RW | MT_NS)
+#define AIPS1_MAP	MAP_REGION_FLAT(AIPS1_BASE, AIPSx_SIZE, MT_DEVICE | MT_RW)
+#define AIPS4_MAP	MAP_REGION_FLAT(AIPS4_BASE, AIPSx_SIZE, MT_DEVICE | MT_RW | MT_NS)
+#define GIC_MAP		MAP_REGION_FLAT(PLAT_GICD_BASE, 0x200000, MT_DEVICE | MT_RW)
+#define TRDC_A_MAP	MAP_REGION_FLAT(TRDC_A_BASE, TRDC_x_SISE, MT_DEVICE | MT_RW)
+#define TRDC_W_MAP	MAP_REGION_FLAT(TRDC_W_BASE, TRDC_x_SISE, MT_DEVICE | MT_RW)
+#define TRDC_M_MAP	MAP_REGION_FLAT(TRDC_M_BASE, TRDC_x_SISE, MT_DEVICE | MT_RW)
+#define TRDC_N_MAP	MAP_REGION_FLAT(TRDC_N_BASE, TRDC_x_SISE, MT_DEVICE | MT_RW)
+
+#define COUNTER_FREQUENCY		24000000
+
+#endif /* platform_def.h */
diff --git a/plat/imx/imx93/include/pwr_ctrl.h b/plat/imx/imx93/include/pwr_ctrl.h
new file mode 100644
index 0000000..9bcf486
--- /dev/null
+++ b/plat/imx/imx93/include/pwr_ctrl.h
@@ -0,0 +1,216 @@
+/*
+ * Copyright 2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PWR_CTRL_H
+#define PWR_CTRL_H
+
+#include <stdbool.h>
+
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * GPC definitions & declarations
+ ******************************************************************************/
+/* GPC GLOBAL */
+#define GPC_GLOBAL_BASE		U(GPC_BASE + 0x4000)
+#define GPC_AUTHEN_CTRL		U(0x4)
+#define GPC_DOMAIN		U(0x10)
+#define GPC_MASTER		U(0x1c)
+#define GPC_SYS_SLEEP		U(0x40)
+#define PMIC_CTRL		U(0x100)
+#define PMIC_PRE_DLY_CTRL	U(0x104)
+#define PMIC_STBY_ACK_CTRL	U(0x108)
+#define GPC_ROSC_CTRL		U(0x200)
+#define GPC_AON_MEM_CTRL	U(0x204)
+#define GPC_EFUSE_CTRL		U(0x208)
+
+#define FORCE_CPUx_DISABLE(x)	(1 << (16 + (x)))
+#define PMIC_STBY_EN		BIT(0)
+#define ROSC_OFF_EN		BIT(0)
+
+/* GPC CPU_CTRL */
+#define CM_SLICE(x)		(GPC_BASE + 0x800 * (x))
+#define CM_AUTHEN_CTRL		U(0x4)
+#define CM_MISC			U(0xc)
+#define CM_MODE_CTRL		U(0x10)
+#define CM_IRQ_WAKEUP_MASK0	U(0x100)
+#define CM_SYS_SLEEP_CTRL	U(0x380)
+#define IMR_NUM			U(8)
+
+/* CM_MISC */
+#define SLEEP_HOLD_EN		BIT(1)
+#define IRQ_MUX			BIT(5)
+#define SW_WAKEUP		BIT(6)
+
+/* CM_SYS_SLEEP_CTRL */
+#define SS_WAIT			BIT(0)
+#define SS_STOP			BIT(1)
+#define SS_SUSPEND		BIT(2)
+
+#define CM_MODE_RUN		U(0x0)
+#define CM_MODE_WAIT		U(0x1)
+#define CM_MODE_STOP		U(0x2)
+#define CM_MODE_SUSPEND		U(0x3)
+
+#define LPM_SETTING(d, m)	((m) << (((d) % 8) * 4))
+
+enum gpc_cmc_slice {
+	CPU_M33,
+	CPU_A55C0,
+	CPU_A55C1,
+	CPU_A55_PLAT,
+};
+
+/* set gpc domain assignment */
+static inline void gpc_assign_domains(unsigned int domains)
+{
+	mmio_write_32(GPC_GLOBAL_BASE + GPC_DOMAIN, domains);
+}
+
+/* force a cpu into sleep status */
+static inline void gpc_force_cpu_suspend(unsigned int cpu)
+{
+	mmio_setbits_32(GPC_GLOBAL_BASE + GPC_SYS_SLEEP, FORCE_CPUx_DISABLE(cpu));
+}
+
+static inline void gpc_pmic_stby_en(bool en)
+{
+	mmio_write_32(GPC_GLOBAL_BASE + PMIC_CTRL, en ? 1 : 0);
+}
+
+static inline void gpc_rosc_off(bool off)
+{
+	mmio_write_32(GPC_GLOBAL_BASE + GPC_ROSC_CTRL, off ? 1 : 0);
+}
+
+static inline void gpc_set_cpu_mode(unsigned int cpu, unsigned int mode)
+{
+	mmio_write_32(CM_SLICE(cpu) + CM_MODE_CTRL, mode);
+}
+
+static inline void gpc_select_wakeup_gic(unsigned int cpu)
+{
+	mmio_setbits_32(CM_SLICE(cpu) + CM_MISC, IRQ_MUX);
+}
+
+static inline void gpc_select_wakeup_raw_irq(unsigned int cpu)
+{
+	mmio_clrbits_32(CM_SLICE(cpu) + CM_MISC, IRQ_MUX);
+}
+
+static inline void gpc_assert_sw_wakeup(unsigned int cpu)
+{
+	mmio_setbits_32(CM_SLICE(cpu) + CM_MISC, SW_WAKEUP);
+}
+
+static inline void gpc_deassert_sw_wakeup(unsigned int cpu)
+{
+	mmio_clrbits_32(CM_SLICE(cpu) + CM_MISC, SW_WAKEUP);
+}
+
+static inline void gpc_clear_cpu_sleep_hold(unsigned int cpu)
+{
+	mmio_clrbits_32(CM_SLICE(cpu) + CM_MISC, SLEEP_HOLD_EN);
+}
+
+static inline void gpc_set_irq_mask(unsigned int cpu, unsigned int idx, uint32_t mask)
+{
+	mmio_write_32(CM_SLICE(cpu) + idx * 0x4 + CM_IRQ_WAKEUP_MASK0, mask);
+}
+
+/*******************************************************************************
+ * SRC definitions & declarations
+ ******************************************************************************/
+#define SRC_SLICE(x)		(SRC_BASE + 0x400 * (x))
+#define SRC_AUTHEN_CTRL		U(0x4)
+#define SRC_LPM_SETTING0	U(0x10)
+#define SRC_LPM_SETTING1	U(0x14)
+#define SRC_LPM_SETTING2	U(0x18)
+#define SRC_SLICE_SW_CTRL	U(0x20)
+
+#define SRC_MEM_CTRL		U(0x4)
+#define MEM_LP_EN		BIT(2)
+#define MEM_LP_RETN		BIT(1)
+
+enum mix_mem_mode {
+	MEM_OFF,
+	MEM_RETN,
+};
+
+enum src_mix_mem_slice {
+	SRC_GLOBAL,
+
+	/* MIX slice */
+	SRC_SENTINEL,
+	SRC_AON,
+	SRC_WKUP,
+	SRC_DDR,
+	SRC_DPHY,
+	SRC_ML,
+	SRC_NIC,
+	SRC_HSIO,
+	SRC_MEDIA,
+	SRC_M33P,
+	SRC_A55C0,
+	SRC_A55C1,
+	SRC_A55P,
+
+	/* MEM slice */
+	SRC_AON_MEM,
+	SRC_WKUP_MEM,
+	SRC_DDR_MEM,
+	SRC_DPHY_MEM,
+	SRC_ML_MEM,
+	SRC_NIC_MEM,
+	SRC_NIC_OCRAM,
+	SRC_HSIO_MEM,
+	SRC_MEDIA_MEM,
+	SRC_A55P0_MEM,
+	SRC_A55P1_MEM,
+	SRC_A55_SCU_MEM,
+	SRC_A55_L3_MEM,
+};
+
+static inline void src_authen_config(unsigned int mix, unsigned int wlist,
+				unsigned int lpm_en)
+{
+	mmio_write_32(SRC_SLICE(mix) + SRC_AUTHEN_CTRL, (wlist << 16) | (lpm_en << 2));
+}
+
+static inline void src_mix_set_lpm(unsigned int mix, unsigned int did, unsigned int lpm_mode)
+{
+	mmio_clrsetbits_32(SRC_SLICE(mix) + SRC_LPM_SETTING1 + (did / 8) * 0x4,
+			   LPM_SETTING(did, 0x7), LPM_SETTING(did, lpm_mode));
+}
+
+static inline void src_mem_lpm_en(unsigned int mix, bool retn)
+{
+	mmio_setbits_32(SRC_SLICE(mix) + SRC_MEM_CTRL, MEM_LP_EN | (retn ? MEM_LP_RETN : 0));
+}
+
+static inline void src_mem_lpm_dis(unsigned int mix)
+{
+	mmio_clrbits_32(SRC_SLICE(mix) + SRC_MEM_CTRL, MEM_LP_EN | MEM_LP_RETN);
+}
+
+/*******************************************************************************
+ * BLK_CTRL_S definitions & declarations
+ ******************************************************************************/
+#define HW_LP_HANDHSK		U(0x110)
+#define HW_LP_HANDHSK2		U(0x114)
+#define CA55_CPUWAIT		U(0x118)
+#define CA55_RVBADDR0_L		U(0x11c)
+#define CA55_RVBADDR0_H		U(0x120)
+
+/*******************************************************************************
+ * Other definitions & declarations
+ ******************************************************************************/
+void pwr_sys_init(void);
+
+#endif /* PWR_CTRL_H */
+
diff --git a/plat/imx/imx93/plat_topology.c b/plat/imx/imx93/plat_topology.c
new file mode 100644
index 0000000..739e2b9
--- /dev/null
+++ b/plat/imx/imx93/plat_topology.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright 2022-2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <plat/common/platform.h>
+
+const unsigned char imx_power_domain_tree_desc[] = {
+	PWR_DOMAIN_AT_MAX_LVL,
+	PLATFORM_CLUSTER_COUNT,
+	PLATFORM_CLUSTER0_CORE_COUNT,
+};
+
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	return imx_power_domain_tree_desc;
+}
+
+/*
+ * Only one cluster is planned for i.MX9 family, no need
+ * to consider the cluster id
+ */
+int plat_core_pos_by_mpidr(u_register_t mpidr)
+{
+	unsigned int cpu_id;
+
+	mpidr &= MPIDR_AFFINITY_MASK;
+
+	if (mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)) {
+		return -1;
+	}
+
+	cpu_id = MPIDR_AFFLVL1_VAL(mpidr);
+
+	return cpu_id;
+}
diff --git a/plat/imx/imx93/platform.mk b/plat/imx/imx93/platform.mk
new file mode 100644
index 0000000..ed7e81f
--- /dev/null
+++ b/plat/imx/imx93/platform.mk
@@ -0,0 +1,46 @@
+#
+# Copyright 2022-2023 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+PLAT_INCLUDES		:=	-Iplat/imx/common/include		\
+				-Iplat/imx/imx93/include		\
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
+GICV3_SUPPORT_GIC600  :=      1
+
+# Include GICv3 driver files
+include drivers/arm/gic/v3/gicv3.mk
+
+IMX_GIC_SOURCES		:=	${GICV3_SOURCES}			\
+				plat/common/plat_gicv3.c		\
+				plat/common/plat_psci_common.c		\
+				plat/imx/common/plat_imx8_gic.c
+
+BL31_SOURCES		+=	plat/common/aarch64/crash_console_helpers.S   \
+				plat/imx/imx93/aarch64/plat_helpers.S		\
+				plat/imx/imx93/plat_topology.c			\
+				plat/imx/common/lpuart_console.S		\
+				plat/imx/imx93/trdc.c			\
+				plat/imx/imx93/pwr_ctrl.c			\
+				plat/imx/imx93/imx93_bl31_setup.c		\
+				plat/imx/imx93/imx93_psci.c			\
+				lib/cpus/aarch64/cortex_a55.S			\
+				drivers/delay_timer/delay_timer.c		\
+				drivers/delay_timer/generic_delay_timer.c	\
+				drivers/nxp/trdc/imx_trdc.c			\
+				${IMX_GIC_SOURCES}				\
+				${XLAT_TABLES_LIB_SRCS}
+
+RESET_TO_BL31		:=	1
+HW_ASSISTED_COHERENCY	:= 	1
+USE_COHERENT_MEM	:=	0
+PROGRAMMABLE_RESET_ADDRESS :=	1
+COLD_BOOT_SINGLE_CPU	:=	1
+
+BL32_BASE               ?=      0x96000000
+BL32_SIZE               ?=      0x02000000
+$(eval $(call add_define,BL32_BASE))
+$(eval $(call add_define,BL32_SIZE))
diff --git a/plat/imx/imx93/pwr_ctrl.c b/plat/imx/imx93/pwr_ctrl.c
new file mode 100644
index 0000000..624c605
--- /dev/null
+++ b/plat/imx/imx93/pwr_ctrl.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright 2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <stdbool.h>
+
+#include <platform_def.h>
+#include <pwr_ctrl.h>
+
+/*Do the necessary GPC, SRC, BLK_CTRL_S init */
+void pwr_sys_init(void)
+{
+	unsigned int cpu;
+
+	/*
+	 * Assigned A55 cluster to 3, m33 to 2, A55 CORE0 & CORE1 to 0/1.
+	 * domain0/1 only used for trigger LPM of themselves. A55 cluster & M33's
+	 * domain assignment should be align with the TRDC DID.
+	 */
+	gpc_assign_domains(0x3102);
+
+	/* CA55 core0/1 config */
+	for (cpu = CPU_A55C0; cpu <= CPU_A55_PLAT; cpu++) {
+		/* clear the cpu sleep hold */
+		gpc_clear_cpu_sleep_hold(cpu);
+		/* use gic wakeup source by default */
+		gpc_select_wakeup_gic(cpu);
+		/*
+		 * Ignore A55 core0/1's LPM trigger for system sleep.
+		 * normally, for A55 side, only the A55 cluster(plat)
+		 * domain will be used to trigger the system wide low
+		 * power mode transition.
+		 */
+		if (cpu != CPU_A55_PLAT) {
+			gpc_force_cpu_suspend(cpu);
+		}
+	}
+
+	/* boot core(A55C0) */
+	src_mem_lpm_en(SRC_A55P0_MEM, MEM_OFF);
+	/* For A55 core, only need to be on in RUN mode */
+	src_mix_set_lpm(SRC_A55C0, 0x0, CM_MODE_WAIT);
+	/* whitelist: 0x1 for domain 0 only */
+	src_authen_config(SRC_A55C0, 0x1, 0x1);
+
+	/* A55 cluster */
+	gpc_select_wakeup_gic(CPU_A55_PLAT);
+	gpc_clear_cpu_sleep_hold(CPU_A55_PLAT);
+
+	/* SCU MEM must be OFF when A55 PLAT OFF */
+	src_mem_lpm_en(SRC_A55_SCU_MEM, MEM_OFF);
+	/* L3 memory in retention by default */
+	src_mem_lpm_en(SRC_A55_L3_MEM, MEM_RETN);
+
+	src_mix_set_lpm(SRC_A55P, 0x3, 0x1);
+	/* whitelist: 0x8 for domain 3 only */
+	src_authen_config(SRC_A55P, 0x8, 0x1);
+
+	/* enable the HW LP handshake between S401 & A55 cluster */
+	mmio_setbits_32(BLK_CTRL_S_BASE + HW_LP_HANDHSK, BIT(5));
+}
+
diff --git a/plat/imx/imx93/trdc.c b/plat/imx/imx93/trdc.c
new file mode 100644
index 0000000..0d09aa6
--- /dev/null
+++ b/plat/imx/imx93/trdc.c
@@ -0,0 +1,68 @@
+/*
+ * Copyright 2022-2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+
+#include "trdc_config.h"
+
+struct trdc_mgr_info trdc_mgr_blks[] = {
+	{ TRDC_A_BASE, 0, 0, 39, 40 },
+	{ TRDC_W_BASE, 0, 0, 70, 71 },
+	{ TRDC_M_BASE, 1, 0, 1, 2 },
+	{ TRDC_N_BASE, 0, 1, 1, 2 },
+};
+
+unsigned int trdc_mgr_num = ARRAY_SIZE(trdc_mgr_blks);
+
+struct trdc_config_info trdc_cfg_info[] = {
+	{	TRDC_A_BASE,
+		trdc_a_mbc_glbac, ARRAY_SIZE(trdc_a_mbc_glbac),
+		trdc_a_mbc, ARRAY_SIZE(trdc_a_mbc),
+		trdc_a_mrc_glbac, ARRAY_SIZE(trdc_a_mrc_glbac),
+		trdc_a_mrc, ARRAY_SIZE(trdc_a_mrc)
+	}, /* TRDC_A */
+	{	TRDC_W_BASE,
+		trdc_w_mbc_glbac, ARRAY_SIZE(trdc_w_mbc_glbac),
+		trdc_w_mbc, ARRAY_SIZE(trdc_w_mbc),
+		trdc_w_mrc_glbac, ARRAY_SIZE(trdc_w_mrc_glbac),
+		trdc_w_mrc, ARRAY_SIZE(trdc_w_mrc)
+	}, /* TRDC_W */
+	{	TRDC_N_BASE,
+		trdc_n_mbc_glbac, ARRAY_SIZE(trdc_n_mbc_glbac),
+		trdc_n_mbc, ARRAY_SIZE(trdc_n_mbc),
+		trdc_n_mrc_glbac, ARRAY_SIZE(trdc_n_mrc_glbac),
+		trdc_n_mrc, ARRAY_SIZE(trdc_n_mrc)
+	}, /* TRDC_N */
+};
+
+void trdc_config(void)
+{
+	unsigned int i;
+
+	/* Set MTR to DID1 */
+	trdc_mda_set_noncpu(TRDC_A_BASE, 4, false, 0x2, 0x2, 0x1);
+
+	/* Set M33 to DID2*/
+	trdc_mda_set_cpu(TRDC_A_BASE, 1, 0, 0x2, 0x0, 0x2, 0x0, 0x0, 0x0);
+
+	/* Configure the access permission for TRDC MGR and MC slots */
+	for (i = 0U; i < ARRAY_SIZE(trdc_mgr_blks); i++) {
+		trdc_mgr_mbc_setup(&trdc_mgr_blks[i]);
+	}
+
+	/* Configure TRDC user settings from config table */
+	for (i = 0U; i < ARRAY_SIZE(trdc_cfg_info); i++) {
+		trdc_setup(&trdc_cfg_info[i]);
+	}
+
+	NOTICE("TRDC init done\n");
+}
diff --git a/plat/imx/imx93/trdc_config.h b/plat/imx/imx93/trdc_config.h
new file mode 100644
index 0000000..c623a19
--- /dev/null
+++ b/plat/imx/imx93/trdc_config.h
@@ -0,0 +1,254 @@
+/*
+ * Copyright 2022-2023 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <drivers/nxp/trdc/imx_trdc.h>
+
+#define TRDC_A_BASE	U(0x44270000)
+#define TRDC_W_BASE	U(0x42460000)
+#define TRDC_M_BASE	U(0x42460000)
+#define TRDC_N_BASE	U(0x49010000)
+
+/* GLBAC7 is used for TRDC only, any setting to GLBAC7 will be ignored */
+
+/* aonmix */
+struct trdc_glbac_config trdc_a_mbc_glbac[] = {
+	/* MBC0 */
+	{ 0, 0, SP(RW)  | SU(RW)   | NP(RW)  | NU(RW) },
+	/* MBC1 */
+	{ 1, 0, SP(RW)  | SU(RW)   | NP(RW)  | NU(RW) },
+	{ 1, 1, SP(RW)  | SU(R)    | NP(RW)  | NU(R)  },
+	{ 1, 2, SP(RWX) | SU(RWX)  | NP(RWX) | NU(RWX)  },
+};
+
+struct trdc_mbc_config trdc_a_mbc[] = {
+	{ 0, 0, 0, MBC_BLK_ALL, 0, true }, /* MBC0 AIPS1 for S401 DID0 */
+	{ 0, 0, 1, MBC_BLK_ALL, 0, true }, /* MBC0 Sentinel_SOC_In for S401 DID0 */
+	{ 0, 0, 2, MBC_BLK_ALL, 0, true }, /* MBC0 GPIO1 for S401 DID0 */
+	{ 1, 0, 0, MBC_BLK_ALL, 0, true }, /* MBC1 CM33 code TCM for S401 DID0 */
+	{ 1, 0, 1, MBC_BLK_ALL, 0, true }, /* MBC1 CM33 system TCM for S401 DID0 */
+
+	{ 0, 1, 0, MBC_BLK_ALL, 0, true }, /* MBC0 AIPS1 for MTR DID1 */
+	{ 0, 1, 1, MBC_BLK_ALL, 0, true }, /* MBC0 Sentinel_SOC_In for MTR DID1 */
+
+	{ 0, 2, 0, MBC_BLK_ALL, 0, true }, /* MBC0 AIPS1 for M33 DID2 */
+	{ 0, 2, 1, MBC_BLK_ALL, 0, true }, /* MBC0 Sentinel_SOC_In for M33 DID2 */
+	{ 0, 2, 2, MBC_BLK_ALL, 0, true }, /* MBC0 GPIO1 for M33 DID2 */
+	{ 1, 2, 0, MBC_BLK_ALL, 2, true  }, /* MBC1 CM33 code TCM for M33 DID2 */
+	{ 1, 2, 1, MBC_BLK_ALL, 2, true  }, /* MBC1 CM33 system TCM for M33 DID2 */
+
+	{ 0, 3, 0, MBC_BLK_ALL, 0, false }, /* MBC0 AIPS1 for A55 DID3 */
+	{ 0, 3, 1, MBC_BLK_ALL, 0, false }, /* MBC0 Sentinel_SOC_In for A55 DID3 */
+	{ 0, 3, 2, MBC_BLK_ALL, 0, false }, /* MBC0 GPIO1 for A55 DID3 */
+	{ 1, 3, 0, MBC_BLK_ALL, 1, false }, /* MBC1 CM33 code TCM for A55 DID3 */
+	{ 1, 3, 1, MBC_BLK_ALL, 1, false }, /* MBC1 CM33 system TCM for A55 DID3 */
+	{ 1, 10, 1, MBC_BLK_ALL, 2, false }, /* MBC1 CM33 system TCM for SoC masters DID10 */
+
+	{ 0, 7, 0, MBC_BLK_ALL, 0, false }, /* MBC0 AIPS1 for eDMA DID7 */
+};
+
+struct trdc_glbac_config trdc_a_mrc_glbac[] = {
+	{ 0, 0, SP(RWX) | SU(RWX) | NP(RWX) | NU(RWX) },
+	{ 0, 1, SP(R)   | SU(0)   | NP(R)   | NU(0)   },
+};
+
+struct trdc_mrc_config trdc_a_mrc[] = {
+	{ 0, 2, 0, 0x00000000, 0x00040000, 0, true }, /* MRC0 M33 ROM for M33 DID2 */
+	{ 0, 3, 0, 0x00100000, 0x00040000, 1, true }, /* MRC0 M33 ROM for A55 DID3 */
+};
+
+/* wakeupmix */
+struct trdc_glbac_config trdc_w_mbc_glbac[] = {
+	/* MBC0 */
+	{ 0, 0, SP(RW) | SU(RW) | NP(RW) | NU(RW) },
+	/* MBC1 */
+	{ 1, 0, SP(RW) | SU(RW) | NP(RW) | NU(RW) },
+};
+
+struct trdc_mbc_config trdc_w_mbc[] = {
+	{ 0, 1, 0, MBC_BLK_ALL, 0, true }, /* MBC0 AIPS2 for MTR DID1 */
+	{ 1, 1, 0, MBC_BLK_ALL, 0, true }, /* MBC1 AIPS3 for MTR DID1 */
+
+	{ 0, 2, 0, MBC_BLK_ALL, 0, true }, /* MBC0 AIPS2 for M33 DID2 */
+	{ 0, 2, 1, MBC_BLK_ALL, 0, true }, /* MBC0 GPIO2_In for M33 DID2 */
+	{ 0, 2, 2, MBC_BLK_ALL, 0, true }, /* MBC0 GPIO3 for M33 DID2 */
+	{ 0, 2, 3, MBC_BLK_ALL, 0, true }, /* MBC0 DAP  for M33 DID2 */
+	{ 1, 2, 0, MBC_BLK_ALL, 0, true }, /* MBC1 AIPS3 for M33 DID2 */
+	{ 1, 2, 1, MBC_BLK_ALL, 0, true }, /* MBC1 AHB_ISPAP for M33 DID2 */
+	{ 1, 2, 2, MBC_BLK_ALL, 0, true },  /* MBC1 NIC_MAIN_GPV for M33 DID2 */
+	{ 1, 2, 3, MBC_BLK_ALL, 0, true }, /* MBC1 GPIO4 for M33 DID2 */
+
+	{ 0, 3, 0, MBC_BLK_ALL, 0, false }, /* MBC0 AIPS2 for A55 DID3 */
+	{ 0, 3, 1, MBC_BLK_ALL, 0, false }, /* MBC0 GPIO2_In for A55 DID3 */
+	{ 0, 3, 2, MBC_BLK_ALL, 0, false }, /* MBC0 GPIO3 for A55 DID3 */
+	{ 0, 3, 3, MBC_BLK_ALL, 0, false }, /* MBC0 DAP  for A55 DID3 */
+	{ 1, 3, 0, MBC_BLK_ALL, 0, false }, /* MBC1 AIPS3 for A55 DID3 */
+	{ 1, 3, 1, MBC_BLK_ALL, 0, false }, /* MBC1 AHB_ISPAP for A55 DID3 */
+	{ 1, 3, 2, MBC_BLK_ALL, 0, true },  /* MBC1 NIC_MAIN_GPV for A55 DID3 */
+	{ 1, 3, 3, MBC_BLK_ALL, 0, false }, /* MBC1 GPIO4 for A55 DID3 */
+
+	{ 0, 7, 0, MBC_BLK_ALL, 0, false }, /* MBC0 AIPS2 for eDMA DID7 */
+	{ 1, 7, 0, MBC_BLK_ALL, 0, false }, /* MBC1 AIPS3 for eDMA DID7  */
+};
+
+struct trdc_glbac_config trdc_w_mrc_glbac[] = {
+	/* MRC0 */
+	{ 0, 0, SP(RX)  | SU(RX)   | NP(RX)   | NU(RX)    },
+	/* MRC1 */
+	{ 1, 0, SP(RWX) | SU(RWX)  | NP(RWX)  | NU(RWX)   },
+};
+
+struct trdc_mrc_config trdc_w_mrc[] = {
+	{ 0, 3, 0, 0x00000000, 0x00040000, 0, false }, /* MRC0 A55 ROM for A55 DID3 */
+	{ 1, 2, 0, 0x28000000, 0x08000000, 0, true  }, /* MRC1 FLEXSPI1 for M33 DID2 */
+	{ 1, 3, 0, 0x28000000, 0x08000000, 0, false }, /* MRC1 FLEXSPI1 for A55 DID3 */
+};
+
+/* nicmix */
+struct trdc_glbac_config trdc_n_mbc_glbac[] = {
+	/* MBC0 */
+	{ 0, 0, SP(RW) | SU(RW) | NP(RW) | NU(RW) },
+	/* MBC1 */
+	{ 1, 0, SP(RW) | SU(RW) | NP(RW) | NU(RW) },
+	/* MBC2 */
+	{ 2, 0, SP(RW) | SU(RW) | NP(RW) | NU(RW) },
+	{ 2, 1, SP(R) | SU(R) | NP(R) | NU(R) },
+	/* MBC3 */
+	{ 3, 0, SP(RW) | SU(RW) | NP(RW) | NU(RW) },
+	{ 3, 1, SP(RWX) | SU(RWX) | NP(RWX) | NU(RWX) },
+};
+
+struct trdc_mbc_config trdc_n_mbc[] = {
+	{ 0, 0, 0, MBC_BLK_ALL, 0, true }, /* MBC0 DDRCFG for S401 DID0 */
+	{ 0, 0, 1, MBC_BLK_ALL, 0, true }, /* MBC0 AIPS4 for  S401 DID0 */
+	{ 0, 0, 2, MBC_BLK_ALL, 0, true }, /* MBC0 MEDIAMIX for  S401 DID0 */
+	{ 0, 0, 3, MBC_BLK_ALL, 0, true }, /* MBC0 HSIOMIX for  S401 DID0 */
+	{ 1, 0, 0, MBC_BLK_ALL, 0, true }, /* MBC1 MTR_DCA, TCU, TROUT for  S401 DID0 */
+	{ 1, 0, 1, MBC_BLK_ALL, 0, true }, /* MBC1 MTR_DCA, TCU, TROUT for  S401 DID0 */
+	{ 1, 0, 2, MBC_BLK_ALL, 0, true }, /* MBC1 MLMIX for  S401 DID0 */
+	{ 1, 0, 3, MBC_BLK_ALL, 0, true }, /* MBC1 MLMIX for  S401 DID0 */
+	{ 2, 0, 0, MBC_BLK_ALL, 0, true }, /* MBC2 GIC for  S401 DID0 */
+	{ 2, 0, 1, MBC_BLK_ALL, 0, true }, /* MBC2 GIC for  S401 DID0 */
+	{ 3, 0, 0, MBC_BLK_ALL, 0, true }, /* MBC3 OCRAM for  S401 DID0 */
+	{ 3, 0, 1, MBC_BLK_ALL, 0, true }, /* MBC3 OCRAM for  S401 DID0 */
+
+	{ 0, 1, 0, MBC_BLK_ALL, 0, true }, /* MBC0 DDRCFG for MTR DID1 */
+	{ 0, 1, 1, MBC_BLK_ALL, 0, true }, /* MBC0 AIPS4 for  MTR DID1 */
+	{ 0, 1, 2, MBC_BLK_ALL, 0, true }, /* MBC0 MEDIAMIX for MTR DID1 */
+	{ 0, 1, 3, MBC_BLK_ALL, 0, true }, /* MBC0 HSIOMIX for  MTR DID1 */
+	{ 1, 1, 0, MBC_BLK_ALL, 0, true }, /* MBC1 MTR_DCA, TCU, TROUT for  MTR DID1 */
+	{ 1, 1, 1, MBC_BLK_ALL, 0, true }, /* MBC1 MTR_DCA, TCU, TROUT for  MTR DID1 */
+	{ 1, 1, 2, MBC_BLK_ALL, 0, true }, /* MBC1 MLMIX for  MTR DID1 */
+	{ 1, 1, 3, MBC_BLK_ALL, 0, true }, /* MBC1 MLMIX for  MTR DID1 */
+
+	{ 0, 2, 0, MBC_BLK_ALL, 0, true }, /* MBC0 DDRCFG for M33 DID2 */
+	{ 0, 2, 1, MBC_BLK_ALL, 0, true }, /* MBC0 AIPS4 for M33 DID2 */
+	{ 0, 2, 2, MBC_BLK_ALL, 0, true }, /* MBC0 MEDIAMIX for M33 DID2 */
+	{ 0, 2, 3, MBC_BLK_ALL, 0, true }, /* MBC0 HSIOMIX for M33 DID2 */
+	{ 1, 2, 0, MBC_BLK_ALL, 0, true }, /* MBC1 MTR_DCA, TCU, TROUT for M33 DID2 */
+	{ 1, 2, 1, MBC_BLK_ALL, 0, true }, /* MBC1 MTR_DCA, TCU, TROUT for M33 DID2 */
+	{ 1, 2, 2, MBC_BLK_ALL, 0, true }, /* MBC1 MLMIX for M33 DID2 */
+	{ 1, 2, 3, MBC_BLK_ALL, 0, true }, /* MBC1 MLMIX for M33 DID2 */
+	{ 2, 2, 0, MBC_BLK_ALL, 1, true }, /* MBC2 GIC for M33 DID2 */
+	{ 2, 2, 1, MBC_BLK_ALL, 1, true }, /* MBC2 GIC for M33 DID2 */
+	{ 3, 2, 0, MBC_BLK_ALL, 0, true  }, /* MBC3 OCRAM for M33 DID2 */
+	{ 3, 2, 1, MBC_BLK_ALL, 0, true  }, /* MBC3 OCRAM for M33 DID2 */
+
+	{ 0, 3, 0, MBC_BLK_ALL, 0, false }, /* MBC0 DDRCFG for A55 DID3 */
+	{ 0, 3, 1, MBC_BLK_ALL, 0, false }, /* MBC0 AIPS4 for A55 DID3 */
+	{ 0, 3, 2, MBC_BLK_ALL, 0, false }, /* MBC0 MEDIAMIX for A55 DID3 */
+	{ 0, 3, 3, MBC_BLK_ALL, 0, false }, /* MBC0 HSIOMIX for A55 DID3 */
+	{ 1, 3, 0, MBC_BLK_ALL, 0, false }, /* MBC1 MTR_DCA, TCU, TROUT for A55 DID3 */
+	{ 1, 3, 1, MBC_BLK_ALL, 0, false }, /* MBC1 MTR_DCA, TCU, TROUT for A55 DID3 */
+	{ 1, 3, 2, MBC_BLK_ALL, 0, false }, /* MBC1 MLMIX for A55 DID3 */
+	{ 1, 3, 3, MBC_BLK_ALL, 0, false }, /* MBC1 MLMIX for A55 DID3 */
+	{ 2, 3, 0, MBC_BLK_ALL, 0, false }, /* MBC2 GIC for A55 DID3 */
+	{ 2, 3, 1, MBC_BLK_ALL, 0, false }, /* MBC2 GIC for A55 DID3 */
+	{ 3, 3, 0, MBC_BLK_ALL, 1, true  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 1, MBC_BLK_ALL, 1, true  }, /* MBC3 OCRAM for A55 DID3 */
+
+	{ 3, 3, 0, 0, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 0, 1, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 0, 2, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 0, 3, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 0, 4, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 0, 5, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 1, 0, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 1, 1, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 1, 2, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 1, 3, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 1, 4, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+	{ 3, 3, 1, 5, 0, false  }, /* MBC3 OCRAM for A55 DID3 */
+
+	{ 0, 7, 1, MBC_BLK_ALL, 0, false }, /* MBC0 AIPS4 for eDMA DID7 */
+	{ 0, 7, 2, MBC_BLK_ALL, 0, false }, /* MBC0 MEDIAMIX for eDMA DID7 */
+	{ 0, 7, 3, MBC_BLK_ALL, 0, false }, /* MBC0 HSIOMIX for eDMA DID7 */
+
+	{ 3, 10, 0, MBC_BLK_ALL, 0, false }, /* MBC3 OCRAM for DID10 */
+	{ 3, 10, 1, MBC_BLK_ALL, 0, false }, /* MBC3 OCRAM for DID10 */
+};
+
+struct trdc_glbac_config trdc_n_mrc_glbac[] = {
+	{ 0, 0, SP(RW)  | SU(RW)  | NP(RW)  | NU(RW)  },
+	{ 0, 1, SP(RWX) | SU(RWX) | NP(RWX) | NU(RWX) },
+};
+
+#if defined(SPD_opteed)
+#define TEE_SHM_SIZE 0x200000
+
+#define DRAM_MEM_0_START (0x80000000)
+#define DRAM_MEM_0_SIZE (BL32_BASE - 0x80000000)
+
+#define DRAM_MEM_1_START (BL32_BASE)
+#define DRAM_MEM_1_SIZE (BL32_SIZE - TEE_SHM_SIZE)
+
+#define DRAM_MEM_2_START (DRAM_MEM_1_START + DRAM_MEM_1_SIZE)
+#define DRAM_MEM_2_SIZE (0x80000000 - DRAM_MEM_1_SIZE - DRAM_MEM_0_SIZE)
+
+struct trdc_mrc_config trdc_n_mrc[] = {
+	{ 0, 0, 0, 0x80000000, 0x80000000, 0, false }, /* MRC0 DRAM for S400 DID0 */
+	{ 0, 1, 0, 0x80000000, 0x80000000, 0, false }, /* MRC0 DRAM for MTR DID1 */
+	{ 0, 2, 0, 0x80000000, 0x80000000, 0, true }, /* MRC0 DRAM for M33 DID2 */
+	{ 0, 8, 0, 0x80000000, 0x80000000, 1, false }, /* MRC0 DRAM for Coresight, Testport DID8 */
+	{ 0, 9, 0, 0x80000000, 0x80000000, 1, false }, /* MRC0 DRAM for DAP DID9 */
+
+	{ 0, 3, 0, DRAM_MEM_0_START, DRAM_MEM_0_SIZE, 1, false }, /* MRC0 DRAM for A55 DID3 */
+	{ 0, 5, 0, DRAM_MEM_0_START, DRAM_MEM_0_SIZE, 0, false }, /* MRC0 DRAM for USDHC1 DID5 */
+	{ 0, 6, 0, DRAM_MEM_0_START, DRAM_MEM_0_SIZE, 0, false }, /* MRC0 DRAM for USDHC2 DID6 */
+	{ 0, 7, 0, DRAM_MEM_0_START, DRAM_MEM_0_SIZE, 0, false }, /* MRC0 DRAM for eDMA DID7 */
+	{ 0, 10, 0, DRAM_MEM_0_START, DRAM_MEM_0_SIZE, 0, false }, /* MRC0 DRAM for SoC masters DID10 */
+	{ 0, 11, 0, DRAM_MEM_0_START, DRAM_MEM_0_SIZE, 0, false }, /* MRC0 DRAM for USB DID11 */
+
+	/* OPTEE memory for  secure access only. */
+	{ 0, 3, 1, DRAM_MEM_1_START, DRAM_MEM_1_SIZE, 1, true }, /* MRC0 DRAM for A55 DID3 */
+	{ 0, 5, 1, DRAM_MEM_1_START, DRAM_MEM_1_SIZE, 0, true }, /* MRC0 DRAM for USDHC1 DID5 */
+	{ 0, 6, 1, DRAM_MEM_1_START, DRAM_MEM_1_SIZE, 0, true }, /* MRC0 DRAM for USDHC2 DID6 */
+	{ 0, 7, 1, DRAM_MEM_1_START, DRAM_MEM_1_SIZE, 0, true }, /* MRC0 DRAM for eDMA DID7 */
+	{ 0, 10, 1, DRAM_MEM_1_START, DRAM_MEM_1_SIZE, 0, true }, /* MRC0 DRAM for SoC masters DID10 */
+	{ 0, 11, 1, DRAM_MEM_1_START, DRAM_MEM_1_SIZE, 0, true }, /* MRC0 DRAM for USB DID11 */
+
+	{ 0, 3, 2, DRAM_MEM_2_START, DRAM_MEM_2_SIZE, 1, false }, /* MRC0 DRAM for A55 DID3 */
+	{ 0, 5, 2, DRAM_MEM_2_START, DRAM_MEM_2_SIZE, 0, false }, /* MRC0 DRAM for USDHC1 DID5 */
+	{ 0, 6, 2, DRAM_MEM_2_START, DRAM_MEM_2_SIZE, 0, false }, /* MRC0 DRAM for USDHC2 DID6 */
+	{ 0, 7, 2, DRAM_MEM_2_START, DRAM_MEM_2_SIZE, 0, false }, /* MRC0 DRAM for eDMA DID7 */
+	{ 0, 10, 2, DRAM_MEM_2_START, DRAM_MEM_2_SIZE, 0, false }, /* MRC0 DRAM for SoC masters DID10 */
+	{ 0, 11, 2, DRAM_MEM_2_START, DRAM_MEM_2_SIZE, 0, false }, /* MRC0 DRAM for USB DID11 */
+
+};
+#else
+struct trdc_mrc_config trdc_n_mrc[] = {
+	{ 0, 0, 0, 0x80000000, 0x80000000, 0, false }, /* MRC0 DRAM for S400 DID0 */
+	{ 0, 1, 0, 0x80000000, 0x80000000, 0, false }, /* MRC0 DRAM for MTR DID1 */
+	{ 0, 2, 0, 0x80000000, 0x80000000, 0, true }, /* MRC0 DRAM for M33 DID2 */
+	{ 0, 3, 0, 0x80000000, 0x80000000, 1, false }, /* MRC0 DRAM for A55 DID3 */
+	{ 0, 5, 0, 0x80000000, 0x80000000, 0, false }, /* MRC0 DRAM for USDHC1 DID5 */
+	{ 0, 6, 0, 0x80000000, 0x80000000, 0, false }, /* MRC0 DRAM for USDHC2 DID6 */
+	{ 0, 7, 0, 0x80000000, 0x80000000, 0, false }, /* MRC0 DRAM for eDMA DID7 */
+	{ 0, 8, 0, 0x80000000, 0x80000000, 1, false }, /* MRC0 DRAM for Coresight, Testport DID8 */
+	{ 0, 9, 0, 0x80000000, 0x80000000, 1, false }, /* MRC0 DRAM for DAP DID9 */
+	{ 0, 10, 0, 0x80000000, 0x80000000, 0, false }, /* MRC0 DRAM for SoC masters DID10 */
+	{ 0, 11, 0, 0x80000000, 0x80000000, 0, false }, /* MRC0 DRAM for USB DID11 */
+};
+#endif
diff --git a/plat/intel/soc/agilex/include/socfpga_plat_def.h b/plat/intel/soc/agilex/include/socfpga_plat_def.h
index 85dfeab..a744d09 100644
--- a/plat/intel/soc/agilex/include/socfpga_plat_def.h
+++ b/plat/intel/soc/agilex/include/socfpga_plat_def.h
@@ -87,6 +87,18 @@
 #define PLAT_SYS_COUNTER_FREQ_IN_TICKS	(400000000)
 #define PLAT_HZ_CONVERT_TO_MHZ	(1000000)
 
+/*******************************************************************************
+ * SDMMC related pointer function
+ ******************************************************************************/
+#define SDMMC_READ_BLOCKS	mmc_read_blocks
+#define SDMMC_WRITE_BLOCKS	mmc_write_blocks
+
+/*******************************************************************************
+ * sysmgr.boot_scratch_cold6 & 7 (64bit) are used to indicate L2 reset
+ * is done and HPS should trigger warm reset via RMR_EL3.
+ ******************************************************************************/
+#define L2_RESET_DONE_REG			0xFFD12218
+
 /* Platform specific system counter */
 #define PLAT_SYS_COUNTER_FREQ_IN_MHZ	get_cpu_clk()
 
diff --git a/plat/intel/soc/agilex5/bl2_plat_setup.c b/plat/intel/soc/agilex5/bl2_plat_setup.c
new file mode 100644
index 0000000..a2fafd2
--- /dev/null
+++ b/plat/intel/soc/agilex5/bl2_plat_setup.c
@@ -0,0 +1,174 @@
+/*
+ * Copyright (c) 2019-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <common/desc_image_load.h>
+#include <drivers/cadence/cdns_sdmmc.h>
+#include <drivers/generic_delay_timer.h>
+#include <drivers/synopsys/dw_mmc.h>
+#include <drivers/ti/uart/uart_16550.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+
+#include "agilex5_clock_manager.h"
+#include "agilex5_memory_controller.h"
+#include "agilex5_mmc.h"
+#include "agilex5_pinmux.h"
+#include "agilex5_system_manager.h"
+#include "ccu/ncore_ccu.h"
+#include "combophy/combophy.h"
+#include "nand/nand.h"
+#include "qspi/cadence_qspi.h"
+#include "sdmmc/sdmmc.h"
+#include "socfpga_emac.h"
+#include "socfpga_f2sdram_manager.h"
+#include "socfpga_handoff.h"
+#include "socfpga_mailbox.h"
+#include "socfpga_private.h"
+#include "socfpga_reset_manager.h"
+#include "wdt/watchdog.h"
+
+
+/* Declare mmc_info */
+static struct mmc_device_info mmc_info;
+
+/* Declare cadence idmac descriptor */
+extern struct cdns_idmac_desc cdns_desc[8] __aligned(32);
+
+const mmap_region_t agilex_plat_mmap[] = {
+	MAP_REGION_FLAT(DRAM_BASE, DRAM_SIZE,
+		MT_MEMORY | MT_RW | MT_NS),
+	MAP_REGION_FLAT(PSS_BASE, PSS_SIZE,
+		MT_DEVICE | MT_RW | MT_NS),
+	MAP_REGION_FLAT(MPFE_BASE, MPFE_SIZE,
+		MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(OCRAM_BASE, OCRAM_SIZE,
+		MT_NON_CACHEABLE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(CCU_BASE, CCU_SIZE,
+		MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(MEM64_BASE, MEM64_SIZE,
+		MT_DEVICE | MT_RW | MT_NS),
+	MAP_REGION_FLAT(GIC_BASE, GIC_SIZE,
+		MT_DEVICE | MT_RW | MT_SECURE),
+	{0},
+};
+
+boot_source_type boot_source = BOOT_SOURCE;
+
+void bl2_el3_early_platform_setup(u_register_t x0, u_register_t x1,
+				u_register_t x2, u_register_t x4)
+{
+	static console_t console;
+
+	handoff reverse_handoff_ptr = { 0 };
+
+	generic_delay_timer_init();
+	config_clkmgr_handoff(&reverse_handoff_ptr);
+	mailbox_init();
+	enable_nonsecure_access();
+
+	deassert_peripheral_reset();
+	if (combo_phy_init(&reverse_handoff_ptr) != 0) {
+		ERROR("Combo Phy initialization failed\n");
+	}
+
+	console_16550_register(PLAT_INTEL_UART_BASE, PLAT_UART_CLOCK,
+	PLAT_BAUDRATE, &console);
+
+	/* Store magic number */
+	mmio_write_32(L2_RESET_DONE_REG, PLAT_L2_RESET_REQ);
+}
+
+void bl2_el3_plat_arch_setup(void)
+{
+	handoff reverse_handoff_ptr;
+
+	struct cdns_sdmmc_params params = EMMC_INIT_PARAMS((uintptr_t) &cdns_desc, get_mmc_clk());
+
+	mmc_info.mmc_dev_type = MMC_DEVICE_TYPE;
+	mmc_info.ocr_voltage = OCR_3_3_3_4 | OCR_3_2_3_3;
+
+	/* Request ownership and direct access to QSPI */
+	mailbox_hps_qspi_enable();
+
+	switch (boot_source) {
+	case BOOT_SOURCE_SDMMC:
+		NOTICE("SDMMC boot\n");
+		sdmmc_init(&reverse_handoff_ptr, &params, &mmc_info);
+		socfpga_io_setup(boot_source);
+		break;
+
+	case BOOT_SOURCE_QSPI:
+		NOTICE("QSPI boot\n");
+		cad_qspi_init(0, QSPI_CONFIG_CPHA, QSPI_CONFIG_CPOL,
+			QSPI_CONFIG_CSDA, QSPI_CONFIG_CSDADS,
+			QSPI_CONFIG_CSEOT, QSPI_CONFIG_CSSOT, 0);
+		socfpga_io_setup(boot_source);
+		break;
+
+	case BOOT_SOURCE_NAND:
+		NOTICE("NAND boot\n");
+		nand_init(&reverse_handoff_ptr);
+		socfpga_io_setup(boot_source);
+		break;
+
+	default:
+		ERROR("Unsupported boot source\n");
+		panic();
+		break;
+	}
+}
+
+uint32_t get_spsr_for_bl33_entry(void)
+{
+	unsigned long el_status;
+	unsigned int mode;
+	uint32_t spsr;
+
+	/* Figure out what mode we enter the non-secure world in */
+	el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
+	el_status &= ID_AA64PFR0_ELX_MASK;
+
+	mode = (el_status) ? MODE_EL2 : MODE_EL1;
+
+	/*
+	 * TODO: Consider the possibility of specifying the SPSR in
+	 * the FIP ToC and allowing the platform to have a say as
+	 * well.
+	 */
+	spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	return spsr;
+}
+
+int bl2_plat_handle_post_image_load(unsigned int image_id)
+{
+	bl_mem_params_node_t *bl_mem_params = get_bl_mem_params_node(image_id);
+
+	assert(bl_mem_params);
+
+	switch (image_id) {
+	case BL33_IMAGE_ID:
+		bl_mem_params->ep_info.args.arg0 = 0xffff & read_mpidr();
+		bl_mem_params->ep_info.spsr = get_spsr_for_bl33_entry();
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * Perform any BL3-1 platform setup code
+ ******************************************************************************/
+void bl2_platform_setup(void)
+{
+}
diff --git a/plat/intel/soc/agilex5/bl31_plat_setup.c b/plat/intel/soc/agilex5/bl31_plat_setup.c
new file mode 100644
index 0000000..5ae4bf7
--- /dev/null
+++ b/plat/intel/soc/agilex5/bl31_plat_setup.c
@@ -0,0 +1,284 @@
+/*
+ * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <drivers/arm/gic_common.h>
+#include <drivers/arm/gicv3.h>
+#include <drivers/ti/uart/uart_16550.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_mmu_helpers.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
+
+#include "agilex5_power_manager.h"
+#include "ccu/ncore_ccu.h"
+#include "socfpga_mailbox.h"
+#include "socfpga_private.h"
+#include "socfpga_reset_manager.h"
+
+/* Get non-secure SPSR for BL33. Zephyr and Linux */
+uint32_t arm_get_spsr_for_bl33_entry(void);
+
+static entry_point_info_t bl32_image_ep_info;
+static entry_point_info_t bl33_image_ep_info;
+
+/* The GICv3 driver only needs to be initialized in EL3 */
+static uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
+
+#define SMMU_SDMMC
+
+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;
+}
+
+void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
+				u_register_t arg2, u_register_t arg3)
+{
+	static console_t console;
+
+	mmio_write_64(PLAT_SEC_ENTRY, PLAT_SEC_WARM_ENTRY);
+
+	console_16550_register(PLAT_INTEL_UART_BASE, PLAT_UART_CLOCK,
+	PLAT_BAUDRATE, &console);
+
+	init_ncore_ccu();
+	setup_smmu_stream_id();
+
+	/*
+	 * Check params passed from BL31 should not be NULL,
+	 */
+	void *from_bl2 = (void *) arg0;
+
+#if RESET_TO_BL31
+	/* There are no parameters from BL2 if BL31 is a reset vector */
+	assert(from_bl2 == NULL);
+	void *plat_params_from_bl2 = (void *) arg3;
+
+	assert(plat_params_from_bl2 == NULL);
+
+	/* Populate entry point information for BL33 */
+	SET_PARAM_HEAD(&bl33_image_ep_info,
+				PARAM_EP,
+				VERSION_1,
+				0);
+
+# if ARM_LINUX_KERNEL_AS_BL33
+	/*
+	 * According to the file ``Documentation/arm64/booting.txt`` of the
+	 * Linux kernel tree, Linux expects the physical address of the device
+	 * tree blob (DTB) in x0, while x1-x3 are reserved for future use and
+	 * must be 0.
+	 */
+	bl33_image_ep_info.args.arg0 = (u_register_t)ARM_PRELOADED_DTB_BASE;
+	bl33_image_ep_info.args.arg1 = 0U;
+	bl33_image_ep_info.args.arg2 = 0U;
+	bl33_image_ep_info.args.arg3 = 0U;
+# endif
+
+#else /* RESET_TO_BL31 */
+	bl_params_t *params_from_bl2 = (bl_params_t *)from_bl2;
+
+	assert(params_from_bl2 != NULL);
+
+	/*
+	 * Copy BL32 (if populated by BL31) and BL33 entry point information.
+	 * They are stored in Secure RAM, in BL31's address space.
+	 */
+
+	if (params_from_bl2->h.type == PARAM_BL_PARAMS &&
+		params_from_bl2->h.version >= VERSION_2) {
+
+		bl_params_node_t *bl_params = params_from_bl2->head;
+
+		while (bl_params) {
+			if (bl_params->image_id == BL33_IMAGE_ID) {
+				bl33_image_ep_info = *bl_params->ep_info;
+			}
+				bl_params = bl_params->next_params_info;
+		}
+	} else {
+		struct socfpga_bl31_params *arg_from_bl2 =
+			(struct socfpga_bl31_params *) from_bl2;
+
+		assert(arg_from_bl2->h.type == PARAM_BL31);
+		assert(arg_from_bl2->h.version >= VERSION_1);
+
+		bl32_image_ep_info = *arg_from_bl2->bl32_ep_info;
+		bl33_image_ep_info = *arg_from_bl2->bl33_ep_info;
+	}
+
+	bl33_image_ep_info.args.arg0 = (u_register_t)ARM_PRELOADED_DTB_BASE;
+	bl33_image_ep_info.args.arg1 = 0U;
+	bl33_image_ep_info.args.arg2 = 0U;
+	bl33_image_ep_info.args.arg3 = 0U;
+#endif
+
+	/*
+	 * Tell BL31 where the non-trusted software image
+	 * is located and the entry state information
+	 */
+	bl33_image_ep_info.pc = plat_get_ns_image_entrypoint();
+	bl33_image_ep_info.spsr = arm_get_spsr_for_bl33_entry();
+
+	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
+}
+
+static const interrupt_prop_t agx5_interrupt_props[] = {
+	PLAT_INTEL_SOCFPGA_G1S_IRQ_PROPS(INTR_GROUP1S),
+	PLAT_INTEL_SOCFPGA_G0_IRQ_PROPS(INTR_GROUP0)
+};
+
+static const gicv3_driver_data_t plat_gicv3_gic_data = {
+	.gicd_base = PLAT_INTEL_SOCFPGA_GICD_BASE,
+	.gicr_base = PLAT_INTEL_SOCFPGA_GICR_BASE,
+	.interrupt_props = agx5_interrupt_props,
+	.interrupt_props_num = ARRAY_SIZE(agx5_interrupt_props),
+	.rdistif_num = PLATFORM_CORE_COUNT,
+	.rdistif_base_addrs = rdistif_base_addrs,
+};
+
+/*******************************************************************************
+ * Perform any BL3-1 platform setup code
+ ******************************************************************************/
+void bl31_platform_setup(void)
+{
+	socfpga_delay_timer_init();
+
+	/* Initialize the gic cpu and distributor interfaces */
+	gicv3_driver_init(&plat_gicv3_gic_data);
+	gicv3_distif_init();
+	gicv3_rdistif_init(plat_my_core_pos());
+	gicv3_cpuif_enable(plat_my_core_pos());
+	mailbox_hps_stage_notify(HPS_EXECUTION_STATE_SSBL);
+#if !defined(SIMICS_RUN)
+	ncore_enable_ocram_firewall();
+#endif
+
+}
+
+const mmap_region_t plat_agilex_mmap[] = {
+	MAP_REGION_FLAT(DRAM_BASE, DRAM_SIZE, MT_MEMORY | MT_RW | MT_NS),
+	MAP_REGION_FLAT(PSS_BASE, PSS_SIZE, MT_DEVICE | MT_RW | MT_NS),
+	MAP_REGION_FLAT(MPFE_BASE, MPFE_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(OCRAM_BASE, OCRAM_SIZE, MT_NON_CACHEABLE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(CCU_BASE, CCU_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(MEM64_BASE, MEM64_SIZE, MT_DEVICE | MT_RW | MT_NS),
+	MAP_REGION_FLAT(GIC_BASE, GIC_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+	{0}
+};
+
+/*******************************************************************************
+ * Perform the very early platform specific architectural setup here. At the
+ * moment this is only intializes the mmu in a quick and dirty way.
+ ******************************************************************************/
+void bl31_plat_arch_setup(void)
+{
+	uint32_t boot_core = 0x00;
+	uint32_t cpuid = 0x00;
+
+	cpuid = read_mpidr();
+	boot_core = (mmio_read_32(AGX5_PWRMGR(MPU_BOOTCONFIG)) & 0xC00);
+	NOTICE("BL31: Boot Core = %x\n", boot_core);
+	NOTICE("BL31: CPU ID = %x\n", cpuid);
+
+}
+
+/* Get non-secure image entrypoint for BL33. Zephyr and Linux */
+uintptr_t plat_get_ns_image_entrypoint(void)
+{
+#ifdef PRELOADED_BL33_BASE
+	return PRELOADED_BL33_BASE;
+#else
+	return PLAT_NS_IMAGE_OFFSET;
+#endif
+}
+
+/* Get non-secure SPSR for BL33. Zephyr and Linux */
+uint32_t arm_get_spsr_for_bl33_entry(void)
+{
+	unsigned int mode;
+	uint32_t spsr;
+
+	/* Figure out what mode we enter the non-secure world in */
+	mode = (el_implemented(2) != EL_IMPL_NONE) ? MODE_EL2 : MODE_EL1;
+
+	/*
+	 * TODO: Consider the possibility of specifying the SPSR in
+	 * the FIP ToC and allowing the platform to have a say as
+	 * well.
+	 */
+	spsr = SPSR_64((uint64_t)mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	return spsr;
+}
+
+/* SMP: Secondary cores BL31 setup reset vector */
+void bl31_plat_set_secondary_cpu_entrypoint(unsigned int cpu_id)
+{
+	unsigned int pch_cpu = 0x00;
+	unsigned int pchctlr_old = 0x00;
+	unsigned int pchctlr_new = 0x00;
+	uint32_t boot_core = 0x00;
+
+	boot_core = (mmio_read_32(AGX5_PWRMGR(MPU_BOOTCONFIG)) & 0xC00);
+	/* Update the p-channel based on cpu id */
+	pch_cpu = 1 << cpu_id;
+
+	if (boot_core == 0x00) {
+		/* Update reset vector to 0x00 */
+		mmio_write_64(RSTMGR_CPUxRESETBASELOW_CPU2,
+(uint64_t) plat_secondary_cpus_bl31_entry >> 2);
+	} else {
+		/* Update reset vector to 0x00 */
+		mmio_write_64(RSTMGR_CPUxRESETBASELOW_CPU0,
+(uint64_t) plat_secondary_cpus_bl31_entry >> 2);
+	}
+
+	/* Update reset vector to 0x00 */
+	mmio_write_64(RSTMGR_CPUxRESETBASELOW_CPU1, (uint64_t) plat_secondary_cpus_bl31_entry >> 2);
+	mmio_write_64(RSTMGR_CPUxRESETBASELOW_CPU3, (uint64_t) plat_secondary_cpus_bl31_entry >> 2);
+
+	/* On all cores - temporary */
+	pchctlr_old = mmio_read_32(AGX5_PWRMGR(MPU_PCHCTLR));
+	pchctlr_new = pchctlr_old | (pch_cpu<<1);
+	mmio_write_32(AGX5_PWRMGR(MPU_PCHCTLR), pchctlr_new);
+
+	/* We will only release the target secondary CPUs */
+	/* Bit mask for each CPU BIT0-3 */
+	mmio_write_32(RSTMGR_CPUSTRELEASE_CPUx, pch_cpu);
+}
+
+void bl31_plat_set_secondary_cpu_off(void)
+{
+	unsigned int pch_cpu = 0x00;
+	unsigned int pch_cpu_off = 0x00;
+	unsigned int cpu_id = plat_my_core_pos();
+
+	pch_cpu_off = 1 << cpu_id;
+
+	pch_cpu = mmio_read_32(AGX5_PWRMGR(MPU_PCHCTLR));
+	pch_cpu = pch_cpu & ~(pch_cpu_off << 1);
+
+	mmio_write_32(AGX5_PWRMGR(MPU_PCHCTLR), pch_cpu);
+}
+
+void bl31_plat_enable_mmu(uint32_t flags)
+{
+	/* TODO: Enable mmu when needed */
+}
diff --git a/plat/intel/soc/agilex5/include/agilex5_clock_manager.h b/plat/intel/soc/agilex5/include/agilex5_clock_manager.h
new file mode 100644
index 0000000..566a80d
--- /dev/null
+++ b/plat/intel/soc/agilex5/include/agilex5_clock_manager.h
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CLOCKMANAGER_H
+#define CLOCKMANAGER_H
+
+#include "socfpga_handoff.h"
+
+/* Clock Manager Registers */
+#define CLKMGR_OFFSET					0x10d10000
+
+#define CLKMGR_CTRL					0x0
+#define CLKMGR_STAT					0x4
+#define CLKMGR_TESTIOCTROL				0x8
+#define CLKMGR_INTRGEN					0xc
+#define CLKMGR_INTRMSK					0x10
+#define CLKMGR_INTRCLR					0x14
+#define CLKMGR_INTRSTS					0x18
+#define CLKMGR_INTRSTK					0x1c
+#define CLKMGR_INTRRAW					0x20
+
+/* Main PLL Group */
+#define CLKMGR_MAINPLL					0x10d10024
+#define CLKMGR_MAINPLL_EN				0x0
+#define CLKMGR_MAINPLL_ENS				0x4
+#define CLKMGR_MAINPLL_BYPASS				0xc
+#define CLKMGR_MAINPLL_BYPASSS				0x10
+#define CLKMGR_MAINPLL_BYPASSR				0x14
+#define CLKMGR_MAINPLL_NOCCLK				0x1c
+#define CLKMGR_MAINPLL_NOCDIV				0x20
+#define CLKMGR_MAINPLL_PLLGLOB				0x24
+#define CLKMGR_MAINPLL_FDBCK				0x28
+#define CLKMGR_MAINPLL_MEM				0x2c
+#define CLKMGR_MAINPLL_MEMSTAT				0x30
+#define CLKMGR_MAINPLL_VCOCALIB				0x34
+#define CLKMGR_MAINPLL_PLLC0				0x38
+#define CLKMGR_MAINPLL_PLLC1				0x3c
+#define CLKMGR_MAINPLL_PLLC2				0x40
+#define CLKMGR_MAINPLL_PLLC3				0x44
+#define CLKMGR_MAINPLL_PLLM				0x48
+#define CLKMGR_MAINPLL_FHOP				0x4c
+#define CLKMGR_MAINPLL_SSC				0x50
+#define CLKMGR_MAINPLL_LOSTLOCK				0x54
+
+/* Peripheral PLL Group */
+#define CLKMGR_PERPLL					0x10d1007c
+#define CLKMGR_PERPLL_EN				0x0
+#define CLKMGR_PERPLL_ENS				0x4
+#define CLKMGR_PERPLL_BYPASS				0xc
+#define CLKMGR_PERPLL_EMACCTL				0x18
+#define CLKMGR_PERPLL_GPIODIV				0x1c
+#define CLKMGR_PERPLL_PLLGLOB				0x20
+#define CLKMGR_PERPLL_FDBCK				0x24
+#define CLKMGR_PERPLL_MEM				0x28
+#define CLKMGR_PERPLL_MEMSTAT				0x2c
+#define CLKMGR_PERPLL_PLLC0				0x30
+#define CLKMGR_PERPLL_PLLC1				0x34
+#define CLKMGR_PERPLL_VCOCALIB				0x38
+#define CLKMGR_PERPLL_PLLC2				0x3c
+#define CLKMGR_PERPLL_PLLC3				0x40
+#define CLKMGR_PERPLL_PLLM				0x44
+#define CLKMGR_PERPLL_LOSTLOCK				0x50
+
+/* Altera Group */
+#define CLKMGR_ALTERA					0x10d100d0
+#define CLKMGR_ALTERA_JTAG				0x0
+#define CLKMGR_ALTERA_EMACACTR				0x4
+#define CLKMGR_ALTERA_EMACBCTR				0x8
+#define CLKMGR_ALTERA_EMACPTPCTR			0xc
+#define CLKMGR_ALTERA_GPIODBCTR				0x10
+#define CLKMGR_ALTERA_S2FUSER0CTR			0x18
+#define CLKMGR_ALTERA_S2FUSER1CTR			0x1c
+#define CLKMGR_ALTERA_PSIREFCTR				0x20
+#define CLKMGR_ALTERA_EXTCNTRST				0x24
+#define CLKMGR_ALTERA_USB31CTR				0x28
+#define CLKMGR_ALTERA_DSUCTR				0x2c
+#define CLKMGR_ALTERA_CORE01CTR				0x30
+#define CLKMGR_ALTERA_CORE23CTR				0x34
+#define CLKMGR_ALTERA_CORE2CTR				0x38
+#define CLKMGR_ALTERA_CORE3CTR				0x3c
+
+/* Membus */
+#define CLKMGR_MEM_REQ					BIT(24)
+#define CLKMGR_MEM_WR					BIT(25)
+#define CLKMGR_MEM_ERR					BIT(26)
+#define CLKMGR_MEM_WDAT_OFFSET				16
+#define CLKMGR_MEM_ADDR					0x4027
+#define CLKMGR_MEM_WDAT					0x80
+
+/* Clock Manager Macros */
+#define CLKMGR_CTRL_BOOTMODE_SET_MSK			0x00000001
+#define CLKMGR_STAT_BUSY_E_BUSY				0x1
+#define CLKMGR_STAT_BUSY(x)				(((x) & 0x00000001) >> 0)
+#define CLKMGR_STAT_MAINPLLLOCKED(x)			(((x) & 0x00000100) >> 8)
+#define CLKMGR_STAT_PERPLLLOCKED(x)			(((x) & 0x00010000) >> 16)
+#define CLKMGR_INTRCLR_MAINLOCKLOST_SET_MSK		0x00000004
+#define CLKMGR_INTRCLR_PERLOCKLOST_SET_MSK		0x00000008
+#define CLKMGR_INTOSC_HZ				460000000
+
+/* Main PLL Macros */
+#define CLKMGR_MAINPLL_EN_RESET				0x0000005e
+#define CLKMGR_MAINPLL_ENS_RESET			0x0000005e
+
+/* Peripheral PLL Macros */
+#define CLKMGR_PERPLL_EN_RESET				0x040007FF
+#define CLKMGR_PERPLL_ENS_RESET			0x040007FF
+
+#define CLKMGR_PERPLL_EN_SDMMCCLK			BIT(5)
+#define CLKMGR_PERPLL_GPIODIV_GPIODBCLK_SET(x)		(((x) << 0) & 0x0000ffff)
+
+/* Altera Macros */
+#define CLKMGR_ALTERA_EXTCNTRST_RESET			0xff
+
+/* Shared Macros */
+#define CLKMGR_PSRC(x)					(((x) & 0x00030000) >> 16)
+#define CLKMGR_PSRC_MAIN				0
+#define CLKMGR_PSRC_PER					1
+
+#define CLKMGR_PLLGLOB_PSRC_EOSC1			0x0
+#define CLKMGR_PLLGLOB_PSRC_INTOSC			0x1
+#define CLKMGR_PLLGLOB_PSRC_F2S				0x2
+
+#define CLKMGR_PLLM_MDIV(x)				((x) & 0x000003ff)
+#define CLKMGR_PLLGLOB_PD_SET_MSK			0x00000001
+#define CLKMGR_PLLGLOB_RST_SET_MSK			0x00000002
+
+#define CLKMGR_PLLGLOB_REFCLKDIV(x)			(((x) & 0x00003f00) >> 8)
+#define CLKMGR_PLLGLOB_AREFCLKDIV(x)			(((x) & 0x00000f00) >> 8)
+#define CLKMGR_PLLGLOB_DREFCLKDIV(x)			(((x) & 0x00003000) >> 12)
+
+#define CLKMGR_VCOCALIB_HSCNT_SET(x)			(((x) << 0) & 0x000003ff)
+#define CLKMGR_VCOCALIB_MSCNT_SET(x)			(((x) << 16) & 0x00ff0000)
+
+#define CLKMGR_CLR_LOSTLOCK_BYPASS			0x20000000
+
+typedef struct {
+	uint32_t  clk_freq_of_eosc1;
+	uint32_t  clk_freq_of_f2h_free;
+	uint32_t  clk_freq_of_cb_intosc_ls;
+} CLOCK_SOURCE_CONFIG;
+
+void config_clkmgr_handoff(handoff *hoff_ptr);
+uint32_t get_wdt_clk(void);
+uint32_t get_uart_clk(void);
+uint32_t get_mmc_clk(void);
+
+#endif
diff --git a/plat/intel/soc/agilex5/include/agilex5_memory_controller.h b/plat/intel/soc/agilex5/include/agilex5_memory_controller.h
new file mode 100644
index 0000000..1708488
--- /dev/null
+++ b/plat/intel/soc/agilex5/include/agilex5_memory_controller.h
@@ -0,0 +1,175 @@
+/*
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef AGX_MEMORYCONTROLLER_H
+#define AGX_MEMORYCONTROLLER_H
+
+#include "socfpga_plat_def.h"
+
+#define AGX_MPFE_IOHMC_REG_DRAMADDRW				0xf80100a8
+#define AGX_MPFE_IOHMC_CTRLCFG0					0xf8010028
+#define AGX_MPFE_IOHMC_CTRLCFG1					0xf801002c
+#define AGX_MPFE_IOHMC_CTRLCFG2					0xf8010030
+#define AGX_MPFE_IOHMC_CTRLCFG3					0xf8010034
+#define AGX_MPFE_IOHMC_DRAMADDRW				0xf80100a8
+#define AGX_MPFE_IOHMC_DRAMTIMING0				0xf8010050
+#define AGX_MPFE_IOHMC_CALTIMING0				0xf801007c
+#define AGX_MPFE_IOHMC_CALTIMING1				0xf8010080
+#define AGX_MPFE_IOHMC_CALTIMING2				0xf8010084
+#define AGX_MPFE_IOHMC_CALTIMING3				0xf8010088
+#define AGX_MPFE_IOHMC_CALTIMING4				0xf801008c
+#define AGX_MPFE_IOHMC_CALTIMING9				0xf80100a0
+#define AGX_MPFE_IOHMC_CALTIMING9_ACT_TO_ACT(x)			(((x) & 0x000000ff) >> 0)
+#define AGX_MPFE_IOHMC_CTRLCFG1_CFG_ADDR_ORDER(value)		(((value) & 0x00000060) >> 5)
+
+#define AGX_MPFE_HMC_ADP_ECCCTRL1				0xf8011100
+#define AGX_MPFE_HMC_ADP_ECCCTRL2				0xf8011104
+#define AGX_MPFE_HMC_ADP_RSTHANDSHAKESTAT			0xf8011218
+#define AGX_MPFE_HMC_ADP_RSTHANDSHAKESTAT_SEQ2CORE		0x000000ff
+#define AGX_MPFE_HMC_ADP_RSTHANDSHAKECTRL			0xf8011214
+
+
+#define AGX_MPFE_IOHMC_REG_CTRLCFG1				0xf801002c
+
+#define AGX_MPFE_IOHMC_REG_NIOSRESERVE0_OFST			0xf8010110
+
+#define IOHMC_DRAMADDRW_COL_ADDR_WIDTH(x)			(((x) & 0x0000001f) >> 0)
+#define IOHMC_DRAMADDRW_ROW_ADDR_WIDTH(x)			(((x) & 0x000003e0) >> 5)
+#define IOHMC_DRAMADDRW_CS_ADDR_WIDTH(x)			(((x) & 0x00070000) >> 16)
+#define IOHMC_DRAMADDRW_BANK_GRP_ADDR_WIDTH(x)			(((x) & 0x0000c000) >> 14)
+#define IOHMC_DRAMADDRW_BANK_ADDR_WIDTH(x)			(((x) & 0x00003c00) >> 10)
+
+#define AGX_MPFE_DDR(x)						(0xf8000000 + x)
+#define AGX_MPFE_HMC_ADP_DDRCALSTAT				0xf801100c
+#define AGX_MPFE_DDR_MAIN_SCHED					0xf8000400
+#define AGX_MPFE_DDR_MAIN_SCHED_DDRCONF				0xf8000408
+#define AGX_MPFE_DDR_MAIN_SCHED_DDRTIMING			0xf800040c
+#define AGX_MPFE_DDR_MAIN_SCHED_DDRCONF_SET_MSK			0x0000001f
+#define AGX_MPFE_DDR_MAIN_SCHED_DDRMODE				0xf8000410
+#define AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV			0xf800043c
+#define AGX_MPFE_DDR_MAIN_SCHED_READLATENCY			0xf8000414
+#define AGX_MPFE_DDR_MAIN_SCHED_ACTIVATE			0xf8000438
+#define AGX_MPFE_DDR_MAIN_SCHED_ACTIVATE_FAWBANK_OFST		10
+#define AGX_MPFE_DDR_MAIN_SCHED_ACTIVATE_FAW_OFST		4
+#define AGX_MPFE_DDR_MAIN_SCHED_ACTIVATE_RRD_OFST		0
+#define AGX_MPFE_DDR_MAIN_SCHED_DDRCONF_SET(x)			(((x) << 0) & 0x0000001f)
+#define AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSRDTORD_OFST		0
+#define AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSRDTORD_MSK		(BIT(0) | BIT(1))
+#define AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSRDTOWR_OFST		2
+#define AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSRDTOWR_MSK		(BIT(2) | BIT(3))
+#define AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSWRTORD_OFST		4
+#define AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSWRTORD_MSK		(BIT(4) | BIT(5))
+
+#define AGX_MPFE_HMC_ADP(x)					(0xf8011000 + (x))
+#define AGX_MPFE_HMC_ADP_HPSINTFCSEL				0xf8011210
+#define AGX_MPFE_HMC_ADP_DDRIOCTRL				0xf8011008
+#define HMC_ADP_DDRIOCTRL					0x8
+#define HMC_ADP_DDRIOCTRL_IO_SIZE(x)				(((x) & 0x00000003) >> 0)
+#define HMC_ADP_DDRIOCTRL_CTRL_BURST_LENGTH(x)			(((x) & 0x00003e00) >> 9)
+#define ADP_DRAMADDRWIDTH					0xe0
+
+#define ACT_TO_ACT_DIFF_BANK(value)				(((value) & 0x00fc0000) >> 18)
+#define ACT_TO_ACT(value)					(((value) & 0x0003f000) >> 12)
+#define ACT_TO_RDWR(value)					(((value) & 0x0000003f) >> 0)
+#define ACT_TO_ACT(value)					(((value) & 0x0003f000) >> 12)
+
+/* timing 2 */
+#define RD_TO_RD_DIFF_CHIP(value)				(((value) & 0x00000fc0) >> 6)
+#define RD_TO_WR_DIFF_CHIP(value)				(((value) & 0x3f000000) >> 24)
+#define RD_TO_WR(value)						(((value) & 0x00fc0000) >> 18)
+#define RD_TO_PCH(value)					(((value) & 0x00000fc0) >> 6)
+
+/* timing 3 */
+#define CALTIMING3_WR_TO_RD_DIFF_CHIP(value)			(((value) & 0x0003f000) >> 12)
+#define CALTIMING3_WR_TO_RD(value)				(((value) & 0x00000fc0) >> 6)
+
+/* timing 4 */
+#define PCH_TO_VALID(value)					(((value) & 0x00000fc0) >> 6)
+
+#define DDRTIMING_BWRATIO_OFST					31
+#define DDRTIMING_WRTORD_OFST					26
+#define DDRTIMING_RDTOWR_OFST					21
+#define DDRTIMING_BURSTLEN_OFST					18
+#define DDRTIMING_WRTOMISS_OFST					12
+#define DDRTIMING_RDTOMISS_OFST					6
+#define DDRTIMING_ACTTOACT_OFST					0
+
+#define ADP_DDRIOCTRL_IO_SIZE(x)				(((x) & 0x3) >> 0)
+
+#define DDRMODE_AUTOPRECHARGE_OFST				1
+#define DDRMODE_BWRATIOEXTENDED_OFST				0
+
+
+#define AGX_MPFE_IOHMC_REG_DRAMTIMING0_CFG_TCL(x)		(((x) & 0x7f) >> 0)
+#define AGX_MPFE_IOHMC_REG_CTRLCFG0_CFG_MEM_TYPE(x)		(((x) & 0x0f) >> 0)
+
+#define AGX_CCU_CPU0_MPRT_DDR					0xf7004400
+#define AGX_CCU_CPU0_MPRT_MEM0					0xf70045c0
+#define AGX_CCU_CPU0_MPRT_MEM1A					0xf70045e0
+#define AGX_CCU_CPU0_MPRT_MEM1B					0xf7004600
+#define AGX_CCU_CPU0_MPRT_MEM1C					0xf7004620
+#define AGX_CCU_CPU0_MPRT_MEM1D					0xf7004640
+#define AGX_CCU_CPU0_MPRT_MEM1E					0xf7004660
+#define AGX_CCU_IOM_MPRT_MEM0					0xf7018560
+#define AGX_CCU_IOM_MPRT_MEM1A					0xf7018580
+#define	AGX_CCU_IOM_MPRT_MEM1B					0xf70185a0
+#define	AGX_CCU_IOM_MPRT_MEM1C					0xf70185c0
+#define	AGX_CCU_IOM_MPRT_MEM1D					0xf70185e0
+#define	AGX_CCU_IOM_MPRT_MEM1E					0xf7018600
+
+#define AGX_NOC_FW_DDR_SCR					0xf8020200
+#define AGX_NOC_FW_DDR_SCR_MPUREGION0ADDR_LIMITEXT		0xf802021c
+#define AGX_NOC_FW_DDR_SCR_MPUREGION0ADDR_LIMIT			0xf8020218
+#define AGX_NOC_FW_DDR_SCR_NONMPUREGION0ADDR_LIMITEXT		0xf802029c
+#define AGX_NOC_FW_DDR_SCR_NONMPUREGION0ADDR_LIMIT		0xf8020298
+
+#define AGX_SOC_NOC_FW_DDR_SCR_ENABLE				0xf8020200
+#define AGX_SOC_NOC_FW_DDR_SCR_ENABLESET			0xf8020204
+#define AGX_CCU_NOC_DI_SET_MSK					0x10
+
+#define AGX_SYSMGR_CORE_HMC_CLK					0xffd120b4
+#define AGX_SYSMGR_CORE_HMC_CLK_STATUS				0x00000001
+
+#define AGX_MPFE_IOHMC_NIOSRESERVE0_NIOS_RESERVE0(x)		(((x) & 0xffff) >> 0)
+#define AGX_MPFE_HMC_ADP_DDRIOCTRL_IO_SIZE_MSK			0x00000003
+#define AGX_MPFE_HMC_ADP_DDRIOCTRL_IO_SIZE_OFST			0
+#define AGX_MPFE_HMC_ADP_HPSINTFCSEL_ENABLE			0x001f1f1f
+#define AGX_IOHMC_CTRLCFG1_ENABLE_ECC_OFST			7
+
+#define AGX_MPFE_HMC_ADP_ECCCTRL1_AUTOWB_CNT_RST_SET_MSK	0x00010000
+#define AGX_MPFE_HMC_ADP_ECCCTRL1_CNT_RST_SET_MSK		0x00000100
+#define AGX_MPFE_HMC_ADP_ECCCTRL1_ECC_EN_SET_MSK		0x00000001
+
+#define AGX_MPFE_HMC_ADP_ECCCTRL2_AUTOWB_EN_SET_MSK		0x00000001
+#define AGX_MPFE_HMC_ADP_ECCCTRL2_OVRW_RB_ECC_EN_SET_MSK	0x00010000
+#define AGX_MPFE_HMC_ADP_ECCCTRL2_RMW_EN_SET_MSK		0x00000100
+#define AGX_MPFE_HMC_ADP_DDRCALSTAT_CAL(value)			(((value) & 0x1) >> 0)
+
+
+#define AGX_MPFE_HMC_ADP_DDRIOCTRL_IO_SIZE(x)			(((x) & 0x00003) >> 0)
+#define IOHMC_DRAMADDRW_CFG_BANK_ADDR_WIDTH(x)			(((x) & 0x03c00) >> 10)
+#define IOHMC_DRAMADDRW_CFG_BANK_GROUP_ADDR_WIDTH(x)		(((x) & 0x0c000) >> 14)
+#define IOHMC_DRAMADDRW_CFG_COL_ADDR_WIDTH(x)			(((x) & 0x0001f) >> 0)
+#define IOHMC_DRAMADDRW_CFG_CS_ADDR_WIDTH(x)			(((x) & 0x70000) >> 16)
+#define IOHMC_DRAMADDRW_CFG_ROW_ADDR_WIDTH(x)			(((x) & 0x003e0) >> 5)
+
+#define AGX_SDRAM_0_LB_ADDR					0x0
+#define AGX_DDR_SIZE						0x40000000
+
+/* Macros */
+#define SOCFPGA_MEMCTRL_ECCCTRL1				0x008
+#define SOCFPGA_MEMCTRL_ERRINTEN				0x010
+#define SOCFPGA_MEMCTRL_ERRINTENS				0x014
+#define SOCFPGA_MEMCTRL_ERRINTENR				0x018
+#define SOCFPGA_MEMCTRL_INTMODE					0x01C
+#define SOCFPGA_MEMCTRL_INTSTAT					0x020
+#define SOCFPGA_MEMCTRL_DIAGINTTEST				0x024
+#define SOCFPGA_MEMCTRL_DERRADDRA				0x02C
+
+#define SOCFPGA_MEMCTRL(_reg)					(SOCFPGA_MEMCTRL_REG_BASE \
+								+ (SOCFPGA_MEMCTRL_##_reg))
+
+#endif
diff --git a/plat/intel/soc/agilex5/include/agilex5_mmc.h b/plat/intel/soc/agilex5/include/agilex5_mmc.h
new file mode 100644
index 0000000..c8a5fba
--- /dev/null
+++ b/plat/intel/soc/agilex5/include/agilex5_mmc.h
@@ -0,0 +1,7 @@
+/*
+ * Copyright (c) 2020-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+void agx5_mmc_init(void);
diff --git a/plat/intel/soc/agilex5/include/agilex5_pinmux.h b/plat/intel/soc/agilex5/include/agilex5_pinmux.h
new file mode 100644
index 0000000..8a8e8c7
--- /dev/null
+++ b/plat/intel/soc/agilex5/include/agilex5_pinmux.h
@@ -0,0 +1,202 @@
+/*
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef AGX5_PINMUX_H
+#define AGX5_PINMUX_H
+
+/* PINMUX REGISTER ADDRESS */
+#define AGX5_PINMUX_PIN0SEL					0x10d13000
+#define AGX5_PINMUX_IO0CTRL					0x10d13130
+#define AGX5_PINMUX_EMAC0_USEFPGA				0x10d13300
+#define AGX5_PINMUX_IO0_DELAY					0x10d13400
+#define AGX5_PERIPHERAL						0x10d14044
+
+#include "socfpga_handoff.h"
+
+/* PINMUX DEFINE */
+#define PINMUX_HANDOFF_ARRAY_SIZE(x)				(sizeof(x) / sizeof((x)[0]))
+#define PINMUX_HANDOFF_CONFIG_ADDR				0xbeec
+#define PINMUX_HANDOFF_CONFIG_VAL				0x7e000
+
+/* Macros */
+#define SOCFPGA_PINMUX_SEL_NAND					(0x03)
+#define SOCFPGA_PINMUX_PIN0SEL					(0x00)
+#define SOCFPGA_PINMUX_PIN1SEL					(0x04)
+#define SOCFPGA_PINMUX_PIN2SEL					(0x08)
+#define SOCFPGA_PINMUX_PIN3SEL					(0x0C)
+#define SOCFPGA_PINMUX_PIN4SEL					(0x10)
+#define SOCFPGA_PINMUX_PIN5SEL					(0x14)
+#define SOCFPGA_PINMUX_PIN6SEL					(0x18)
+#define SOCFPGA_PINMUX_PIN7SEL					(0x1C)
+#define SOCFPGA_PINMUX_PIN8SEL					(0x20)
+#define SOCFPGA_PINMUX_PIN9SEL					(0x24)
+#define SOCFPGA_PINMUX_PIN10SEL					(0x28)
+#define SOCFPGA_PINMUX_PIN11SEL					(0x2C)
+#define SOCFPGA_PINMUX_PIN12SEL					(0x30)
+#define SOCFPGA_PINMUX_PIN13SEL					(0x34)
+#define SOCFPGA_PINMUX_PIN14SEL					(0x38)
+#define SOCFPGA_PINMUX_PIN15SEL					(0x3C)
+#define SOCFPGA_PINMUX_PIN16SEL					(0x40)
+#define SOCFPGA_PINMUX_PIN17SEL					(0x44)
+#define SOCFPGA_PINMUX_PIN18SEL					(0x48)
+#define SOCFPGA_PINMUX_PIN19SEL					(0x4C)
+#define SOCFPGA_PINMUX_PIN20SEL					(0x50)
+#define SOCFPGA_PINMUX_PIN21SEL					(0x54)
+#define SOCFPGA_PINMUX_PIN22SEL					(0x58)
+#define SOCFPGA_PINMUX_PIN23SEL					(0x5C)
+#define SOCFPGA_PINMUX_PIN24SEL					(0x60)
+#define SOCFPGA_PINMUX_PIN25SEL					(0x64)
+#define SOCFPGA_PINMUX_PIN26SEL					(0x68)
+#define SOCFPGA_PINMUX_PIN27SEL					(0x6C)
+#define SOCFPGA_PINMUX_PIN28SEL					(0x70)
+#define SOCFPGA_PINMUX_PIN29SEL					(0x74)
+#define SOCFPGA_PINMUX_PIN30SEL					(0x78)
+#define SOCFPGA_PINMUX_PIN31SEL					(0x7C)
+#define SOCFPGA_PINMUX_PIN32SEL					(0x80)
+#define SOCFPGA_PINMUX_PIN33SEL					(0x84)
+#define SOCFPGA_PINMUX_PIN34SEL					(0x88)
+#define SOCFPGA_PINMUX_PIN35SEL					(0x8C)
+#define SOCFPGA_PINMUX_PIN36SEL					(0x90)
+#define SOCFPGA_PINMUX_PIN37SEL					(0x94)
+#define SOCFPGA_PINMUX_PIN38SEL					(0x98)
+#define SOCFPGA_PINMUX_PIN39SEL					(0x9C)
+#define SOCFPGA_PINMUX_PIN40SEL					(0x100)
+#define SOCFPGA_PINMUX_PIN41SEL					(0x104)
+#define SOCFPGA_PINMUX_PIN42SEL					(0x108)
+#define SOCFPGA_PINMUX_PIN43SEL					(0x10C)
+#define SOCFPGA_PINMUX_PIN44SEL					(0x110)
+#define SOCFPGA_PINMUX_PIN45SEL					(0x114)
+#define SOCFPGA_PINMUX_PIN46SEL					(0x118)
+#define SOCFPGA_PINMUX_PIN47SEL					(0x11C)
+
+#define SOCFPGA_PINMUX_IO0CTRL					(0x00)
+#define SOCFPGA_PINMUX_IO1CTRL					(0x04)
+#define SOCFPGA_PINMUX_IO2CTRL					(0x08)
+#define SOCFPGA_PINMUX_IO3CTRL					(0x0C)
+#define SOCFPGA_PINMUX_IO4CTRL					(0x10)
+#define SOCFPGA_PINMUX_IO5CTRL					(0x14)
+#define SOCFPGA_PINMUX_IO6CTRL					(0x18)
+#define SOCFPGA_PINMUX_IO7CTRL					(0x1C)
+#define SOCFPGA_PINMUX_IO8CTRL					(0x20)
+#define SOCFPGA_PINMUX_IO9CTRL					(0x24)
+#define SOCFPGA_PINMUX_IO10CTRL					(0x28)
+#define SOCFPGA_PINMUX_IO11CTRL					(0x2C)
+#define SOCFPGA_PINMUX_IO12CTRL					(0x30)
+#define SOCFPGA_PINMUX_IO13CTRL					(0x34)
+#define SOCFPGA_PINMUX_IO14CTRL					(0x38)
+#define SOCFPGA_PINMUX_IO15CTRL					(0x3C)
+#define SOCFPGA_PINMUX_IO16CTRL					(0x40)
+#define SOCFPGA_PINMUX_IO17CTRL					(0x44)
+#define SOCFPGA_PINMUX_IO18CTRL					(0x48)
+#define SOCFPGA_PINMUX_IO19CTRL					(0x4C)
+#define SOCFPGA_PINMUX_IO20CTRL					(0x50)
+#define SOCFPGA_PINMUX_IO21CTRL					(0x54)
+#define SOCFPGA_PINMUX_IO22CTRL					(0x58)
+#define SOCFPGA_PINMUX_IO23CTRL					(0x5C)
+#define SOCFPGA_PINMUX_IO24CTRL					(0x60)
+#define SOCFPGA_PINMUX_IO25CTRL					(0x64)
+#define SOCFPGA_PINMUX_IO26CTRL					(0x68)
+#define SOCFPGA_PINMUX_IO27CTRL					(0x6C)
+#define SOCFPGA_PINMUX_IO28CTRL					(0xD0)
+#define SOCFPGA_PINMUX_IO29CTRL					(0xD4)
+#define SOCFPGA_PINMUX_IO30CTRL					(0xD8)
+#define SOCFPGA_PINMUX_IO31CTRL					(0xDC)
+#define SOCFPGA_PINMUX_IO32CTRL					(0xE0)
+#define SOCFPGA_PINMUX_IO33CTRL					(0xE4)
+#define SOCFPGA_PINMUX_IO34CTRL					(0xE8)
+#define SOCFPGA_PINMUX_IO35CTRL					(0xEC)
+#define SOCFPGA_PINMUX_IO36CTRL					(0xF0)
+#define SOCFPGA_PINMUX_IO37CTRL					(0xF4)
+#define SOCFPGA_PINMUX_IO38CTRL					(0xF8)
+#define SOCFPGA_PINMUX_IO39CTRL					(0xFC)
+#define SOCFPGA_PINMUX_IO40CTRL					(0x100)
+#define SOCFPGA_PINMUX_IO41CTRL					(0x104)
+#define SOCFPGA_PINMUX_IO42CTRL					(0x108)
+#define SOCFPGA_PINMUX_IO43CTRL					(0x10C)
+#define SOCFPGA_PINMUX_IO44CTRL					(0x110)
+#define SOCFPGA_PINMUX_IO45CTRL					(0x114)
+#define SOCFPGA_PINMUX_IO46CTRL					(0x118)
+#define SOCFPGA_PINMUX_IO47CTRL					(0x11C)
+
+#define SOCFPGA_PINMUX_EMAC0_USEFPGA				(0x00)
+#define SOCFPGA_PINMUX_EMAC1_USEFPGA				(0x04)
+#define SOCFPGA_PINMUX_EMAC2_USEFPGA				(0x08)
+#define SOCFPGA_PINMUX_I2C0_USEFPGA				(0x0C)
+#define SOCFPGA_PINMUX_I2C1_USEFPGA				(0x10)
+#define SOCFPGA_PINMUX_I2C_EMAC0_USEFPGA			(0x14)
+#define SOCFPGA_PINMUX_I2C_EMAC1_USEFPGA			(0x18)
+#define SOCFPGA_PINMUX_I2C_EMAC2_USEFPGA			(0x1C)
+#define SOCFPGA_PINMUX_NAND_USEFPGA				(0x20)
+#define SOCFPGA_PINMUX_SPIM0_USEFPGA				(0x28)
+#define SOCFPGA_PINMUX_SPIM1_USEFPGA				(0x2C)
+#define SOCFPGA_PINMUX_SPIS0_USEFPGA				(0x30)
+#define SOCFPGA_PINMUX_SPIS1_USEFPGA				(0x34)
+#define SOCFPGA_PINMUX_UART0_USEFPGA				(0x38)
+#define SOCFPGA_PINMUX_UART1_USEFPGA				(0x3C)
+#define SOCFPGA_PINMUX_MDIO0_USEFPGA				(0x40)
+#define SOCFPGA_PINMUX_MDIO1_USEFPGA				(0x44)
+#define SOCFPGA_PINMUX_MDIO2_USEFPGA				(0x48)
+#define SOCFPGA_PINMUX_JTAG_USEFPGA				(0x50)
+#define SOCFPGA_PINMUX_SDMMC_USEFPGA				(0x54)
+
+#define SOCFPGA_PINMUX_IO0DELAY					(0x00)
+#define SOCFPGA_PINMUX_IO1DELAY					(0x04)
+#define SOCFPGA_PINMUX_IO2DELAY					(0x08)
+#define SOCFPGA_PINMUX_IO3DELAY					(0x0C)
+#define SOCFPGA_PINMUX_IO4DELAY					(0x10)
+#define SOCFPGA_PINMUX_IO5DELAY					(0x14)
+#define SOCFPGA_PINMUX_IO6DELAY					(0x18)
+#define SOCFPGA_PINMUX_IO7DELAY					(0x1C)
+#define SOCFPGA_PINMUX_IO8DELAY					(0x20)
+#define SOCFPGA_PINMUX_IO9DELAY					(0x24)
+#define SOCFPGA_PINMUX_IO10DELAY				(0x28)
+#define SOCFPGA_PINMUX_IO11DELAY				(0x2C)
+#define SOCFPGA_PINMUX_IO12DELAY				(0x30)
+#define SOCFPGA_PINMUX_IO13DELAY				(0x34)
+#define SOCFPGA_PINMUX_IO14DELAY				(0x38)
+#define SOCFPGA_PINMUX_IO15DELAY				(0x3C)
+#define SOCFPGA_PINMUX_IO16DELAY				(0x40)
+#define SOCFPGA_PINMUX_IO17DELAY				(0x44)
+#define SOCFPGA_PINMUX_IO18DELAY				(0x48)
+#define SOCFPGA_PINMUX_IO19DELAY				(0x4C)
+#define SOCFPGA_PINMUX_IO20DELAY				(0x50)
+#define SOCFPGA_PINMUX_IO21DELAY				(0x54)
+#define SOCFPGA_PINMUX_IO22DELAY				(0x58)
+#define SOCFPGA_PINMUX_IO23DELAY				(0x5C)
+#define SOCFPGA_PINMUX_IO24DELAY				(0x60)
+#define SOCFPGA_PINMUX_IO25DELAY				(0x64)
+#define SOCFPGA_PINMUX_IO26DELAY				(0x68)
+#define SOCFPGA_PINMUX_IO27DELAY				(0x6C)
+#define SOCFPGA_PINMUX_IO28DELAY				(0x70)
+#define SOCFPGA_PINMUX_IO29DELAY				(0x74)
+#define SOCFPGA_PINMUX_IO30DELAY				(0x78)
+#define SOCFPGA_PINMUX_IO31DELAY				(0x7C)
+#define SOCFPGA_PINMUX_IO32DELAY				(0x80)
+#define SOCFPGA_PINMUX_IO33DELAY				(0x84)
+#define SOCFPGA_PINMUX_IO34DELAY				(0x88)
+#define SOCFPGA_PINMUX_IO35DELAY				(0x8C)
+#define SOCFPGA_PINMUX_IO36DELAY				(0x90)
+#define SOCFPGA_PINMUX_IO37DELAY				(0x94)
+#define SOCFPGA_PINMUX_IO38DELAY				(0x98)
+#define SOCFPGA_PINMUX_IO39DELAY				(0x9C)
+#define SOCFPGA_PINMUX_IO40DELAY				(0xA0)
+#define SOCFPGA_PINMUX_IO41DELAY				(0xA4)
+#define SOCFPGA_PINMUX_IO42DELAY				(0xA8)
+#define SOCFPGA_PINMUX_IO43DELAY				(0xAC)
+#define SOCFPGA_PINMUX_IO44DELAY				(0xB0)
+#define SOCFPGA_PINMUX_IO45DELAY				(0xB4)
+#define SOCFPGA_PINMUX_IO46DELAY				(0xB8)
+#define SOCFPGA_PINMUX_IO47DELAY				(0xBC)
+
+#define SOCFPGA_PINMUX_I3C0_USEFPGA				(0xC0)
+#define SOCFPGA_PINMUX_I3C1_USEFPGA				(0xC4)
+
+#define SOCFPGA_PINMUX(_reg)					(SOCFPGA_PINMUX_REG_BASE \
+								+ (SOCFPGA_PINMUX_##_reg))
+
+void config_pinmux(handoff *handoff);
+void config_peripheral(handoff *handoff);
+#endif
diff --git a/plat/intel/soc/agilex5/include/agilex5_power_manager.h b/plat/intel/soc/agilex5/include/agilex5_power_manager.h
new file mode 100644
index 0000000..1bba74b
--- /dev/null
+++ b/plat/intel/soc/agilex5/include/agilex5_power_manager.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef POWERMANAGER_H
+#define POWERMANAGER_H
+
+#include "socfpga_handoff.h"
+
+#define AGX5_PWRMGR_BASE					0x10d14000
+
+/* DSU */
+#define AGX5_PWRMGR_DSU_FWENCTL					0x0
+#define AGX5_PWRMGR_DSU_PGENCTL					0x4
+#define AGX5_PWRMGR_DSU_PGSTAT					0x8
+#define AGX5_PWRMGR_DSU_PWRCTLR					0xc
+#define AGX5_PWRMGR_DSU_PWRSTAT0				0x10
+#define AGX5_PWRMGR_DSU_PWRSTAT1				0x14
+
+/* DSU Macros*/
+#define AGX5_PWRMGR_DSU_FWEN(x)					((x) & 0xf)
+#define AGX5_PWRMGR_DSU_PGEN(x)					((x) & 0xf)
+#define AGX5_PWRMGR_DSU_PGEN_OUT(x)				((x) & 0xf)
+#define AGX5_PWRMGR_DSU_SINGLE_PACCEPT(x)			((x) & 0x1)
+#define AGX5_PWRMGR_DSU_SINGLE_PDENY(x)				(((x) & 0x1) << 1)
+#define AGX5_PWRMGR_DSU_SINGLE_FSM_STATE(x)			(((x) & 0xff) << 8)
+#define AGX5_PWRMGR_DSU_SINGLE_PCH_DONE(x)			(((x) & 0x1) << 31)
+#define AGX5_PWRMGR_DSU_MULTI_PACTIVE_IN(x)			((x) & 0xff)
+#define AGX5_PWRMGR_DSU_MULTI_PACCEPT(x)			(((x) & 0xff) << 8)
+#define AGX5_PWRMGR_DSU_MULTI_PDENY(x)				(((x) & 0xff) << 16)
+#define AGX5_PWRMGR_DSU_MULTI_PCH_DONE(x)			(((x) & 0x1) << 31)
+
+/* CPU */
+#define AGX5_PWRMGR_CPU_PWRCTLR0				0x18
+#define AGX5_PWRMGR_CPU_PWRCTLR1				0x20
+#define AGX5_PWRMGR_CPU_PWRCTLR2				0x28
+#define AGX5_PWRMGR_CPU_PWRCTLR3				0x30
+#define AGX5_PWRMGR_CPU_PWRSTAT0				0x1c
+#define AGX5_PWRMGR_CPU_PWRSTAT1				0x24
+#define AGX5_PWRMGR_CPU_PWRSTAT2				0x2c
+#define AGX5_PWRMGR_CPU_PWRSTAT3				0x34
+
+/* APS */
+#define AGX5_PWRMGR_APS_FWENCTL					0x38
+#define AGX5_PWRMGR_APS_PGENCTL					0x3C
+#define AGX5_PWRMGR_APS_PGSTAT					0x40
+
+/* PSS */
+#define AGX5_PWRMGR_PSS_FWENCTL					0x44
+#define AGX5_PWRMGR_PSS_PGENCTL					0x48
+#define AGX5_PWRMGR_PSS_PGSTAT					0x4c
+
+/* PSS Macros*/
+#define AGX5_PWRMGR_PSS_FWEN(x)					((x) & 0xff)
+#define AGX5_PWRMGR_PSS_PGEN(x)					((x) & 0xff)
+#define AGX5_PWRMGR_PSS_PGEN_OUT(x)				((x) & 0xff)
+
+/* MPU */
+#define AGX5_PWRMGR_MPU_PCHCTLR					0x50
+#define AGX5_PWRMGR_MPU_PCHSTAT					0x54
+#define AGX5_PWRMGR_MPU_BOOTCONFIG				0x58
+#define AGX5_PWRMGR_CPU_POWER_STATE_MASK			0x1E
+
+/* MPU Macros*/
+#define AGX5_PWRMGR_MPU_TRIGGER_PCH_DSU(x)			((x) & 0x1)
+#define AGX5_PWRMGR_MPU_TRIGGER_PCH_CPU(x)			(((x) & 0xf) << 1)
+#define AGX5_PWRMGR_MPU_STATUS_PCH_CPU(x)			(((x) & 0xf) << 1)
+
+/* Shared Macros */
+#define AGX5_PWRMGR(_reg)					(AGX5_PWRMGR_BASE + \
+								(AGX5_PWRMGR_##_reg))
+
+/* POWER MANAGER ERROR CODE */
+#define AGX5_PWRMGR_HANDOFF_PERIPHERAL				-1
+#define AGX5_PWRMGR_PSS_STAT_BUSY_E_BUSY			0x0
+#define AGX5_PWRMGR_PSS_STAT_BUSY(x)				(((x) & 0x000000FF) >> 0)
+
+int pss_sram_power_off(handoff *hoff_ptr);
+int wait_verify_fsm(uint16_t timeout, uint32_t peripheral_handoff);
+
+#endif
diff --git a/plat/intel/soc/agilex5/include/agilex5_system_manager.h b/plat/intel/soc/agilex5/include/agilex5_system_manager.h
new file mode 100644
index 0000000..9a58cdb
--- /dev/null
+++ b/plat/intel/soc/agilex5/include/agilex5_system_manager.h
@@ -0,0 +1,200 @@
+/*
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef AGX5_SOCFPGA_SYSTEMMANAGER_H
+#define AGX5_SOCFPGA_SYSTEMMANAGER_H
+
+#include "socfpga_plat_def.h"
+
+/* System Manager Register Map */
+#define SOCFPGA_SYSMGR_SILICONID_1					0x00
+#define SOCFPGA_SYSMGR_SILICONID_2					0x04
+#define SOCFPGA_SYSMGR_WDDBG						0x08
+#define SOCFPGA_SYSMGR_MPU_STATUS					0x10
+#define SOCFPGA_SYSMGR_SDMMC_L3_MASTER					0x2C
+#define SOCFPGA_SYSMGR_NAND_L3_MASTER					0x34
+#define SOCFPGA_SYSMGR_USB0_L3_MASTER					0x38
+#define SOCFPGA_SYSMGR_USB1_L3_MASTER					0x3C
+#define SOCFPGA_SYSMGR_TSN_GLOBAL					0x40
+#define SOCFPGA_SYSMGR_EMAC_0						0x44 /* TSN_0 */
+#define SOCFPGA_SYSMGR_EMAC_1						0x48 /* TSN_1 */
+#define SOCFPGA_SYSMGR_EMAC_2						0x4C /* TSN_2 */
+#define SOCFPGA_SYSMGR_TSN_0_ACE					0x50
+#define SOCFPGA_SYSMGR_TSN_1_ACE					0x54
+#define SOCFPGA_SYSMGR_TSN_2_ACE					0x58
+#define SOCFPGA_SYSMGR_FPGAINTF_EN_1					0x68
+#define SOCFPGA_SYSMGR_FPGAINTF_EN_2					0x6C
+#define SOCFPGA_SYSMGR_FPGAINTF_EN_3					0x70
+#define SOCFPGA_SYSMGR_DMAC0_L3_MASTER					0x74
+#define SOCFPGA_SYSMGR_ETR_L3_MASTER					0x78
+#define SOCFPGA_SYSMGR_DMAC1_L3_MASTER					0x7C
+#define SOCFPGA_SYSMGR_SEC_CTRL_SLT					0x80
+#define SOCFPGA_SYSMGR_OSC_TRIM						0x84
+#define SOCFPGA_SYSMGR_DMAC0_CTRL_STATUS_REG				0x88
+#define SOCFPGA_SYSMGR_DMAC1_CTRL_STATUS_REG				0x8C
+#define SOCFPGA_SYSMGR_ECC_INTMASK_VALUE				0x90
+#define SOCFPGA_SYSMGR_ECC_INTMASK_SET					0x94
+#define SOCFPGA_SYSMGR_ECC_INTMASK_CLR					0x98
+#define SOCFPGA_SYSMGR_ECC_INTMASK_SERR					0x9C
+#define SOCFPGA_SYSMGR_ECC_INTMASK_DERR					0xA0
+/* NOC configuration value */
+#define SOCFPGA_SYSMGR_NOC_TIMEOUT					0xC0
+#define SOCFPGA_SYSMGR_NOC_IDLEREQ_SET					0xC4
+#define SOCFPGA_SYSMGR_NOC_IDLEREQ_CLR					0xC8
+#define SOCFPGA_SYSMGR_NOC_IDLEREQ_VAL					0xCC
+#define SOCFPGA_SYSMGR_NOC_IDLEACK					0xD0
+#define SOCFPGA_SYSMGR_NOC_IDLESTATUS					0xD4
+#define SOCFPGA_SYSMGR_FPGA2SOC_CTRL					0xD8
+#define SOCFPGA_SYSMGR_FPGA_CFG						0xDC
+#define SOCFPGA_SYSMGR_GPO						0xE4
+#define SOCFPGA_SYSMGR_GPI						0xE8
+#define SOCFPGA_SYSMGR_MPU						0xF0
+#define SOCFPGA_SYSMGR_SDM_HPS_SPARE					0xF4
+#define SOCFPGA_SYSMGR_HPS_SDM_SPARE					0xF8
+#define SOCFPGA_SYSMGR_DFI_INTF						0xFC
+#define SOCFPGA_SYSMGR_NAND_DD_CTRL					0x100
+#define SOCFPGA_SYSMGR_NAND_PHY_CTRL_REG				0x104
+#define SOCFPGA_SYSMGR_NAND_PHY_TSEL_REG				0x108
+#define SOCFPGA_SYSMGR_NAND_DQ_TIMING_REG				0x10C
+#define SOCFPGA_SYSMGR_PHY_DQS_TIMING_REG				0x110
+#define SOCFPGA_SYSMGR_NAND_PHY_GATE_LPBK_CTRL_REG			0x114
+#define SOCFPGA_SYSMGR_NAND_PHY_DLL_MASTER_CTRL_REG			0x118
+#define SOCFPGA_SYSMGR_NAND_PHY_DLL_SLAVE_CTRL_REG			0x11C
+#define SOCFPGA_SYSMGR_NAND_DD_DEFAULT_SETTING_REG0			0x120
+#define SOCFPGA_SYSMGR_NAND_DD_DEFAULT_SETTING_REG1			0x124
+#define SOCFPGA_SYSMGR_NAND_DD_STATUS_REG				0x128
+#define SOCFPGA_SYSMGR_NAND_DD_ID_LOW_REG				0x12C
+#define SOCFPGA_SYSMGR_NAND_DD_ID_HIGH_REG				0x130
+#define SOCFPGA_SYSMGR_NAND_WRITE_PROT_EN_REG				0x134
+#define SOCFPGA_SYSMGR_SDMMC_CMD_QUEUE_SETTING_REG			0x138
+#define SOCFPGA_SYSMGR_I3C_SLV_PID_LOW					0x13C
+#define SOCFPGA_SYSMGR_I3C_SLV_PID_HIGH					0x140
+#define SOCFPGA_SYSMGR_I3C_SLV_CTRL_0					0x144
+#define SOCFPGA_SYSMGR_I3C_SLV_CTRL_1					0x148
+#define SOCFPGA_SYSMGR_F2S_BRIDGE_CTRL					0x14C
+#define SOCFPGA_SYSMGR_DMA_TBU_STASH_CTRL_REG_0_DMA0			0x150
+#define SOCFPGA_SYSMGR_DMA_TBU_STASH_CTRL_REG_0_DMA1			0x154
+#define SOCFPGA_SYSMGR_SDM_TBU_STASH_CTRL_REG_1_SDM			0x158
+#define SOCFPGA_SYSMGR_IO_TBU_STASH_CTRL_REG_2_USB2			0x15C
+#define SOCFPGA_SYSMGR_IO_TBU_STASH_CTRL_REG_2_USB3			0x160
+#define SOCFPGA_SYSMGR_IO_TBU_STASH_CTRL_REG_2_SDMMC			0x164
+#define SOCFPGA_SYSMGR_IO_TBU_STASH_CTRL_REG_2_NAND			0x168
+#define SOCFPGA_SYSMGR_IO_TBU_STASH_CTRL_REG_2_ETR			0x16C
+#define SOCFPGA_SYSMGR_TSN_TBU_STASH_CTRL_REG_3_TSN0			0x170
+#define SOCFPGA_SYSMGR_TSN_TBU_STASH_CTRL_REG_3_TSN1			0x174
+#define SOCFPGA_SYSMGR_TSN_TBU_STASH_CTRL_REG_3_TSN2			0x178
+#define SOCFPGA_SYSMGR_DMA_TBU_STREAM_CTRL_REG_0_DMA0			0x17C
+#define SOCFPGA_SYSMGR_DMA_TBU_STREAM_CTRL_REG_0_DMA1			0x180
+#define SOCFPGA_SYSMGR_SDM_TBU_STREAM_CTRL_REG_1_SDM			0x184
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_CTRL_REG_2_USB2			0x188
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_CTRL_REG_2_USB3			0x18C
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_CTRL_REG_2_SDMMC			0x190
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_CTRL_REG_2_NAND			0x194
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_CTRL_REG_2_ETR			0x198
+#define SOCFPGA_SYSMGR_TSN_TBU_STREAM_CTRL_REG_3_TSN0			0x19C
+#define SOCFPGA_SYSMGR_TSN_TBU_STREAM_CTRL_REG_3_TSN1			0x1A0
+#define SOCFPGA_SYSMGR_TSN_TBU_STREAM_CTRL_REG_3_TSN2			0x1A4
+#define SOCFPGA_SYSMGR_DMA_TBU_STREAM_ID_AX_REG_0_DMA0			0x1A8
+#define SOCFPGA_SYSMGR_DMA_TBU_STREAM_ID_AX_REG_0_DMA1			0x1AC
+#define SOCFPGA_SYSMGR_SDM_TBU_STREAM_ID_AX_REG_1_SDM			0x1B0
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_ID_AX_REG_2_USB2			0x1B4
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_ID_AX_REG_2_USB3			0x1B8
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_ID_AX_REG_2_SDMMC			0x1BC
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_ID_AX_REG_2_NAND			0x1C0
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_ID_AX_REG_2_ETR			0x1C4
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_ID_AX_REG_2_TSN0			0x1C8
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_ID_AX_REG_2_TSN1			0x1CC
+#define SOCFPGA_SYSMGR_IO_TBU_STREAM_ID_AX_REG_2_TSN2			0x1D0
+#define SOCFPGA_SYSMGR_USB3_MISC_CTRL_REG0				0x1F0
+#define SOCFPGA_SYSMGR_USB3_MISC_CTRL_REG1				0x1F4
+
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_0				0x200
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_1				0x204
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_2				0x208
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_3				0x20C
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_4				0x210
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_5				0x214
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_6				0x218
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_7				0x21C
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_8				0x220
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_COLD_9				0x224
+#define SOCFPGA_SYSMGR_MPFE_CONFIG					0x228
+#define SOCFPGA_SYSMGR_MPFE_status					0x22C
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_0				0x230
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_1				0x234
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_2				0x238
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_3				0x23C
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_4				0x240
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_5				0x244
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_6				0x248
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_7				0x24C
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_8				0x250
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_WARM_9				0x254
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_0				0x258
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_1				0x25C
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_2				0x260
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_3				0x264
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_4				0x268
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_5				0x26C
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_6				0x270
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_7				0x274
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_8				0x278
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_9				0x27C
+
+#define DMA0_STREAM_CTRL_REG						0x10D1217C
+#define DMA1_STREAM_CTRL_REG						0x10D12180
+#define SDM_STREAM_CTRL_REG						0x10D12184
+#define USB2_STREAM_CTRL_REG						0x10D12188
+#define USB3_STREAM_CTRL_REG						0x10D1218C
+#define SDMMC_STREAM_CTRL_REG						0x10D12190
+#define NAND_STREAM_CTRL_REG						0x10D12194
+#define ETR_STREAM_CTRL_REG						0x10D12198
+#define TSN0_STREAM_CTRL_REG						0x10D1219C
+#define TSN1_STREAM_CTRL_REG						0x10D121A0
+#define TSN2_STREAM_CTRL_REG						0x10D121A4
+
+/* Stream ID configuration value for Agilex5 */
+#define TSN0								0x00010001
+#define TSN1								0x00020002
+#define TSN2								0x00030003
+#define NAND								0x00040004
+#define SDMMC								0x00050005
+#define USB0								0x00060006
+#define USB1								0x00070007
+#define DMA0								0x00080008
+#define DMA1								0x00090009
+#define SDM								0x000A000A
+#define CORE_SIGHT_DEBUG						0x000B000B
+
+/* Field Masking */
+#define SYSMGR_SDMMC_DRVSEL(x)						(((x) & 0x7) << 0)
+#define SYSMGR_SDMMC_SMPLSEL(x)						(((x) & 0x7) << 4)
+
+#define SYSMGR_F2S_BRIDGE_CTRL_EN					BIT(0)
+#define IDLE_DATA_LWSOC2FPGA						BIT(4)
+#define IDLE_DATA_SOC2FPGA						BIT(0)
+#define IDLE_DATA_MASK							(IDLE_DATA_LWSOC2FPGA \
+									| IDLE_DATA_SOC2FPGA)
+#define SYSMGR_ECC_OCRAM_MASK						BIT(1)
+#define SYSMGR_ECC_DDR0_MASK						BIT(16)
+#define SYSMGR_ECC_DDR1_MASK						BIT(17)
+
+#define WSTREAMIDEN_REG_CTRL						BIT(0)
+#define RSTREAMIDEN_REG_CTRL						BIT(1)
+#define WMMUSECSID_REG_VAL						BIT(4)
+#define RMMUSECSID_REG_VAL						BIT(5)
+
+/* Macros */
+#define SOCFPGA_SYSMGR(_reg)						(SOCFPGA_SYSMGR_REG_BASE \
+									+ (SOCFPGA_SYSMGR_##_reg))
+
+#define ENABLE_STREAMID							WSTREAMIDEN_REG_CTRL \
+									| RSTREAMIDEN_REG_CTRL
+#define ENABLE_STREAMID_SECURE_TX					WSTREAMIDEN_REG_CTRL \
+									| RSTREAMIDEN_REG_CTRL \
+									| WMMUSECSID_REG_VAL \
+									| RMMUSECSID_REG_VAL
+
+#endif /* AGX5_SOCFPGA_SYSTEMMANAGER_H */
diff --git a/plat/intel/soc/agilex5/include/socfpga_plat_def.h b/plat/intel/soc/agilex5/include/socfpga_plat_def.h
new file mode 100644
index 0000000..8a49d61
--- /dev/null
+++ b/plat/intel/soc/agilex5/include/socfpga_plat_def.h
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_SOCFPGA_DEF_H
+#define PLAT_SOCFPGA_DEF_H
+
+#include "agilex5_memory_controller.h"
+#include "agilex5_system_manager.h"
+#include <platform_def.h>
+
+/* Platform Setting */
+#define PLATFORM_MODEL						PLAT_SOCFPGA_AGILEX5
+#define BOOT_SOURCE						BOOT_SOURCE_SDMMC
+#define MMC_DEVICE_TYPE						1  /* MMC = 0, SD = 1 */
+#define XLAT_TABLES_V2						U(1)
+#define PLAT_PRIMARY_CPU_A55					0x000
+#define PLAT_PRIMARY_CPU_A76					0x200
+#define PLAT_CLUSTER_ID_MPIDR_AFF_SHIFT				MPIDR_AFF2_SHIFT
+#define PLAT_CPU_ID_MPIDR_AFF_SHIFT				MPIDR_AFF1_SHIFT
+#define PLAT_L2_RESET_REQ			0xB007C0DE
+
+/* System Counter */ /* TODO: Update back to 400MHz */
+#define PLAT_SYS_COUNTER_FREQ_IN_TICKS				(80000000)
+#define PLAT_SYS_COUNTER_FREQ_IN_MHZ				(80)
+
+/* FPGA config helpers */
+#define INTEL_SIP_SMC_FPGA_CONFIG_ADDR				0x400000
+#define INTEL_SIP_SMC_FPGA_CONFIG_SIZE				0x2000000
+
+/* QSPI Setting */
+#define CAD_QSPIDATA_OFST					0x10900000
+#define CAD_QSPI_OFFSET						0x108d2000
+
+/* Register Mapping */
+#define SOCFPGA_CCU_NOC_REG_BASE				0x1c000000
+#define SOCFPGA_F2SDRAMMGR_REG_BASE				0x18001000
+
+#define SOCFPGA_MMC_REG_BASE					0x10808000
+#define SOCFPGA_MEMCTRL_REG_BASE				0x108CC000
+#define SOCFPGA_RSTMGR_REG_BASE					0x10d11000
+#define SOCFPGA_SYSMGR_REG_BASE					0x10d12000
+#define SOCFPGA_PINMUX_REG_BASE					0x10d13000
+#define SOCFPGA_NAND_REG_BASE					0x10B80000
+
+#define SOCFPGA_L4_PER_SCR_REG_BASE				0x10d21000
+#define SOCFPGA_L4_SYS_SCR_REG_BASE				0x10d21100
+#define SOCFPGA_SOC2FPGA_SCR_REG_BASE				0x10d21200
+#define SOCFPGA_LWSOC2FPGA_SCR_REG_BASE				0x10d21300
+
+/* Define maximum page size for NAND flash devices */
+#define PLATFORM_MTD_MAX_PAGE_SIZE				U(0x1000)
+
+/*******************************************************************************
+ * Platform memory map related constants
+ ******************************************************************************/
+#define DRAM_BASE						(0x80000000)
+#define DRAM_SIZE						(0x80000000)
+
+#define OCRAM_BASE						(0x00000000)
+#define OCRAM_SIZE						(0x00080000)
+
+#define MEM64_BASE						(0x0080000000)
+#define MEM64_SIZE						(0x0080000000)
+
+//128MB PSS
+#define PSS_BASE						(0x10000000)
+#define PSS_SIZE						(0x08000000)
+
+//64MB MPFE
+#define MPFE_BASE						(0x18000000)
+#define MPFE_SIZE						(0x04000000)
+
+//16MB CCU
+#define CCU_BASE						(0x1C000000)
+#define CCU_SIZE						(0x01000000)
+
+//1MB GIC
+#define GIC_BASE						(0x1D000000)
+#define GIC_SIZE						(0x00100000)
+
+#define BL2_BASE						(0x00000000)
+#define BL2_LIMIT						(0x0001b000)
+
+#define BL31_BASE						(0x80000000)
+#define BL31_LIMIT						(0x82000000)
+
+/*******************************************************************************
+ * UART related constants
+ ******************************************************************************/
+#define PLAT_UART0_BASE						(0x10C02000)
+#define PLAT_UART1_BASE						(0x10C02100)
+
+/*******************************************************************************
+ * GIC related constants
+ ******************************************************************************/
+#define PLAT_GIC_BASE						(0x1D000000)
+#define PLAT_GICC_BASE						(PLAT_GIC_BASE + 0x20000)
+#define PLAT_GICD_BASE						(PLAT_GIC_BASE + 0x00000)
+#define PLAT_GICR_BASE						(PLAT_GIC_BASE + 0x60000)
+
+#define PLAT_INTEL_SOCFPGA_GICR_BASE				PLAT_GICR_BASE
+
+/*******************************************************************************
+ * SDMMC related pointer function
+ ******************************************************************************/
+#define SDMMC_READ_BLOCKS	sdmmc_read_blocks
+#define SDMMC_WRITE_BLOCKS	sdmmc_write_blocks
+
+/*******************************************************************************
+ * sysmgr.boot_scratch_cold6 & 7 (64bit) are used to indicate L2 reset
+ * is done and HPS should trigger warm reset via RMR_EL3.
+ ******************************************************************************/
+#define L2_RESET_DONE_REG			0x10D12218
+
+#endif /* PLAT_SOCFPGA_DEF_H */
diff --git a/plat/intel/soc/agilex5/platform.mk b/plat/intel/soc/agilex5/platform.mk
new file mode 100644
index 0000000..779c629
--- /dev/null
+++ b/plat/intel/soc/agilex5/platform.mk
@@ -0,0 +1,107 @@
+#
+# Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+include lib/xlat_tables_v2/xlat_tables.mk
+PLAT_INCLUDES		:=	\
+			-Iplat/intel/soc/agilex5/include/		\
+			-Iplat/intel/soc/common/drivers/		\
+			-Iplat/intel/soc/common/include/
+
+# GIC-600 configuration
+GICV3_SUPPORT_GIC600	:=	1
+# Include GICv3 driver files
+include drivers/arm/gic/v3/gicv3.mk
+AGX5_GICv3_SOURCES	:=	\
+			${GICV3_SOURCES}				\
+			plat/common/plat_gicv3.c
+
+PLAT_BL_COMMON_SOURCES	:=	\
+			${AGX5_GICv3_SOURCES}				\
+			drivers/cadence/combo_phy/cdns_combo_phy.c	\
+			drivers/cadence/emmc/cdns_sdmmc.c	\
+			drivers/cadence/nand/cdns_nand.c	\
+			drivers/delay_timer/delay_timer.c		\
+			drivers/delay_timer/generic_delay_timer.c	\
+			drivers/ti/uart/aarch64/16550_console.S		\
+			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/drivers/combophy/combophy.c			\
+			plat/intel/soc/common/drivers/sdmmc/sdmmc.c			\
+			plat/intel/soc/common/drivers/ddr/ddr.c			\
+			plat/intel/soc/common/drivers/nand/nand.c			\
+			plat/intel/soc/common/socfpga_delay_timer.c
+
+BL2_SOURCES		+=	\
+		common/desc_image_load.c				\
+		lib/xlat_tables_v2/aarch64/enable_mmu.S	\
+		lib/xlat_tables_v2/xlat_tables_context.c \
+		lib/xlat_tables_v2/xlat_tables_core.c \
+		lib/xlat_tables_v2/aarch64/xlat_tables_arch.c \
+		lib/xlat_tables_v2/xlat_tables_utils.c \
+		drivers/mmc/mmc.c					\
+		drivers/intel/soc/stratix10/io/s10_memmap_qspi.c	\
+		drivers/io/io_storage.c					\
+		drivers/io/io_block.c					\
+		drivers/io/io_fip.c					\
+		drivers/io/io_mtd.c					\
+		drivers/partition/partition.c				\
+		drivers/partition/gpt.c					\
+		drivers/synopsys/emmc/dw_mmc.c				\
+		lib/cpus/aarch64/cortex_a55.S				\
+		lib/cpus/aarch64/cortex_a76.S				\
+		plat/intel/soc/agilex5/soc/agilex5_clock_manager.c	\
+		plat/intel/soc/agilex5/soc/agilex5_memory_controller.c	\
+		plat/intel/soc/agilex5/soc/agilex5_mmc.c			\
+		plat/intel/soc/agilex5/soc/agilex5_pinmux.c		\
+		plat/intel/soc/agilex5/soc/agilex5_power_manager.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/socfpga_vab.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/drivers/qspi/cadence_qspi.c	\
+		plat/intel/soc/agilex5/bl2_plat_setup.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					\
+		${XLAT_TABLES_LIB_SRCS}						\
+		lib/cpus/aarch64/aem_generic.S				\
+		lib/cpus/aarch64/cortex_a55.S				\
+		lib/cpus/aarch64/cortex_a76.S				\
+		plat/common/plat_psci_common.c				\
+		plat/intel/soc/agilex5/bl31_plat_setup.c		\
+		plat/intel/soc/agilex5/soc/agilex5_power_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		\
+		plat/intel/soc/common/soc/socfpga_mailbox.c		\
+		plat/intel/soc/common/soc/socfpga_reset_manager.c
+
+# Configs for A76 and A55
+HW_ASSISTED_COHERENCY := 1
+USE_COHERENT_MEM := 0
+CTX_INCLUDE_AARCH32_REGS := 0
+ERRATA_A55_1530923 := 1
+
+$(eval $(call add_define,ARM_PRELOADED_DTB_BASE))
+
+PROGRAMMABLE_RESET_ADDRESS	:= 0
+RESET_TO_BL2			:= 1
+BL2_INV_DCACHE			:= 0
+MULTI_CONSOLE_API		:= 1
diff --git a/plat/intel/soc/agilex5/soc/agilex5_clock_manager.c b/plat/intel/soc/agilex5/soc/agilex5_clock_manager.c
new file mode 100644
index 0000000..cc68153
--- /dev/null
+++ b/plat/intel/soc/agilex5/soc/agilex5_clock_manager.c
@@ -0,0 +1,253 @@
+/*
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#include "agilex5_clock_manager.h"
+#include "agilex5_system_manager.h"
+#include "socfpga_handoff.h"
+
+uint32_t wait_pll_lock(void)
+{
+	uint32_t data;
+	uint32_t count = 0;
+
+	do {
+		data = mmio_read_32(CLKMGR_OFFSET + CLKMGR_STAT);
+		count++;
+		if (count >= 1000)
+			return -ETIMEDOUT;
+
+	} while ((CLKMGR_STAT_MAINPLLLOCKED(data) == 0) ||
+			(CLKMGR_STAT_PERPLLLOCKED(data) == 0));
+	return 0;
+}
+
+uint32_t wait_fsm(void)
+{
+	uint32_t data;
+	uint32_t count = 0;
+
+	do {
+		data = mmio_read_32(CLKMGR_OFFSET + CLKMGR_STAT);
+		count++;
+		if (count >= 1000)
+			return -ETIMEDOUT;
+
+	} while (CLKMGR_STAT_BUSY(data) == CLKMGR_STAT_BUSY_E_BUSY);
+
+	return 0;
+}
+
+uint32_t pll_source_sync_config(uint32_t pll_mem_offset, uint32_t data)
+{
+	uint32_t val = 0;
+	uint32_t count = 0;
+	uint32_t req_status = 0;
+
+	val = (CLKMGR_MEM_WR | CLKMGR_MEM_REQ |
+		(data << CLKMGR_MEM_WDAT_OFFSET) | CLKMGR_MEM_ADDR);
+	mmio_write_32(pll_mem_offset, val);
+
+	do {
+		req_status = mmio_read_32(pll_mem_offset);
+		count++;
+	} while ((req_status & CLKMGR_MEM_REQ) && (count < 10));
+
+	if (count >= 10)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+uint32_t pll_source_sync_read(uint32_t pll_mem_offset)
+{
+	uint32_t val = 0;
+	uint32_t rdata = 0;
+	uint32_t count = 0;
+	uint32_t req_status = 0;
+
+	val = (CLKMGR_MEM_REQ | CLKMGR_MEM_ADDR);
+	mmio_write_32(pll_mem_offset, val);
+
+	do {
+		req_status = mmio_read_32(pll_mem_offset);
+		count++;
+	} while ((req_status & CLKMGR_MEM_REQ) && (count < 10));
+
+	if (count >= 10)
+		return -ETIMEDOUT;
+
+	rdata = mmio_read_32(pll_mem_offset + 0x4);
+	INFO("rdata (%x) = %x\n", pll_mem_offset + 0x4, rdata);
+
+	return rdata;
+}
+
+void config_clkmgr_handoff(handoff *hoff_ptr)
+{
+	/* Take both PLL out of reset and power up */
+
+	mmio_setbits_32(CLKMGR_MAINPLL + CLKMGR_MAINPLL_PLLGLOB,
+			CLKMGR_PLLGLOB_PD_SET_MSK |
+			CLKMGR_PLLGLOB_RST_SET_MSK);
+	mmio_setbits_32(CLKMGR_PERPLL + CLKMGR_PERPLL_PLLGLOB,
+			CLKMGR_PLLGLOB_PD_SET_MSK |
+			CLKMGR_PLLGLOB_RST_SET_MSK);
+
+	/* PLL lock */
+	wait_pll_lock();
+
+	/* Bypass all mainpllgrp's clocks to input clock ref */
+	mmio_write_32(CLKMGR_MAINPLL + CLKMGR_MAINPLL_BYPASSS, 0xff);
+	/* Bypass all perpllgrp's clocks to input clock ref */
+	mmio_write_32(CLKMGR_PERPLL + CLKMGR_PERPLL_BYPASS, 0xff);
+
+	/* Pass clock source frequency into scratch register */
+	mmio_write_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_1),
+		hoff_ptr->hps_osc_clk_hz);
+	mmio_write_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_2),
+		hoff_ptr->fpga_clk_hz);
+
+	/* Take all PLLs out of bypass */
+	mmio_write_32(CLKMGR_MAINPLL + CLKMGR_MAINPLL_BYPASS, 0);
+	wait_fsm();
+	mmio_write_32(CLKMGR_PERPLL + CLKMGR_PERPLL_BYPASS, 0);
+	wait_fsm();
+
+	/* Enable mainpllgrp's software-managed clock */
+	mmio_write_32(CLKMGR_MAINPLL + CLKMGR_MAINPLL_EN,
+			CLKMGR_MAINPLL_EN_RESET);
+	mmio_write_32(CLKMGR_PERPLL + CLKMGR_PERPLL_EN,
+			CLKMGR_PERPLL_EN_RESET);
+}
+
+/* Extract reference clock from platform clock source */
+uint32_t get_ref_clk(uint32_t pllglob)
+{
+	uint32_t arefclkdiv, ref_clk;
+	uint32_t scr_reg;
+
+	switch (CLKMGR_PSRC(pllglob)) {
+	case CLKMGR_PLLGLOB_PSRC_EOSC1:
+		scr_reg = SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_1);
+		ref_clk = mmio_read_32(scr_reg);
+		break;
+	case CLKMGR_PLLGLOB_PSRC_INTOSC:
+		ref_clk = CLKMGR_INTOSC_HZ;
+		break;
+	case CLKMGR_PLLGLOB_PSRC_F2S:
+		scr_reg = SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_2);
+		ref_clk = mmio_read_32(scr_reg);
+		break;
+	default:
+		ref_clk = 0;
+		assert(0);
+		break;
+	}
+
+	arefclkdiv = CLKMGR_PLLGLOB_AREFCLKDIV(pllglob);
+	ref_clk /= arefclkdiv;
+
+	return ref_clk;
+}
+
+/* Calculate clock frequency based on parameter */
+uint32_t get_clk_freq(uint32_t psrc_reg, uint32_t main_pllc, uint32_t per_pllc)
+{
+	uint32_t ref_clk = 0;
+
+	uint32_t clk_psrc, mdiv;
+	uint32_t pllm_reg, pllc_reg, pllc_div, pllglob_reg;
+
+
+	clk_psrc = mmio_read_32(CLKMGR_MAINPLL + psrc_reg);
+	clk_psrc = 0;
+
+	switch (clk_psrc) {
+	case 0:
+		pllm_reg = CLKMGR_MAINPLL + CLKMGR_MAINPLL_PLLM;
+		pllc_reg = CLKMGR_MAINPLL + main_pllc;
+		pllglob_reg = CLKMGR_MAINPLL + CLKMGR_MAINPLL_PLLGLOB;
+		break;
+	}
+
+	ref_clk = get_ref_clk(mmio_read_32(pllglob_reg));
+	mdiv = CLKMGR_PLLM_MDIV(mmio_read_32(pllm_reg));
+	ref_clk *= mdiv;
+
+	pllc_div = mmio_read_32(pllc_reg) & 0x7ff;
+	NOTICE("return = %d Hz\n", (ref_clk / pllc_div));
+
+	ref_clk = 200000000;
+	return (uint32_t) ref_clk;
+
+}
+
+/* Return L3 interconnect clock */
+uint32_t get_l3_clk(void)
+{
+	uint32_t l3_clk;
+
+	l3_clk = get_clk_freq(CLKMGR_MAINPLL_NOCCLK, CLKMGR_MAINPLL_PLLC1,
+				CLKMGR_PERPLL_PLLC1);
+	return l3_clk;
+}
+
+/* Calculate clock frequency to be used for watchdog timer */
+uint32_t get_wdt_clk(void)
+{
+	uint32_t l3_clk, l4_sys_clk;
+
+	l3_clk = get_l3_clk();
+	l4_sys_clk = l3_clk / 4;
+
+	return l4_sys_clk;
+}
+
+/* Calculate clock frequency to be used for UART driver */
+uint32_t get_uart_clk(void)
+{
+	uint32_t data32, l3_clk, l4_sp_clk;
+
+	l3_clk = get_l3_clk();
+
+	data32 = mmio_read_32(CLKMGR_MAINPLL + CLKMGR_MAINPLL_NOCDIV);
+	data32 = (data32 >> 16) & 0x3;
+
+	l4_sp_clk = l3_clk >> data32;
+
+	return l4_sp_clk;
+}
+
+/* Calculate clock frequency to be used for SDMMC driver */
+uint32_t get_mmc_clk(void)
+{
+	uint32_t mmc_clk;
+
+	//TODO: To update when handoff data is ready
+	//uint32_t data32;
+
+	//mmc_clk = get_clk_freq(CLKMGR_ALTERA_SDMMCCTR, CLKMGR_MAINPLL_PLLC3, CLKMGR_PERPLL_PLLC3);
+
+	//data32 = mmio_read_32(CLKMGR_ALTERA + CLKMGR_ALTERA_SDMMCCTR);
+	//data32 = (data32 & 0x7ff) + 1;
+	//mmc_clk = (mmc_clk / data32) / 4;
+
+
+	mmc_clk = get_clk_freq(CLKMGR_MAINPLL_NOCCLK, CLKMGR_MAINPLL_PLLC3,
+				CLKMGR_PERPLL_PLLC3);
+
+	// TODO: To update when handoff data is ready
+	NOTICE("mmc_clk = %d Hz\n", mmc_clk);
+
+	return mmc_clk;
+}
diff --git a/plat/intel/soc/agilex5/soc/agilex5_memory_controller.c b/plat/intel/soc/agilex5/soc/agilex5_memory_controller.c
new file mode 100644
index 0000000..0ddff4a
--- /dev/null
+++ b/plat/intel/soc/agilex5/soc/agilex5_memory_controller.c
@@ -0,0 +1,400 @@
+/*
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+#include "agilex5_memory_controller.h"
+#include <platform_def.h>
+
+#define ALT_CCU_NOC_DI_SET_MSK		0x10
+
+#define DDR_READ_LATENCY_DELAY		40
+#define MAX_MEM_CAL_RETRY		3
+#define PRE_CALIBRATION_DELAY		1
+#define POST_CALIBRATION_DELAY		1
+#define TIMEOUT_EMIF_CALIBRATION	1000
+#define CLEAR_EMIF_DELAY		1000
+#define CLEAR_EMIF_TIMEOUT		1000
+
+#define DDR_CONFIG(A, B, C, R)	(((A) << 24) | ((B) << 16) | ((C) << 8) | (R))
+#define DDR_CONFIG_ELEMENTS	(ARRAY_SIZE(ddr_config))
+
+/* tWR = Min. 15ns constant, see JEDEC standard eg. DDR4 is JESD79-4.pdf */
+#define tWR_IN_NS 15
+
+void configure_hmc_adaptor_regs(void);
+void configure_ddr_sched_ctrl_regs(void);
+
+/* The followring are the supported configurations */
+uint32_t ddr_config[] = {
+	/* DDR_CONFIG(Address order,Bank,Column,Row) */
+	/* List for DDR3 or LPDDR3 (pinout order > chip, row, bank, column) */
+	DDR_CONFIG(0, 3, 10, 12),
+	DDR_CONFIG(0, 3,  9, 13),
+	DDR_CONFIG(0, 3, 10, 13),
+	DDR_CONFIG(0, 3,  9, 14),
+	DDR_CONFIG(0, 3, 10, 14),
+	DDR_CONFIG(0, 3, 10, 15),
+	DDR_CONFIG(0, 3, 11, 14),
+	DDR_CONFIG(0, 3, 11, 15),
+	DDR_CONFIG(0, 3, 10, 16),
+	DDR_CONFIG(0, 3, 11, 16),
+	DDR_CONFIG(0, 3, 12, 15),	/* 0xa */
+	/* List for DDR4 only (pinout order > chip, bank, row, column) */
+	DDR_CONFIG(1, 3, 10, 14),
+	DDR_CONFIG(1, 4, 10, 14),
+	DDR_CONFIG(1, 3, 10, 15),
+	DDR_CONFIG(1, 4, 10, 15),
+	DDR_CONFIG(1, 3, 10, 16),
+	DDR_CONFIG(1, 4, 10, 16),
+	DDR_CONFIG(1, 3, 10, 17),
+	DDR_CONFIG(1, 4, 10, 17),
+};
+
+static int match_ddr_conf(uint32_t ddr_conf)
+{
+	int i;
+
+	for (i = 0; i < DDR_CONFIG_ELEMENTS; i++) {
+		if (ddr_conf == ddr_config[i])
+			return i;
+	}
+	return 0;
+}
+
+static int check_hmc_clk(void)
+{
+	unsigned long timeout = 0;
+	uint32_t hmc_clk;
+
+	do {
+		hmc_clk = mmio_read_32(AGX_SYSMGR_CORE_HMC_CLK);
+		if (hmc_clk & AGX_SYSMGR_CORE_HMC_CLK_STATUS)
+			break;
+		udelay(1);
+	} while (++timeout < 1000);
+	if (timeout >= 1000)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int clear_emif(void)
+{
+	uint32_t data;
+	unsigned long timeout;
+
+	mmio_write_32(AGX_MPFE_HMC_ADP_RSTHANDSHAKECTRL, 0);
+
+	timeout = 0;
+	do {
+		data = mmio_read_32(AGX_MPFE_HMC_ADP_RSTHANDSHAKESTAT);
+		if ((data & AGX_MPFE_HMC_ADP_RSTHANDSHAKESTAT_SEQ2CORE) == 0)
+			break;
+		udelay(CLEAR_EMIF_DELAY);
+	} while (++timeout < CLEAR_EMIF_TIMEOUT);
+	if (timeout >= CLEAR_EMIF_TIMEOUT)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int mem_calibration(void)
+{
+	int status;
+	uint32_t data;
+	unsigned long timeout;
+	unsigned long retry = 0;
+
+	udelay(PRE_CALIBRATION_DELAY);
+
+	do {
+		if (retry != 0)
+			INFO("DDR: Retrying DRAM calibration\n");
+
+		timeout = 0;
+		do {
+			data = mmio_read_32(AGX_MPFE_HMC_ADP_DDRCALSTAT);
+			if (AGX_MPFE_HMC_ADP_DDRCALSTAT_CAL(data) == 1)
+				break;
+			udelay(500);
+		} while (++timeout < TIMEOUT_EMIF_CALIBRATION);
+
+		if (AGX_MPFE_HMC_ADP_DDRCALSTAT_CAL(data) == 0) {
+			status = clear_emif();
+			if (status)
+				ERROR("Failed to clear Emif\n");
+		} else {
+			break;
+		}
+	} while (++retry < MAX_MEM_CAL_RETRY);
+
+	if (AGX_MPFE_HMC_ADP_DDRCALSTAT_CAL(data) == 0) {
+		ERROR("DDR: DRAM calibration failed.\n");
+		status = -EIO;
+	} else {
+		INFO("DDR: DRAM calibration success.\n");
+		status = 0;
+	}
+
+	udelay(POST_CALIBRATION_DELAY);
+
+	return status;
+}
+
+int init_hard_memory_controller(void)
+{
+	int status;
+
+	status = check_hmc_clk();
+	if (status) {
+		ERROR("DDR: Error, HMC clock not running\n");
+		return status;
+	}
+
+	status = mem_calibration();
+	if (status) {
+		ERROR("DDR: Memory Calibration Failed\n");
+		return status;
+	}
+
+	configure_hmc_adaptor_regs();
+
+	return 0;
+}
+
+void configure_ddr_sched_ctrl_regs(void)
+{
+	uint32_t data, dram_addr_order, ddr_conf, bank, row, col,
+		rd_to_miss, wr_to_miss, burst_len, burst_len_ddr_clk,
+		burst_len_sched_clk, act_to_act, rd_to_wr, wr_to_rd, bw_ratio,
+		t_rtp, t_rp, t_rcd, rd_latency, tw_rin_clk_cycles,
+		bw_ratio_extended, auto_precharge = 0, act_to_act_bank, faw,
+		faw_bank, bus_rd_to_rd, bus_rd_to_wr, bus_wr_to_rd;
+
+	INFO("Init HPS NOC's DDR Scheduler.\n");
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CTRLCFG1);
+	dram_addr_order = AGX_MPFE_IOHMC_CTRLCFG1_CFG_ADDR_ORDER(data);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_DRAMADDRW);
+
+	col  = IOHMC_DRAMADDRW_COL_ADDR_WIDTH(data);
+	row  = IOHMC_DRAMADDRW_ROW_ADDR_WIDTH(data);
+	bank = IOHMC_DRAMADDRW_BANK_ADDR_WIDTH(data) +
+		IOHMC_DRAMADDRW_BANK_GRP_ADDR_WIDTH(data);
+
+	ddr_conf = match_ddr_conf(DDR_CONFIG(dram_addr_order, bank, col, row));
+
+	if (ddr_conf) {
+		mmio_clrsetbits_32(
+			AGX_MPFE_DDR_MAIN_SCHED_DDRCONF,
+			AGX_MPFE_DDR_MAIN_SCHED_DDRCONF_SET_MSK,
+			AGX_MPFE_DDR_MAIN_SCHED_DDRCONF_SET(ddr_conf));
+	} else {
+		ERROR("DDR: Cannot find predefined ddrConf configuration.\n");
+	}
+
+	mmio_write_32(AGX_MPFE_HMC_ADP(ADP_DRAMADDRWIDTH), data);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_DRAMTIMING0);
+	rd_latency = AGX_MPFE_IOHMC_REG_DRAMTIMING0_CFG_TCL(data);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CALTIMING0);
+	act_to_act = ACT_TO_ACT(data);
+	t_rcd = ACT_TO_RDWR(data);
+	act_to_act_bank = ACT_TO_ACT_DIFF_BANK(data);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CALTIMING1);
+	rd_to_wr = RD_TO_WR(data);
+	bus_rd_to_rd = RD_TO_RD_DIFF_CHIP(data);
+	bus_rd_to_wr = RD_TO_WR_DIFF_CHIP(data);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CALTIMING2);
+	t_rtp = RD_TO_PCH(data);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CALTIMING3);
+	wr_to_rd = CALTIMING3_WR_TO_RD(data);
+	bus_wr_to_rd = CALTIMING3_WR_TO_RD_DIFF_CHIP(data);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CALTIMING4);
+	t_rp = PCH_TO_VALID(data);
+
+	data = mmio_read_32(AGX_MPFE_HMC_ADP(HMC_ADP_DDRIOCTRL));
+	bw_ratio = ((HMC_ADP_DDRIOCTRL_IO_SIZE(data) == 0) ? 0 : 1);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CTRLCFG0);
+	burst_len = HMC_ADP_DDRIOCTRL_CTRL_BURST_LENGTH(data);
+	burst_len_ddr_clk = burst_len / 2;
+	burst_len_sched_clk = ((burst_len/2) / 2);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CTRLCFG0);
+	switch (AGX_MPFE_IOHMC_REG_CTRLCFG0_CFG_MEM_TYPE(data)) {
+	case 1:
+		/* DDR4 - 1333MHz */
+		/* 20 (19.995) clock cycles = 15ns */
+		/* Calculate with rounding */
+		tw_rin_clk_cycles = (((tWR_IN_NS * 1333) % 1000) >= 500) ?
+			((tWR_IN_NS * 1333) / 1000) + 1 :
+			((tWR_IN_NS * 1333) / 1000);
+		break;
+	default:
+		/* Others - 1066MHz or slower */
+		/* 16 (15.990) clock cycles = 15ns */
+		/* Calculate with rounding */
+		tw_rin_clk_cycles = (((tWR_IN_NS * 1066) % 1000) >= 500) ?
+			((tWR_IN_NS * 1066) / 1000) + 1 :
+			((tWR_IN_NS * 1066) / 1000);
+		break;
+	}
+
+	rd_to_miss = t_rtp + t_rp + t_rcd - burst_len_sched_clk;
+	wr_to_miss = ((rd_latency + burst_len_ddr_clk + 2 + tw_rin_clk_cycles)
+			/ 2) - rd_to_wr + t_rp + t_rcd;
+
+	mmio_write_32(AGX_MPFE_DDR_MAIN_SCHED_DDRTIMING,
+		bw_ratio << DDRTIMING_BWRATIO_OFST |
+		wr_to_rd << DDRTIMING_WRTORD_OFST|
+		rd_to_wr << DDRTIMING_RDTOWR_OFST |
+		burst_len_sched_clk << DDRTIMING_BURSTLEN_OFST |
+		wr_to_miss << DDRTIMING_WRTOMISS_OFST |
+		rd_to_miss << DDRTIMING_RDTOMISS_OFST |
+		act_to_act << DDRTIMING_ACTTOACT_OFST);
+
+	data = mmio_read_32(AGX_MPFE_HMC_ADP(HMC_ADP_DDRIOCTRL));
+	bw_ratio_extended = ((ADP_DDRIOCTRL_IO_SIZE(data) == 0) ? 1 : 0);
+
+	mmio_write_32(AGX_MPFE_DDR_MAIN_SCHED_DDRMODE,
+		bw_ratio_extended << DDRMODE_BWRATIOEXTENDED_OFST |
+		auto_precharge << DDRMODE_AUTOPRECHARGE_OFST);
+
+	mmio_write_32(AGX_MPFE_DDR_MAIN_SCHED_READLATENCY,
+		(rd_latency / 2) + DDR_READ_LATENCY_DELAY);
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CALTIMING9);
+	faw = AGX_MPFE_IOHMC_CALTIMING9_ACT_TO_ACT(data);
+
+	faw_bank = 1; // always 1 because we always have 4 bank DDR.
+
+	mmio_write_32(AGX_MPFE_DDR_MAIN_SCHED_ACTIVATE,
+		faw_bank << AGX_MPFE_DDR_MAIN_SCHED_ACTIVATE_FAWBANK_OFST |
+		faw << AGX_MPFE_DDR_MAIN_SCHED_ACTIVATE_FAW_OFST |
+		act_to_act_bank << AGX_MPFE_DDR_MAIN_SCHED_ACTIVATE_RRD_OFST);
+
+	mmio_write_32(AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV,
+		((bus_rd_to_rd
+			<< AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSRDTORD_OFST)
+			& AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSRDTORD_MSK) |
+		((bus_rd_to_wr
+			<< AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSRDTOWR_OFST)
+			& AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSRDTOWR_MSK) |
+		((bus_wr_to_rd
+			<< AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSWRTORD_OFST)
+			& AGX_MPFE_DDR_MAIN_SCHED_DEVTODEV_BUSWRTORD_MSK));
+
+}
+
+unsigned long get_physical_dram_size(void)
+{
+	uint32_t data;
+	unsigned long ram_addr_width, ram_ext_if_io_width;
+
+	data = mmio_read_32(AGX_MPFE_HMC_ADP_DDRIOCTRL);
+	switch (AGX_MPFE_HMC_ADP_DDRIOCTRL_IO_SIZE(data)) {
+	case 0:
+		ram_ext_if_io_width = 16;
+		break;
+	case 1:
+		ram_ext_if_io_width = 32;
+		break;
+	case 2:
+		ram_ext_if_io_width = 64;
+		break;
+	default:
+		ram_ext_if_io_width = 0;
+		break;
+	}
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_REG_DRAMADDRW);
+	ram_addr_width = IOHMC_DRAMADDRW_CFG_COL_ADDR_WIDTH(data) +
+		IOHMC_DRAMADDRW_CFG_ROW_ADDR_WIDTH(data) +
+		IOHMC_DRAMADDRW_CFG_BANK_ADDR_WIDTH(data) +
+		IOHMC_DRAMADDRW_CFG_BANK_GROUP_ADDR_WIDTH(data) +
+		IOHMC_DRAMADDRW_CFG_CS_ADDR_WIDTH(data);
+
+	return (1 << ram_addr_width) * (ram_ext_if_io_width / 8);
+}
+
+
+
+void configure_hmc_adaptor_regs(void)
+{
+	uint32_t data;
+	uint32_t dram_io_width;
+
+	/* Configure DDR data rate */
+	dram_io_width = AGX_MPFE_IOHMC_NIOSRESERVE0_NIOS_RESERVE0(
+		mmio_read_32(AGX_MPFE_IOHMC_REG_NIOSRESERVE0_OFST));
+	dram_io_width = (dram_io_width & 0xFF) >> 5;
+
+	data = mmio_read_32(AGX_MPFE_IOHMC_CTRLCFG3);
+
+	dram_io_width |= (data & 0x4);
+
+	mmio_write_32(AGX_MPFE_HMC_ADP_DDRIOCTRL, dram_io_width);
+
+	/* Copy dram addr width from IOHMC to HMC ADP */
+	data = mmio_read_32(AGX_MPFE_IOHMC_DRAMADDRW);
+	mmio_write_32(AGX_MPFE_HMC_ADP(ADP_DRAMADDRWIDTH), data);
+
+	/* Enable nonsecure access to DDR */
+	data = get_physical_dram_size();
+
+	if (data < AGX_DDR_SIZE)
+		data = AGX_DDR_SIZE;
+
+	mmio_write_32(AGX_NOC_FW_DDR_SCR_MPUREGION0ADDR_LIMIT, data - 1);
+	mmio_write_32(AGX_NOC_FW_DDR_SCR_MPUREGION0ADDR_LIMITEXT, 0x1f);
+
+	mmio_write_32(AGX_NOC_FW_DDR_SCR_NONMPUREGION0ADDR_LIMIT, data - 1);
+
+	mmio_write_32(AGX_SOC_NOC_FW_DDR_SCR_ENABLESET, BIT(0) | BIT(8));
+
+	/* ECC enablement */
+	data = mmio_read_32(AGX_MPFE_IOHMC_REG_CTRLCFG1);
+	if (data & (1 << AGX_IOHMC_CTRLCFG1_ENABLE_ECC_OFST)) {
+		mmio_clrsetbits_32(AGX_MPFE_HMC_ADP_ECCCTRL1,
+			AGX_MPFE_HMC_ADP_ECCCTRL1_AUTOWB_CNT_RST_SET_MSK |
+			AGX_MPFE_HMC_ADP_ECCCTRL1_CNT_RST_SET_MSK |
+			AGX_MPFE_HMC_ADP_ECCCTRL1_ECC_EN_SET_MSK,
+			AGX_MPFE_HMC_ADP_ECCCTRL1_AUTOWB_CNT_RST_SET_MSK |
+			AGX_MPFE_HMC_ADP_ECCCTRL1_CNT_RST_SET_MSK);
+
+		mmio_clrsetbits_32(AGX_MPFE_HMC_ADP_ECCCTRL2,
+			AGX_MPFE_HMC_ADP_ECCCTRL2_OVRW_RB_ECC_EN_SET_MSK |
+			AGX_MPFE_HMC_ADP_ECCCTRL2_RMW_EN_SET_MSK |
+			AGX_MPFE_HMC_ADP_ECCCTRL2_AUTOWB_EN_SET_MSK,
+			AGX_MPFE_HMC_ADP_ECCCTRL2_RMW_EN_SET_MSK |
+			AGX_MPFE_HMC_ADP_ECCCTRL2_AUTOWB_EN_SET_MSK);
+
+		mmio_clrsetbits_32(AGX_MPFE_HMC_ADP_ECCCTRL1,
+			AGX_MPFE_HMC_ADP_ECCCTRL1_AUTOWB_CNT_RST_SET_MSK |
+			AGX_MPFE_HMC_ADP_ECCCTRL1_CNT_RST_SET_MSK |
+			AGX_MPFE_HMC_ADP_ECCCTRL1_ECC_EN_SET_MSK,
+			AGX_MPFE_HMC_ADP_ECCCTRL1_ECC_EN_SET_MSK);
+		INFO("Scrubbing ECC\n");
+
+		/* ECC Scrubbing */
+		zeromem((void *)DRAM_BASE, DRAM_SIZE);
+	} else {
+		INFO("ECC is disabled.\n");
+	}
+}
diff --git a/plat/intel/soc/agilex5/soc/agilex5_mmc.c b/plat/intel/soc/agilex5/soc/agilex5_mmc.c
new file mode 100644
index 0000000..48f7341
--- /dev/null
+++ b/plat/intel/soc/agilex5/soc/agilex5_mmc.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2020-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <lib/mmio.h>
+
+#include "agilex5_clock_manager.h"
+#include "agilex5_system_manager.h"
+
+void agx5_mmc_init(void)
+{
+// TODO: To update when handoff data is ready
+
+	//mmio_clrbits_32(CLKMGR_PERPLL + CLKMGR_PERPLL_EN,
+	//	CLKMGR_PERPLL_EN_SDMMCCLK);
+	//mmio_write_32(SOCFPGA_SYSMGR(SDMMC),
+	//	SYSMGR_SDMMC_SMPLSEL(0) | SYSMGR_SDMMC_DRVSEL(3));
+	//mmio_setbits_32(CLKMGR_PERPLL + CLKMGR_PERPLL_EN,
+	//	CLKMGR_PERPLL_EN_SDMMCCLK);
+
+}
diff --git a/plat/intel/soc/agilex5/soc/agilex5_pinmux.c b/plat/intel/soc/agilex5/soc/agilex5_pinmux.c
new file mode 100644
index 0000000..50d9e36
--- /dev/null
+++ b/plat/intel/soc/agilex5/soc/agilex5_pinmux.c
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+
+#include "agilex5_pinmux.h"
+#include "agilex5_system_manager.h"
+
+const uint32_t sysmgr_pinmux_array_sel[] = {
+	0x00000000, 0x00000001, /* usb */
+	0x00000004, 0x00000001,
+	0x00000008, 0x00000001,
+	0x0000000c, 0x00000001,
+	0x00000010, 0x00000001,
+	0x00000014, 0x00000001,
+	0x00000018, 0x00000001,
+	0x0000001c, 0x00000001,
+	0x00000020, 0x00000001,
+	0x00000024, 0x00000001,
+	0x00000028, 0x00000001,
+	0x0000002c, 0x00000001,
+	0x00000030, 0x00000000, /* emac0 */
+	0x00000034, 0x00000000,
+	0x00000038, 0x00000000,
+	0x0000003c, 0x00000000,
+	0x00000040, 0x00000000,
+	0x00000044, 0x00000000,
+	0x00000048, 0x00000000,
+	0x0000004c, 0x00000000,
+	0x00000050, 0x00000000,
+	0x00000054, 0x00000000,
+	0x00000058, 0x00000000,
+	0x0000005c, 0x00000000,
+	0x00000060, 0x00000008, /* gpio1 */
+	0x00000064, 0x00000008,
+	0x00000068, 0x00000005, /* uart0 tx */
+	0x0000006c, 0x00000005, /* uart 0 rx */
+	0x00000070, 0x00000008, /* gpio */
+	0x00000074, 0x00000008,
+	0x00000078, 0x00000004, /* i2c1 */
+	0x0000007c, 0x00000004,
+	0x00000080, 0x00000007, /* jtag */
+	0x00000084, 0x00000007,
+	0x00000088, 0x00000007,
+	0x0000008c, 0x00000007,
+	0x00000090, 0x00000001, /* sdmmc data0 */
+	0x00000094, 0x00000001,
+	0x00000098, 0x00000001,
+	0x0000009c, 0x00000001,
+	0x00000100, 0x00000001,
+	0x00000104, 0x00000001, /* sdmmc.data3 */
+	0x00000108, 0x00000008, /* loan */
+	0x0000010c, 0x00000008, /* gpio */
+	0x00000110, 0x00000008,
+	0x00000114, 0x00000008, /* gpio1.io21 */
+	0x00000118, 0x00000005, /* mdio0.mdio */
+	0x0000011c, 0x00000005  /* mdio0.mdc */
+};
+
+const uint32_t sysmgr_pinmux_array_ctrl[] = {
+	0x00000000, 0x00502c38, /* Q1_1 */
+	0x00000004, 0x00102c38,
+	0x00000008, 0x00502c38,
+	0x0000000c, 0x00502c38,
+	0x00000010, 0x00502c38,
+	0x00000014, 0x00502c38,
+	0x00000018, 0x00502c38,
+	0x0000001c, 0x00502c38,
+	0x00000020, 0x00502c38,
+	0x00000024, 0x00502c38,
+	0x00000028, 0x00502c38,
+	0x0000002c, 0x00502c38,
+	0x00000030, 0x00102c38, /* Q2_1 */
+	0x00000034, 0x00102c38,
+	0x00000038, 0x00502c38,
+	0x0000003c, 0x00502c38,
+	0x00000040, 0x00102c38,
+	0x00000044, 0x00102c38,
+	0x00000048, 0x00502c38,
+	0x0000004c, 0x00502c38,
+	0x00000050, 0x00102c38,
+	0x00000054, 0x00102c38,
+	0x00000058, 0x00502c38,
+	0x0000005c, 0x00502c38,
+	0x00000060, 0x00502c38, /* Q3_1 */
+	0x00000064, 0x00502c38,
+	0x00000068, 0x00102c38,
+	0x0000006c, 0x00502c38,
+	0x000000d0, 0x00502c38,
+	0x000000d4, 0x00502c38,
+	0x000000d8, 0x00542c38,
+	0x000000dc, 0x00542c38,
+	0x000000e0, 0x00502c38,
+	0x000000e4, 0x00502c38,
+	0x000000e8, 0x00102c38,
+	0x000000ec, 0x00502c38,
+	0x000000f0, 0x00502c38, /* Q4_1 */
+	0x000000f4, 0x00502c38,
+	0x000000f8, 0x00102c38,
+	0x000000fc, 0x00502c38,
+	0x00000100, 0x00502c38,
+	0x00000104, 0x00502c38,
+	0x00000108, 0x00102c38,
+	0x0000010c, 0x00502c38,
+	0x00000110, 0x00502c38,
+	0x00000114, 0x00502c38,
+	0x00000118, 0x00542c38,
+	0x0000011c, 0x00102c38
+};
+
+const uint32_t sysmgr_pinmux_array_fpga[] = {
+	0x00000000, 0x00000000,
+	0x00000004, 0x00000000,
+	0x00000008, 0x00000000,
+	0x0000000c, 0x00000000,
+	0x00000010, 0x00000000,
+	0x00000014, 0x00000000,
+	0x00000018, 0x00000000,
+	0x0000001c, 0x00000000,
+	0x00000020, 0x00000000,
+	0x00000028, 0x00000000,
+	0x0000002c, 0x00000000,
+	0x00000030, 0x00000000,
+	0x00000034, 0x00000000,
+	0x00000038, 0x00000000,
+	0x0000003c, 0x00000000,
+	0x00000040, 0x00000000,
+	0x00000044, 0x00000000,
+	0x00000048, 0x00000000,
+	0x00000050, 0x00000000,
+	0x00000054, 0x00000000,
+	0x00000058, 0x0000002a
+};
+
+const uint32_t sysmgr_pinmux_array_iodelay[] = {
+	0x00000000, 0x00000000,
+	0x00000004, 0x00000000,
+	0x00000008, 0x00000000,
+	0x0000000c, 0x00000000,
+	0x00000010, 0x00000000,
+	0x00000014, 0x00000000,
+	0x00000018, 0x00000000,
+	0x0000001c, 0x00000000,
+	0x00000020, 0x00000000,
+	0x00000024, 0x00000000,
+	0x00000028, 0x00000000,
+	0x0000002c, 0x00000000,
+	0x00000030, 0x00000000,
+	0x00000034, 0x00000000,
+	0x00000038, 0x00000000,
+	0x0000003c, 0x00000000,
+	0x00000040, 0x00000000,
+	0x00000044, 0x00000000,
+	0x00000048, 0x00000000,
+	0x0000004c, 0x00000000,
+	0x00000050, 0x00000000,
+	0x00000054, 0x00000000,
+	0x00000058, 0x00000000,
+	0x0000005c, 0x00000000,
+	0x00000060, 0x00000000,
+	0x00000064, 0x00000000,
+	0x00000068, 0x00000000,
+	0x0000006c, 0x00000000,
+	0x00000070, 0x00000000,
+	0x00000074, 0x00000000,
+	0x00000078, 0x00000000,
+	0x0000007c, 0x00000000,
+	0x00000080, 0x00000000,
+	0x00000084, 0x00000000,
+	0x00000088, 0x00000000,
+	0x0000008c, 0x00000000,
+	0x00000090, 0x00000000,
+	0x00000094, 0x00000000,
+	0x00000098, 0x00000000,
+	0x0000009c, 0x00000000,
+	0x00000100, 0x00000000,
+	0x00000104, 0x00000000,
+	0x00000108, 0x00000000,
+	0x0000010c, 0x00000000,
+	0x00000110, 0x00000000,
+	0x00000114, 0x00000000,
+	0x00000118, 0x00000000,
+	0x0000011c, 0x00000000
+};
+
+void config_fpgaintf_mod(void)
+{
+	mmio_write_32(SOCFPGA_SYSMGR(FPGAINTF_EN_2), 1<<8);
+}
+
+void config_pinmux(handoff *hoff_ptr)
+{
+	unsigned int i;
+
+	mmio_write_32(PINMUX_HANDOFF_CONFIG_ADDR, PINMUX_HANDOFF_CONFIG_VAL);
+	for (i = 0; i < PINMUX_HANDOFF_ARRAY_SIZE(hoff_ptr->pinmux_sel_array); i += 2) {
+		mmio_write_32(AGX5_PINMUX_PIN0SEL +
+			hoff_ptr->pinmux_sel_array[i],
+			hoff_ptr->pinmux_sel_array[i + 1]);
+	}
+
+	config_fpgaintf_mod();
+}
+
+void config_peripheral(handoff *hoff_ptr)
+{
+
+	// TODO: This need to be update due to peripheral_pwr_gate_array handoff change
+	// Pending SDM to pass over handoff data
+	// unsigned int i;
+
+	// for (i = 0; i < 4; i += 2) {
+	//	mmio_write_32(AGX_EDGE_PERIPHERAL +
+	//	hoff_ptr->peripheral_pwr_gate_array[i],
+	//	hoff_ptr->peripheral_pwr_gate_array[i+1]);
+	// }
+
+
+	// TODO: This need to be update due to peripheral_pwr_gate_array handoff change
+	mmio_write_32(AGX5_PERIPHERAL,
+	hoff_ptr->peripheral_pwr_gate_array);
+}
diff --git a/plat/intel/soc/agilex5/soc/agilex5_power_manager.c b/plat/intel/soc/agilex5/soc/agilex5_power_manager.c
new file mode 100644
index 0000000..0d81970
--- /dev/null
+++ b/plat/intel/soc/agilex5/soc/agilex5_power_manager.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#include "agilex5_power_manager.h"
+#include "socfpga_reset_manager.h"
+
+int wait_verify_fsm(uint16_t timeout, uint32_t peripheral_handoff)
+{
+	uint32_t data = 0;
+	uint32_t count = 0;
+	uint32_t pgstat = 0;
+
+	/* Wait FSM ready */
+	do {
+		data = mmio_read_32(AGX5_PWRMGR(PSS_PGSTAT));
+		count++;
+		if (count >= 1000) {
+			return -ETIMEDOUT;
+		}
+
+	} while (AGX5_PWRMGR_PSS_STAT_BUSY(data) == AGX5_PWRMGR_PSS_STAT_BUSY_E_BUSY);
+
+	/* Verify PSS SRAM power gated */
+	pgstat = mmio_read_32(AGX5_PWRMGR(PSS_PGSTAT));
+	if (pgstat != (AGX5_PWRMGR_PSS_PGEN_OUT(peripheral_handoff))) {
+		return AGX5_PWRMGR_HANDOFF_PERIPHERAL;
+	}
+
+	return 0;
+}
+
+int pss_sram_power_off(handoff *hoff_ptr)
+{
+	int ret = 0;
+	uint32_t peripheral_handoff = 0;
+
+	/* Get PSS SRAM handoff data */
+	peripheral_handoff = hoff_ptr->peripheral_pwr_gate_array;
+
+	/* Enable firewall for PSS SRAM */
+	mmio_write_32(AGX5_PWRMGR(PSS_FWENCTL),
+			AGX5_PWRMGR_PSS_FWEN(peripheral_handoff));
+
+	/* Wait */
+	udelay(1);
+
+	/* Power gating PSS SRAM */
+	mmio_write_32(AGX5_PWRMGR(PSS_PGENCTL),
+			AGX5_PWRMGR_PSS_PGEN(peripheral_handoff));
+
+	ret = wait_verify_fsm(1000, peripheral_handoff);
+
+	return ret;
+}
+
+void config_pwrmgr_handoff(handoff *hoff_ptr)
+{
+	int ret = 0;
+
+	switch (hoff_ptr->header_magic) {
+	case HANDOFF_MAGIC_PERIPHERAL:
+		ret = pss_sram_power_off(hoff_ptr);
+		break;
+	default:
+		break;
+	}
+
+	if (ret != 0) {
+		ERROR("Config PwrMgr handoff failed. error %d\n", ret);
+		assert(false);
+	}
+}
diff --git a/plat/intel/soc/common/aarch64/plat_helpers.S b/plat/intel/soc/common/aarch64/plat_helpers.S
index 6bf2d82..cbd0121 100644
--- a/plat/intel/soc/common/aarch64/plat_helpers.S
+++ b/plat/intel/soc/common/aarch64/plat_helpers.S
@@ -34,6 +34,11 @@
 func plat_secondary_cold_boot_setup
 	/* Wait until the it gets reset signal from rstmgr gets populated */
 poll_mailbox:
+#if	PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	mov_imm x0, PLAT_SEC_ENTRY
+	cbz	x0, poll_mailbox
+	br      x0
+#else
 	wfi
 	mov_imm	x0, PLAT_SEC_ENTRY
 	ldr	x1, [x0]
@@ -44,8 +49,13 @@
 	cmp	x3, x4
 	b.ne	poll_mailbox
 	br	x1
+#endif
 endfunc plat_secondary_cold_boot_setup
 
+#if	((PLATFORM_MODEL == PLAT_SOCFPGA_STRATIX10) || \
+	(PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX) || \
+	(PLATFORM_MODEL == PLAT_SOCFPGA_N5X))
+
 func platform_is_primary_cpu
 	and	x0, x0, #(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)
 	cmp	x0, #PLAT_PRIMARY_CPU
@@ -53,6 +63,21 @@
 	ret
 endfunc platform_is_primary_cpu
 
+#else
+
+func platform_is_primary_cpu
+	and	x0, x0, #(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)
+	cmp x0, #(PLAT_PRIMARY_CPU_A76)
+	b.eq primary_cpu
+	cmp x0, #(PLAT_PRIMARY_CPU_A55)
+	b.eq primary_cpu
+primary_cpu:
+	cset	x0, eq
+	ret
+endfunc platform_is_primary_cpu
+
+#endif
+
 func plat_is_my_cpu_primary
 	mrs	x0, mpidr_el1
 	b   platform_is_primary_cpu
@@ -62,11 +87,27 @@
 	mrs	x0, mpidr_el1
 	and	x1, x0, #MPIDR_CPU_MASK
 	and	x0, x0, #MPIDR_CLUSTER_MASK
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	add	x0, x1, x0, LSR #8
+#else
 	add	x0, x1, x0, LSR #6
+#endif
 	ret
 endfunc plat_my_core_pos
 
 func warm_reset_req
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	bl	plat_is_my_cpu_primary
+	cbnz	x0, warm_reset
+warm_reset:
+	mov_imm x1, PLAT_SEC_ENTRY
+	str	xzr, [x1]
+	mrs	x1, rmr_el3
+	orr	x1, x1, #0x02
+	msr	rmr_el3, x1
+	isb
+	dsb	sy
+#else
 	str	xzr, [x4]
 	bl	plat_is_my_cpu_primary
 	cbz	x0, cpu_in_wfi
@@ -80,11 +121,28 @@
 cpu_in_wfi:
 	wfi
 	b	cpu_in_wfi
+#endif
 endfunc warm_reset_req
 
+/* TODO: Zephyr warm reset test */
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
 func plat_get_my_entrypoint
 	ldr	x4, =L2_RESET_DONE_REG
 	ldr	x5, [x4]
+	ldr	x1, =PLAT_L2_RESET_REQ
+	cmp	x1, x5
+	b.eq	zephyr_reset_req
+	mov_imm	x1, PLAT_SEC_ENTRY
+	ldr	x0, [x1]
+	ret
+zephyr_reset_req:
+	ldr	x0, =0x00
+	ret
+endfunc plat_get_my_entrypoint
+#else
+func plat_get_my_entrypoint
+	ldr	x4, =L2_RESET_DONE_REG
+	ldr	x5, [x4]
 	ldr	x1, =L2_RESET_DONE_STATUS
 	cmp	x1, x5
 	b.eq	warm_reset_req
@@ -92,7 +150,7 @@
 	ldr	x0, [x1]
 	ret
 endfunc plat_get_my_entrypoint
-
+#endif
 
 	/* ---------------------------------------------
 	 * int plat_crash_console_init(void)
@@ -138,6 +196,13 @@
 	ret
 endfunc platform_mem_init
 
+	/* --------------------------------------------------------
+	 * macro plat_secondary_cpus_bl31_entry;
+	 *
+	 * el3_entrypoint_common init param configuration.
+	 * Called very early in the secondary cores boot process.
+	 * --------------------------------------------------------
+	 */
 func plat_secondary_cpus_bl31_entry
 	el3_entrypoint_common                                   \
 		_init_sctlr=0                                   \
diff --git a/plat/intel/soc/common/drivers/ccu/ncore_ccu.c b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
index 38f8b94..684a625 100644
--- a/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
+++ b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
@@ -3,7 +3,6 @@
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
-
 #include <assert.h>
 #include <common/debug.h>
 #include <errno.h>
@@ -16,89 +15,72 @@
 
 uint32_t poll_active_bit(uint32_t dir);
 
-static coh_ss_id_t subsystem_id;
+#define SMMU_DMI			1
 
 
+static coh_ss_id_t subsystem_id;
 void get_subsystem_id(void)
 {
 	uint32_t snoop_filter, directory, coh_agent;
-
 	snoop_filter = CSIDR_NUM_SF(mmio_read_32(NCORE_CCU_CSR(NCORE_CSIDR)));
 	directory = CSUIDR_NUM_DIR(mmio_read_32(NCORE_CCU_CSR(NCORE_CSUIDR)));
 	coh_agent = CSUIDR_NUM_CAI(mmio_read_32(NCORE_CCU_CSR(NCORE_CSUIDR)));
-
 	subsystem_id.num_snoop_filter = snoop_filter + 1;
 	subsystem_id.num_directory = directory;
 	subsystem_id.num_coh_agent = coh_agent;
 }
-
 uint32_t directory_init(void)
 {
 	uint32_t dir_sf_mtn, dir_sf_en;
 	uint32_t dir, sf, ret;
-
 	for (dir = 0; dir < subsystem_id.num_directory; dir++) {
 		for (sf = 0; sf < subsystem_id.num_snoop_filter; sf++) {
 			dir_sf_mtn = DIRECTORY_UNIT(dir, NCORE_DIRUSFMCR);
 			dir_sf_en = DIRECTORY_UNIT(dir, NCORE_DIRUSFER);
-
 			/* Initialize All Entries */
 			mmio_write_32(dir_sf_mtn, SNOOP_FILTER_ID(sf));
-
 			/* Poll Active Bit */
 			ret = poll_active_bit(dir);
 			if (ret != 0) {
 				ERROR("Timeout during active bit polling");
 				return -ETIMEDOUT;
 			}
-
 			/* Disable snoop filter, a bit per snoop filter */
 			mmio_clrbits_32(dir_sf_en, BIT(sf));
-
 		}
 	}
-
 	return 0;
 }
-
 uint32_t coherent_agent_intfc_init(void)
 {
 	uint32_t dir, ca, ca_id, ca_type, ca_snoop_en;
-
 	for (dir = 0; dir < subsystem_id.num_directory; dir++) {
 		for (ca = 0; ca < subsystem_id.num_coh_agent; ca++) {
 			ca_snoop_en = DIRECTORY_UNIT(ca, NCORE_DIRUCASER0);
 			ca_id = mmio_read_32(COH_AGENT_UNIT(ca, NCORE_CAIUIDR));
-
 			/* Coh Agent Snoop Enable */
 			if (CACHING_AGENT_BIT(ca_id))
-				mmio_write_32(ca_snoop_en, BIT(ca));
-
+				mmio_setbits_32(ca_snoop_en, BIT(ca));
 			/* Coh Agent Snoop DVM Enable */
 			ca_type = CACHING_AGENT_TYPE(ca_id);
 			if (ca_type == ACE_W_DVM || ca_type == ACE_L_W_DVM)
-				mmio_write_32(NCORE_CCU_CSR(NCORE_CSADSER0),
+				mmio_setbits_32(NCORE_CCU_CSR(NCORE_CSADSER0),
 				BIT(ca));
 		}
 	}
-
 	return 0;
 }
-
 uint32_t poll_active_bit(uint32_t dir)
 {
 	uint32_t timeout = 80000;
 	uint32_t poll_dir =  DIRECTORY_UNIT(dir, NCORE_DIRUSFMAR);
-
 	while (timeout > 0) {
 		if (mmio_read_32(poll_dir) == 0)
 			return 0;
 		timeout--;
 	}
-
 	return -1;
 }
-
 void bypass_ocram_firewall(void)
 {
 	mmio_clrbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF1),
@@ -110,7 +92,6 @@
 	mmio_clrbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF4),
 			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
 }
-
 void ncore_enable_ocram_firewall(void)
 {
 	mmio_setbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF1),
@@ -122,15 +103,44 @@
 	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;
-
 	get_subsystem_id();
 	status = directory_init();
 	status = coherent_agent_intfc_init();
 	bypass_ocram_firewall();
-
 	return status;
 }
+
+void setup_smmu_stream_id(void)
+{
+	/* Configure Stream ID for Agilex5 */
+	mmio_write_32(SOCFPGA_SYSMGR(DMA_TBU_STREAM_ID_AX_REG_0_DMA0), DMA0);
+	mmio_write_32(SOCFPGA_SYSMGR(DMA_TBU_STREAM_ID_AX_REG_0_DMA1), DMA1);
+	mmio_write_32(SOCFPGA_SYSMGR(SDM_TBU_STREAM_ID_AX_REG_1_SDM), SDM);
+	/* Reg map showing USB2 but Linux USB0? */
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_USB2), USB0);
+	/* Reg map showing USB3 but Linux USB1? */
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_USB3), USB1);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_SDMMC), SDMMC);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_NAND), NAND);
+	/* To confirm ETR - core sight debug*/
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_ETR), CORE_SIGHT_DEBUG);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_TSN0), TSN0);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_TSN1), TSN1);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_ID_AX_REG_2_TSN2), TSN2);
+
+	/* Enabled Stream ctrl register for Agilex5 */
+	mmio_write_32(SOCFPGA_SYSMGR(DMA_TBU_STREAM_CTRL_REG_0_DMA0), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(DMA_TBU_STREAM_CTRL_REG_0_DMA1), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(SDM_TBU_STREAM_CTRL_REG_1_SDM), ENABLE_STREAMID_SECURE_TX);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_USB2), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_USB3), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_SDMMC), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_NAND), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(IO_TBU_STREAM_CTRL_REG_2_ETR), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(TSN_TBU_STREAM_CTRL_REG_3_TSN0), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(TSN_TBU_STREAM_CTRL_REG_3_TSN1), ENABLE_STREAMID);
+	mmio_write_32(SOCFPGA_SYSMGR(TSN_TBU_STREAM_CTRL_REG_3_TSN2), ENABLE_STREAMID);
+}
diff --git a/plat/intel/soc/common/drivers/ccu/ncore_ccu.h b/plat/intel/soc/common/drivers/ccu/ncore_ccu.h
index 3f662ff..6cdbeb8 100644
--- a/plat/intel/soc/common/drivers/ccu/ncore_ccu.h
+++ b/plat/intel/soc/common/drivers/ccu/ncore_ccu.h
@@ -1,99 +1,420 @@
 /*
- * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
-
 #ifndef NCORE_CCU_H
 #define NCORE_CCU_H
 
+#include <stdbool.h>
+#include <stdint.h>
 
-#define NCORE_CCU_OFFSET		0xf7000000
+#ifndef CCU_ACTIVATE_COH_FPGA
+#define CCU_ACTIVATE_COH_FPGA 0
+#endif
+// Address map for ccu init
+#define addr_CAIUIDR1				(0x1C000000)
+#define addr_GRBUNRRUCR				(0x1c0ffff8)
+#define base_addr_NRS_CAIU0			(0x1c000000)
+#define base_addr_NRS_NCAIU0			(0x1c001000)
+#define base_addr_NRS_NCAIU1			(0x1c002000)
+#define base_addr_NRS_NCAIU2			(0x1c003000)
+#define base_addr_NRS_NCAIU3			(0x1c004000)
+#define base_addr_NRS_DCE0			(0x1c005000)
+#define base_addr_NRS_DCE1			(0x1c006000)
+//#define base_addr_NRS_DMI0			(0x1c007000)
+//#define base_addr_NRS_DMI1			(0x1c008000)
+//DMI
+#define ALT_CCU_CCU_DMI0_DMIUSMCTCR_ADDR	0x1C007300
+#define ALT_CCU_CCU_DMI1_DMIUSMCTCR_ADDR	0x1C008300
+//DSU
+#define ALT_CCU_DSU_CAIUAMIGR_ADDR		0x1C0003C0
+#define ALT_CCU_DSU_CAIUMIFSR_ADDR		0x1C0003C4
+#define ALT_CCU_DSU_CAIUGPRBLR1_ADDR		0x1C000414
+#define ALT_CCU_DSU_CAIUGPRBHR1_ADDR		0x1C000418
+#define ALT_CCU_DSU_CAIUGPRAR1_ADDR		0x1C000410
+#define ALT_CCU_DSU_CAIUGPRBLR2_ADDR		0x1C000424
+#define ALT_CCU_DSU_CAIUGPRBHR2_ADDR		0x1C000428
+#define ALT_CCU_DSU_CAIUGPRAR2_ADDR		0x1C000420
+#define ALT_CCU_DSU_CAIUGPRBLR4_ADDR		0x1C000444
+#define ALT_CCU_DSU_CAIUGPRBHR4_ADDR		0x1C000448
+#define ALT_CCU_DSU_CAIUGPRAR4_ADDR		0x1C000440
+#define ALT_CCU_DSU_CAIUGPRBLR5_ADDR		0x1C000454
+#define ALT_CCU_DSU_CAIUGPRBHR5_ADDR		0x1C000458
+#define ALT_CCU_DSU_CAIUGPRAR5_ADDR		0x1C000450
+#define ALT_CCU_DSU_CAIUGPRBLR6_ADDR		0x1C000464
+#define ALT_CCU_DSU_CAIUGPRBHR6_ADDR		0x1C000468
+#define ALT_CCU_DSU_CAIUGPRAR6_ADDR		0x1C000460
+#define ALT_CCU_DSU_CAIUGPRBLR7_ADDR		0x1C000474
+#define ALT_CCU_DSU_CAIUGPRBHR7_ADDR		0x1C000478
+#define ALT_CCU_DSU_CAIUGPRAR7_ADDR		0x1C000470
+#define ALT_CCU_DSU_CAIUGPRBLR8_ADDR		0x1C000484
+#define ALT_CCU_DSU_CAIUGPRBHR8_ADDR		0x1C000488
+#define ALT_CCU_DSU_CAIUGPRAR8_ADDR		0x1C000480
+#define ALT_CCU_DSU_CAIUGPRBLR9_ADDR		0x1C000494
+#define ALT_CCU_DSU_CAIUGPRBHR9_ADDR		0x1C000498
+#define ALT_CCU_DSU_CAIUGPRAR9_ADDR		0x1C000490
+#define ALT_CCU_DSU_CAIUGPRBLR10_ADDR		0x1C0004A4
+#define ALT_CCU_DSU_CAIUGPRBHR10_ADDR		0x1C0004A8
+#define ALT_CCU_DSU_CAIUGPRAR10_ADDR		0x1C0004A0
+//GIC
+#define ALT_CCU_GIC_M_XAIUAMIGR_ADDR		0x1C0023C0
+#define ALT_CCU_GIC_M_XAIUMIFSR_ADDR		0x1C0023C4
+#define ALT_CCU_GIC_M_XAIUGPRBLR1_ADDR		0x1C002414
+#define ALT_CCU_GIC_M_XAIUGPRBHR1_ADDR		0x1C002418
+#define ALT_CCU_GIC_M_XAIUGPRAR1_ADDR		0x1C002410
+#define ALT_CCU_GIC_M_XAIUGPRBLR6_ADDR		0x1C002464
+#define ALT_CCU_GIC_M_XAIUGPRBHR6_ADDR		0x1C002468
+#define ALT_CCU_GIC_M_XAIUGPRAR6_ADDR		0x1C002460
+#define ALT_CCU_GIC_M_XAIUGPRBLR8_ADDR		0x1C002484
+#define ALT_CCU_GIC_M_XAIUGPRBHR8_ADDR		0x1C002488
+#define ALT_CCU_GIC_M_XAIUGPRAR8_ADDR		0x1C002480
+#define ALT_CCU_GIC_M_XAIUGPRBLR10_ADDR		0x1C0024A4
+#define ALT_CCU_GIC_M_XAIUGPRBHR10_ADDR		0x1C0024A8
+#define ALT_CCU_GIC_M_XAIUGPRAR10_ADDR		0x1C0024A0
+//FPGA2SOC
+#define ALT_CCU_FPGA2SOC_XAIUAMIGR_ADDR		0x1C0013C0
+#define ALT_CCU_FPGA2SOC_XAIUMIFSR_ADDR		0x1C0013C4
+#define ALT_CCU_FPGA2SOC_XAIUGPRBLR1_ADDR	0x1C001414
+#define ALT_CCU_FPGA2SOC_XAIUGPRBHR1_ADDR	0x1C001418
+#define ALT_CCU_FPGA2SOC_XAIUGPRAR1_ADDR	0x1C001410
+#define ALT_CCU_FPGA2SOC_XAIUGPRBLR6_ADDR	0x1C001464
+#define ALT_CCU_FPGA2SOC_XAIUGPRBHR6_ADDR	0x1C001468
+#define ALT_CCU_FPGA2SOC_XAIUGPRAR6_ADDR	0x1C001460
+#define ALT_CCU_FPGA2SOC_XAIUGPRBLR8_ADDR	0x1C001484
+#define ALT_CCU_FPGA2SOC_XAIUGPRBHR8_ADDR	0x1C001488
+#define ALT_CCU_FPGA2SOC_XAIUGPRAR8_ADDR	0x1C001480
+#define ALT_CCU_FPGA2SOC_XAIUGPRBLR10_ADDR	0x1C0014A4
+#define ALT_CCU_FPGA2SOC_XAIUGPRBHR10_ADDR	0x1C0014A8
+#define ALT_CCU_FPGA2SOC_XAIUGPRAR10_ADDR	0x1C0014A0
+//TCU
+#define ALT_CCU_TCU_BASE 0x1C003000
+#define ALT_CCU_TCU_XAIUAMIGR_ADDR		ALT_CCU_TCU_BASE + 0x03C0
+#define ALT_CCU_TCU_XAIUMIFSR_ADDR		ALT_CCU_TCU_BASE + 0x03C4
+#define ALT_CCU_TCU_XAIUGPRBLR0_ADDR		ALT_CCU_TCU_BASE + 0x0404
+#define ALT_CCU_TCU_XAIUGPRBHR0_ADDR		ALT_CCU_TCU_BASE + 0x0408
+#define ALT_CCU_TCU_XAIUGPRAR0_ADDR		ALT_CCU_TCU_BASE + 0x0400
+#define ALT_CCU_TCU_XAIUGPRBLR1_ADDR		ALT_CCU_TCU_BASE + 0x0414
+#define ALT_CCU_TCU_XAIUGPRBHR1_ADDR		ALT_CCU_TCU_BASE + 0x0418
+#define ALT_CCU_TCU_XAIUGPRAR1_ADDR		ALT_CCU_TCU_BASE + 0x0410
+#define ALT_CCU_TCU_XAIUGPRBLR2_ADDR		ALT_CCU_TCU_BASE + 0x0424
+#define ALT_CCU_TCU_XAIUGPRBHR2_ADDR		ALT_CCU_TCU_BASE + 0x0428
+#define ALT_CCU_TCU_XAIUGPRAR2_ADDR		ALT_CCU_TCU_BASE + 0x0420
+#define ALT_CCU_TCU_XAIUGPRBLR6_ADDR		0x1C003464
+#define ALT_CCU_TCU_XAIUGPRBHR6_ADDR		0x1C003468
+#define ALT_CCU_TCU_XAIUGPRAR6_ADDR		0x1C003460
+#define ALT_CCU_TCU_XAIUGPRBLR8_ADDR		0x1C003484
+#define ALT_CCU_TCU_XAIUGPRBHR8_ADDR		0x1C003488
+#define ALT_CCU_TCU_XAIUGPRAR8_ADDR		0x1C003480
+#define ALT_CCU_TCU_XAIUGPRBLR10_ADDR		0x1C0034A4
+#define ALT_CCU_TCU_XAIUGPRBHR10_ADDR		0x1C0034A8
+#define ALT_CCU_TCU_XAIUGPRAR10_ADDR		0x1C0034A0
+//IOM
+#define ALT_CCU_CCU_IOM_XAIUAMIGR_ADDR		0x1C0043C0
+#define ALT_CCU_CCU_IOM_XAIUMIFSR_ADDR		0x1C0013C4
+#define ALT_CCU_IOM_XAIUGPRBLR1_ADDR		0x1C001414
+#define ALT_CCU_IOM_XAIUGPRBHR1_ADDR		0x1C001418
+#define ALT_CCU_IOM_XAIUGPRAR1_ADDR		0x1C001410
+#define ALT_CCU_CCU_IOM_XAIUGPRBLR6_ADDR	0x1C001464
+#define ALT_CCU_CCU_IOM_XAIUGPRBHR6_ADDR	0x1C001468
+#define ALT_CCU_CCU_IOM_XAIUGPRAR6_ADDR		0x1C001460
+#define ALT_CCU_CCU_IOM_XAIUGPRBLR8_ADDR	0x1C001484
+#define ALT_CCU_CCU_IOM_XAIUGPRBHR8_ADDR	0x1C001488
+#define ALT_CCU_CCU_IOM_XAIUGPRAR8_ADDR		0x1C001480
+#define ALT_CCU_CCU_IOM_XAIUGPRBLR10_ADDR	0x1C0014A4
+#define ALT_CCU_CCU_IOM_XAIUGPRBHR10_ADDR	0x1C0014A8
+#define ALT_CCU_CCU_IOM_XAIUGPRAR10_ADDR	0x1C0014A0
+//DCE
+#define ALT_CCU_DCE0_DCEUAMIGR_ADDR		0x1C0053C0
+#define ALT_CCU_DCE0_DCEUMIFSR_ADDR		0x1C0053C4
+#define ALT_CCU_DCE0_DCEUGPRBLR6_ADDR		0x1C005464
+#define ALT_CCU_DCE0_DCEUGPRBHR6_ADDR		0x1C005468
+#define ALT_CCU_DCE0_DCEUGPRAR6_ADDR		0x1C005460
+#define ALT_CCU_DCE0_DCEUGPRBLR8_ADDR		0x1C005484
+#define ALT_CCU_DCE0_DCEUGPRBHR8_ADDR		0x1C005488
+#define ALT_CCU_DCE0_DCEUGPRAR8_ADDR		0x1C005480
+#define ALT_CCU_DCE0_DCEUGPRBLR10_ADDR		0x1C0054A4
+#define ALT_CCU_DCE0_DCEUGPRBHR10_ADDR		0x1C0054A8
+#define ALT_CCU_DCE0_DCEUGPRAR10_ADDR		0x1C0054A0
+#define ALT_CCU_DCE1_DCEUAMIGR_ADDR		0x1C0063C0
+#define ALT_CCU_DCE1_DCEUMIFSR_ADDR		0x1C0063C4
+#define ALT_CCU_DCE1_DCEUGPRBLR6_ADDR		0x1C006464
+#define ALT_CCU_DCE1_DCEUGPRBHR6_ADDR		0x1C006468
+#define ALT_CCU_DCE1_DCEUGPRAR6_ADDR		0x1C006460
+#define ALT_CCU_DCE1_DCEUGPRBLR8_ADDR		0x1C006484
+#define ALT_CCU_DCE1_DCEUGPRBHR8_ADDR		0x1C006488
+#define ALT_CCU_DCE1_DCEUGPRAR8_ADDR		0x1C006480
+#define ALT_CCU_DCE1_DCEUGPRBLR10_ADDR		0x1C0064A4
+#define ALT_CCU_DCE1_DCEUGPRBHR10_ADDR		0x1C0064A8
+#define ALT_CCU_DCE1_DCEUGPRAR10_ADDR		0x1C0064A0
+#define offset_NRS_GPRAR0			(0x400)
+#define offset_NRS_GPRBLR0			(0x404)
+#define offset_NRS_GPRBHR0			(0x408)
+#define offset_NRS_GPRAR1			(0x410)
+#define offset_NRS_GPRBLR1			(0x414)
+#define offset_NRS_GPRBHR1			(0x418)
+#define offset_NRS_GPRAR2			(0x420)
+#define offset_NRS_GPRBLR2			(0x424)
+#define offset_NRS_GPRBHR2			(0x428)
+#define offset_NRS_GPRAR3			(0x430)
+#define offset_NRS_GPRBLR3			(0x434)
+#define offset_NRS_GPRBHR3			(0x438)
+#define offset_NRS_GPRAR4			(0x440)
+#define offset_NRS_GPRBLR4			(0x444)
+#define offset_NRS_GPRBHR4			(0x448)
+#define offset_NRS_GPRAR5			(0x450)
+#define offset_NRS_GPRBLR5			(0x454)
+#define offset_NRS_GPRBHR5			(0x458)
+#define offset_NRS_GPRAR6			(0x460)
+#define offset_NRS_GPRBLR6			(0x464)
+#define offset_NRS_GPRBHR6			(0x468)
+#define offset_NRS_GPRAR7			(0x470)
+#define offset_NRS_GPRBLR7			(0x474)
+#define offset_NRS_GPRBHR7			(0x478)
+#define offset_NRS_GPRAR8			(0x480)
+#define offset_NRS_GPRBLR8			(0x484)
+#define offset_NRS_GPRBHR8			(0x488)
+#define offset_NRS_GPRAR9			(0x490)
+#define offset_NRS_GPRBLR9			(0x494)
+#define offset_NRS_GPRBHR9			(0x498)
+#define offset_NRS_GPRAR10			(0x4a0)
+#define offset_NRS_GPRBLR10			(0x4a4)
+#define offset_NRS_GPRBHR10			(0x4a8)
+#define offset_NRS_AMIGR			(0x3c0)
+#define offset_NRS_MIFSR			(0x3c4)
+#define offset_NRS_DMIUSMCTCR			(0x300)
+#define base_addr_DII0_PSSPERIPHS		(0x10000)
+#define base_addr_DII0_LWHPS2FPGA		(0x20000)
+#define base_addr_DII0_HPS2FPGA_1G		(0x40000)
+#define base_addr_DII0_HPS2FPGA_15G		(0x400000)
+#define base_addr_DII0_HPS2FPGA_240G		(0x4000000)
+#define base_addr_DII1_MPFEREGS			(0x18000)
+#define base_addr_DII2_GICREGS			(0x1D000)
+#define base_addr_DII3_OCRAM			(0x0)
+#define base_addr_BHR				(0x0)
+#define base_addr_DMI_SDRAM_2G			(0x80000)
+#define base_addr_DMI_SDRAM_30G			(0x800000)
+#define base_addr_DMI_SDRAM_480G		(0x8000000)
+// ((0x0<<9) | (0xf<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_PSSPERIPHS			0xC0F00000
+// ((0x0<<9) | (0x11<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_LWHPS2FPGA			0xC1100000
+// ((0x0<<9) | (0x12<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_HPS2FPGA_1G			0xC1200000
+// ((0x0<<9) | (0x16<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_HPS2FPGA_15G			0xC1600000
+// ((0x0<<9) | (0x1a<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII0_HPS2FPGA_240G			0xC1A00000
+// ((0x1<<9) | (0xe<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII1_MPFEREGS			0xC0E00200
+// ((0x2<<9) | (0x8<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII2_GICREGS				0xC0800400
+// ((0x3<<9) | (0x9<<20) | (0x1<<30) | (0x1<<31))
+#define wr_DII3_OCRAM				0xC0900600
+// ((0x0<<9) | (0x12<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_1G_ORDERED			0x81200000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x12<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_1G				0x81200006
+// ((0x0<<9) | (0x13<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_2G_ORDERED			0x81300000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x13<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_2G				0x81300006
+// ((0x0<<9) | (0x16<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_15G_ORDERED		0x81600000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x16<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_15G			0x81600006
+// ((0x0<<9) | (0x17<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_30G_ORDERED		0x81700000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x17<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_30G			0x81700006
+// ((0x0<<9) | (0x1a<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_240G_ORDERED		0x81a00000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x1a<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_240G			0x81a00006
+// ((0x0<<9) | (0x1b<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_480G_ORDERED		0x81b00000
+// ((0x1<<1) | (0x1<<2) | (0x0<<9) | (0x1b<<20) | (0x0<<30) | (0x1<<31))
+#define wr_DMI_SDRAM_480G			0x81b00006
 
+typedef enum CCU_REGION_SECURITY_e {
+    //
+    // Allow secure accesses only.
+    //
+	CCU_REGION_SECURITY_SECURE_ONLY,
+    //
+    // Allow non-secure accesses only.
+    //
+	CCU_REGION_SECURITY_NON_SECURE_ONLY,
+    //
+    // Allow accesses of any security state.
+    //
+	CCU_REGION_SECURITY_DONT_CARE
+} CCU_REGION_SECURITY_t;
+typedef enum CCU_REGION_PRIVILEGE_e {
+    //
+    // Allow privileged accesses only.
+    //
+	CCU_REGION_PRIVILEGE_PRIVILEGED_ONLY,
+    //
+    // Allow unprivileged accesses only.
+    //
+	CCU_REGION_PRIVILEGE_NON_PRIVILEGED_ONLY,
+    //
+    // Allow accesses of any privilege.
+    //
+	CCU_REGION_PRIVILEGE_DONT_CARE
+} CCU_REGION_PRIVILEGE_t;
+//
+// Initializes the CCU by enabling all regions except RAM 1 - 5.
+// This is needed because of an RTL change around 2016.02.24.
+//
+// Runtime measurement:
+//  - arm     : 14,830,000 ps (2016.05.31; sanity/printf_aarch32)
+//  - aarch64 : 14,837,500 ps (2016.05.31; sanity/printf)
+//
+// Runtime history:
+//  - arm     : 20,916,668 ps (2016.05.30; sanity/printf_aarch32)
+//  - aarch64 : 20,924,168 ps (2016.05.30; sanity/printf)
+//
+int ccu_hps_init(void);
 
-/* Coherent Sub-System Address Map */
-#define NCORE_CAIU_OFFSET		0x00000
-#define NCORE_CAIU_SIZE			0x01000
+typedef enum ccu_hps_ram_region_e {
+	ccu_hps_ram_region_ramspace0 = 0,
+	ccu_hps_ram_region_ramspace1 = 1,
+	ccu_hps_ram_region_ramspace2 = 2,
+	ccu_hps_ram_region_ramspace3 = 3,
+	ccu_hps_ram_region_ramspace4 = 4,
+	ccu_hps_ram_region_ramspace5 = 5,
+} ccu_hps_ram_region_t;
 
-#define NCORE_NCBU_OFFSET		0x60000
-#define NCORE_NCBU_SIZE			0x01000
+// Disables a RAM (OCRAM) region with the given ID.
+int ccu_hps_ram_region_disable(int id);
 
-#define NCORE_DIRU_OFFSET		0x80000
-#define NCORE_DIRU_SIZE			0x01000
+// Enables a RAM (OCRAM) region with the given ID.
+int ccu_hps_ram_region_enable(int id);
 
-#define NCORE_CMIU_OFFSET		0xc0000
-#define NCORE_CMIU_SIZE			0x01000
+// Attempts to remap a RAM (OCRAM) region with the given ID to span the given
+// start and end address. It also assigns the security and privilege policy.
+// Regions must be a power-of-two size with a minimum size of 64B.
+int ccu_hps_ram_region_remap(int id, uintptr_t start, uintptr_t end,
+CCU_REGION_SECURITY_t security, CCU_REGION_PRIVILEGE_t privilege);
 
-#define NCORE_CSR_OFFSET		0xff000
-#define NCORE_CSADSERO			0x00040
-#define NCORE_CSUIDR			0x00ff8
-#define NCORE_CSIDR			0x00ffc
+// Verifies that all enabled RAM (OCRAM) regions does not overlap.
+int ccu_hps_ram_validate(void);
 
-/* Directory Unit Register Map */
-#define NCORE_DIRUSFER			0x00010
-#define NCORE_DIRUMRHER			0x00070
-#define NCORE_DIRUSFMCR			0x00080
-#define NCORE_DIRUSFMAR			0x00084
+typedef enum ccu_hps_mem_region_e {
+	ccu_hps_mem_region_ddrspace0  = 0,
+	ccu_hps_mem_region_memspace0  = 1,
+	ccu_hps_mem_region_memspace1a = 2,
+	ccu_hps_mem_region_memspace1b = 3,
+	ccu_hps_mem_region_memspace1c = 4,
+	ccu_hps_mem_region_memspace1d = 5,
+	ccu_hps_mem_region_memspace1e = 6,
+} ccu_hps_mem_region_t;
 
-/* Coherent Agent Interface Unit Register Map */
-#define NCORE_CAIUIDR			0x00ffc
+// Disables mem0 (DDR) region with the given ID.
+int ccu_hps_mem0_region_disable(int id);
 
-/* Snoop Enable Register */
-#define NCORE_DIRUCASER0		0x00040
-#define NCORE_DIRUCASER1		0x00044
-#define NCORE_DIRUCASER2		0x00048
-#define NCORE_DIRUCASER3		0x0004c
+// Enables mem0 (DDR) region with the given ID.
+int ccu_hps_mem0_region_enable(int id);
 
-#define NCORE_CSADSER0			0x00040
-#define NCORE_CSADSER1			0x00044
-#define NCORE_CSADSER2			0x00048
-#define NCORE_CSADSER3			0x0004c
+// Attempts to remap mem0 (DDR) region with the given ID to span the given
+// start and end address. It also assigns the security nad privlege policy.
+// Regions must be a power-of-two in size with a minimum size of 64B.
+int ccu_hps_mem0_region_remap(int id, uintptr_t start, uintptr_t end,
+CCU_REGION_SECURITY_t security, CCU_REGION_PRIVILEGE_t privilege);
 
-/* Protocols Definition */
-#define ACE_W_DVM			0
-#define ACE_L_W_DVM			1
-#define ACE_WO_DVM			2
-#define ACE_L_WO_DVM			3
+// Verifies that all enabled mem0 (DDR) regions does not overlap.
+int ccu_hps_mem0_validate(void);
 
-/* Bypass OC Ram Firewall */
-#define NCORE_FW_OCRAM_BLK_BASE		0x100200
-#define NCORE_FW_OCRAM_BLK_CGF1		0x04
-#define NCORE_FW_OCRAM_BLK_CGF2		0x08
-#define NCORE_FW_OCRAM_BLK_CGF3		0x0c
-#define NCORE_FW_OCRAM_BLK_CGF4		0x10
+typedef enum ccu_hps_ios_region_e {
+	ccu_hps_ios_region_iospace0a = 0,
+	ccu_hps_ios_region_iospace0b = 1,
+	ccu_hps_ios_region_iospace1a = 2,
+	ccu_hps_ios_region_iospace1b = 3,
+	ccu_hps_ios_region_iospace1c = 4,
+	ccu_hps_ios_region_iospace1d = 5,
+	ccu_hps_ios_region_iospace1e = 6,
+	ccu_hps_ios_region_iospace1f = 7,
+	ccu_hps_ios_region_iospace1g = 8,
+	ccu_hps_ios_region_iospace2a = 9,
+	ccu_hps_ios_region_iospace2b = 10,
+	ccu_hps_ios_region_iospace2c = 11,
+} ccu_hps_ios_region_t;
+
+// Disables the IOS (IO Slave) region with the given ID.
+int ccu_hps_ios_region_disable(int id);
+
+// Enables the IOS (IO Slave) region with the given ID.
+int ccu_hps_ios_region_enable(int id);
 
-#define OCRAM_PRIVILEGED_MASK		BIT(29)
-#define OCRAM_SECURE_MASK		BIT(30)
 
+#define NCORE_CCU_OFFSET			0xf7000000
+
+/* Coherent Sub-System Address Map */
+#define NCORE_CAIU_OFFSET			0x00000
+#define NCORE_CAIU_SIZE				0x01000
+#define NCORE_NCBU_OFFSET			0x60000
+#define NCORE_NCBU_SIZE				0x01000
+#define NCORE_DIRU_OFFSET			0x80000
+#define NCORE_DIRU_SIZE				0x01000
+#define NCORE_CMIU_OFFSET			0xc0000
+#define NCORE_CMIU_SIZE				0x01000
+#define NCORE_CSR_OFFSET			0xff000
+#define NCORE_CSADSERO				0x00040
+#define NCORE_CSUIDR				0x00ff8
+#define NCORE_CSIDR				0x00ffc
+/* Directory Unit Register Map */
+#define NCORE_DIRUSFER				0x00010
+#define NCORE_DIRUMRHER				0x00070
+#define NCORE_DIRUSFMCR				0x00080
+#define NCORE_DIRUSFMAR				0x00084
+/* Coherent Agent Interface Unit Register Map */
+#define NCORE_CAIUIDR				0x00ffc
+/* Snoop Enable Register */
+#define NCORE_DIRUCASER0			0x00040
+#define NCORE_DIRUCASER1			0x00044
+#define NCORE_DIRUCASER2			0x00048
+#define NCORE_DIRUCASER3			0x0004c
+#define NCORE_CSADSER0				0x00040
+#define NCORE_CSADSER1				0x00044
+#define NCORE_CSADSER2				0x00048
+#define NCORE_CSADSER3				0x0004c
+/* Protocols Definition */
+#define ACE_W_DVM				0
+#define ACE_L_W_DVM				1
+#define ACE_WO_DVM				2
+#define ACE_L_WO_DVM				3
+/* Bypass OC Ram Firewall */
+#define NCORE_FW_OCRAM_BLK_BASE			0x100200
+#define NCORE_FW_OCRAM_BLK_CGF1			0x04
+#define NCORE_FW_OCRAM_BLK_CGF2			0x08
+#define NCORE_FW_OCRAM_BLK_CGF3			0x0c
+#define NCORE_FW_OCRAM_BLK_CGF4			0x10
+#define OCRAM_PRIVILEGED_MASK			BIT(29)
+#define OCRAM_SECURE_MASK			BIT(30)
 /* Macros */
-#define NCORE_CCU_REG(base)		(NCORE_CCU_OFFSET + (base))
-#define NCORE_CCU_CSR(reg)		(NCORE_CCU_REG(NCORE_CSR_OFFSET)\
+#define NCORE_CCU_REG(base)			(NCORE_CCU_OFFSET + (base))
+#define NCORE_CCU_CSR(reg)			(NCORE_CCU_REG(NCORE_CSR_OFFSET)\
 						+ (reg))
-#define NCORE_CCU_DIR(reg)		(NCORE_CCU_REG(NCORE_DIRU_OFFSET)\
+#define NCORE_CCU_DIR(reg)			(NCORE_CCU_REG(NCORE_DIRU_OFFSET)\
 						+ (reg))
-#define NCORE_CCU_CAI(reg)		(NCORE_CCU_REG(NCORE_CAIU_OFFSET)\
+#define NCORE_CCU_CAI(reg)			(NCORE_CCU_REG(NCORE_CAIU_OFFSET)\
 						+ (reg))
-
-#define DIRECTORY_UNIT(x, reg)		(NCORE_CCU_DIR(reg)\
+#define DIRECTORY_UNIT(x, reg)			(NCORE_CCU_DIR(reg)\
 						+ NCORE_DIRU_SIZE * (x))
-#define COH_AGENT_UNIT(x, reg)		(NCORE_CCU_CAI(reg)\
+#define COH_AGENT_UNIT(x, reg)			(NCORE_CCU_CAI(reg)\
 						+ NCORE_CAIU_SIZE * (x))
-
-#define COH_CPU0_BYPASS_REG(reg)	(NCORE_CCU_REG(NCORE_FW_OCRAM_BLK_BASE)\
+#define COH_CPU0_BYPASS_REG(reg)		(NCORE_CCU_REG(NCORE_FW_OCRAM_BLK_BASE)\
 						+ (reg))
-
-#define CSUIDR_NUM_CMI(x)		(((x) & 0x3f000000) >> 24)
-#define CSUIDR_NUM_DIR(x)		(((x) & 0x003f0000) >> 16)
-#define CSUIDR_NUM_NCB(x)		(((x) & 0x00003f00) >> 8)
-#define CSUIDR_NUM_CAI(x)		(((x) & 0x0000007f) >> 0)
-
-#define CSIDR_NUM_SF(x)			(((x) & 0x007c0000) >> 18)
-
-#define SNOOP_FILTER_ID(x)		(((x) << 16))
-
-#define CACHING_AGENT_BIT(x)		(((x) & 0x08000) >> 15)
-#define CACHING_AGENT_TYPE(x)		(((x) & 0xf0000) >> 16)
-
+#define CSUIDR_NUM_CMI(x)			(((x) & 0x3f000000) >> 24)
+#define CSUIDR_NUM_DIR(x)			(((x) & 0x003f0000) >> 16)
+#define CSUIDR_NUM_NCB(x)			(((x) & 0x00003f00) >> 8)
+#define CSUIDR_NUM_CAI(x)			(((x) & 0x0000007f) >> 0)
+#define CSIDR_NUM_SF(x)				(((x) & 0x007c0000) >> 18)
+#define SNOOP_FILTER_ID(x)			(((x) << 16))
+#define CACHING_AGENT_BIT(x)			(((x) & 0x08000) >> 15)
+#define CACHING_AGENT_TYPE(x)			(((x) & 0xf0000) >> 16)
 
 typedef struct coh_ss_id {
 	uint8_t num_coh_mem;
@@ -105,5 +426,6 @@
 
 uint32_t init_ncore_ccu(void);
 void ncore_enable_ocram_firewall(void);
+void setup_smmu_stream_id(void);
 
 #endif
diff --git a/plat/intel/soc/common/drivers/combophy/combophy.c b/plat/intel/soc/common/drivers/combophy/combophy.c
new file mode 100644
index 0000000..6c53bc1
--- /dev/null
+++ b/plat/intel/soc/common/drivers/combophy/combophy.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/cadence/cdns_sdmmc.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+#include "combophy.h"
+#include "sdmmc/sdmmc.h"
+
+/* Temp assigned handoff data, need to remove when SDM up and run. */
+void config_nand(handoff *hoff_ptr)
+{
+	/* This is hardcoded input value for Combo PHY and SD host controller. */
+	hoff_ptr->peripheral_pwr_gate_array = 0x40;
+
+}
+
+/* DFI configuration */
+int dfi_select(handoff *hoff_ptr)
+{
+	uint32_t data = 0;
+
+	/* Temp assigned handoff data, need to remove when SDM up and run. */
+	handoff reverse_handoff_ptr;
+
+	/* Temp assigned handoff data, need to remove when SDM up and run. */
+	config_nand(&reverse_handoff_ptr);
+
+	if (((reverse_handoff_ptr.peripheral_pwr_gate_array) & PERIPHERAL_SDMMC_MASK) == 0U) {
+		ERROR("SDMMC/NAND is not set properly\n");
+		return -ENXIO;
+	}
+
+	mmio_setbits_32(SOCFPGA_SYSMGR(DFI_INTF),
+		(((reverse_handoff_ptr.peripheral_pwr_gate_array) &
+		PERIPHERAL_SDMMC_MASK) >> PERIPHERAL_SDMMC_OFFSET));
+	data = mmio_read_32(SOCFPGA_SYSMGR(DFI_INTF));
+	if ((data & DFI_INTF_MASK) != (((reverse_handoff_ptr.peripheral_pwr_gate_array) &
+		PERIPHERAL_SDMMC_MASK) >> PERIPHERAL_SDMMC_OFFSET)) {
+		ERROR("DFI is not set properly\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+int combo_phy_init(handoff *hoff_ptr)
+{
+	/* SDMMC/NAND DFI selection based on system manager DFI register */
+	int ret = dfi_select(hoff_ptr);
+
+	if (ret != 0U) {
+		ERROR("DFI configuration failed\n");
+		return ret;
+	}
+
+	return 0;
+}
diff --git a/plat/intel/soc/common/drivers/combophy/combophy.h b/plat/intel/soc/common/drivers/combophy/combophy.h
new file mode 100644
index 0000000..ca571f7
--- /dev/null
+++ b/plat/intel/soc/common/drivers/combophy/combophy.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef COMBOPHY_H
+#define COMBOPHY_H
+
+#include <lib/mmio.h>
+
+#include "socfpga_handoff.h"
+
+#define PERIPHERAL_SDMMC_MASK		0x60
+#define PERIPHERAL_SDMMC_OFFSET		6
+#define DFI_INTF_MASK			0x1
+
+/* FUNCTION DEFINATION */
+/*
+ * @brief Nand controller initialization function
+ *
+ * @hoff_ptr: Pointer to the hand-off data
+ * Return: 0 on success, a negative errno on failure
+ */
+int combo_phy_init(handoff *hoff_ptr);
+int dfi_select(handoff *hoff_ptr);
+
+#endif
diff --git a/plat/intel/soc/common/drivers/ddr/ddr.c b/plat/intel/soc/common/drivers/ddr/ddr.c
new file mode 100644
index 0000000..188302f
--- /dev/null
+++ b/plat/intel/soc/common/drivers/ddr/ddr.c
@@ -0,0 +1,342 @@
+/*
+ * Copyright (c) 2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <common/debug.h>
+#include "ddr.h"
+#include <lib/mmio.h>
+#include "socfpga_handoff.h"
+
+int ddr_calibration_check(void)
+{
+	// DDR calibration check
+	int status = 0;
+	uint32_t u32data_read = 0;
+
+	NOTICE("DDR: Access address 0x%x:...\n", IO96B_0_REG_BASE);
+	u32data_read = mmio_read_32(IO96B_0_REG_BASE);
+	NOTICE("DDR: Access address 0x%x: read 0x%04x\n", IO96B_0_REG_BASE, u32data_read);
+
+	if (u32data_read == -EPERM) {
+		status = -EPERM;
+		assert(u32data_read);
+	}
+
+	u32data_read = 0x0;
+	NOTICE("DDR: Access address 0x%x: ...\n", IO96B_1_REG_BASE);
+	u32data_read = mmio_read_32(IO96B_1_REG_BASE);
+	NOTICE("DDR: Access address 0x%x: read 0x%04x\n", IO96B_1_REG_BASE, u32data_read);
+
+	if (u32data_read == -EPERM) {
+		status = -EPERM;
+		assert(u32data_read);
+	}
+
+	return status;
+}
+
+int iossm_mb_init(void)
+{
+	// int status;
+
+	// Update according to IOSSM mailbox spec
+
+	// if (status) {
+		// return status;
+	// }
+
+	return 0;
+}
+
+int wait_respond(uint16_t timeout)
+{
+	uint32_t status = 0;
+	uint32_t count = 0;
+	uint32_t data = 0;
+
+	/* Wait status command response ready */
+	do {
+		data = mmio_read_32(IO96B_CSR_REG(CMD_RESPONSE_STATUS));
+		count++;
+		if (count >= timeout) {
+			return -ETIMEDOUT;
+		}
+
+	} while (STATUS_COMMAND_RESPONSE(data) != STATUS_COMMAND_RESPONSE_READY);
+
+	status = (data & STATUS_GENERAL_ERROR_MASK) >> STATUS_GENERAL_ERROR_OFFSET;
+	if (status != 0) {
+		return status;
+	}
+
+	status = (data & STATUS_CMD_RESPONSE_ERROR_MASK) >> STATUS_CMD_RESPONSE_ERROR_OFFSET;
+	if (status != 0) {
+		return status;
+	}
+
+	return status;
+}
+
+int iossm_mb_read_response(void)
+{
+	uint32_t status = 0;
+	unsigned int i;
+	uint32_t resp_data[IOSSM_RESP_MAX_WORD_SIZE];
+	uint32_t resp_param_reg;
+
+	// Check STATUS_CMD_RESPONSE_DATA_PTR_VALID in
+	// STATUS_COMMAND_RESPONSE to ensure data pointer response
+
+	/* Read CMD_RESPONSE_STATUS and CMD_RESPONSE_DATA_* */
+	resp_data[0] = mmio_read_32(IO96B_CSR_REG(CMD_RESPONSE_STATUS));
+	resp_data[0] = (resp_data[0] & CMD_RESPONSE_DATA_SHORT_MASK) >>
+			CMD_RESPONSE_DATA_SHORT_OFFSET;
+	resp_param_reg = CMD_RESPONSE_STATUS;
+	for (i = 1; i < IOSSM_RESP_MAX_WORD_SIZE; i++) {
+		resp_param_reg = resp_param_reg - CMD_RESPONSE_OFFSET;
+		resp_data[i] = mmio_read_32(IO96B_CSR_REG(resp_param_reg));
+	}
+
+	/* Wait for STATUS_COMMAND_RESPONSE_READY*/
+	status = wait_respond(1000);
+
+	/* Read CMD_RESPONSE_STATUS and CMD_RESPONSE_DATA_* */
+	mmio_setbits_32(STATUS_COMMAND_RESPONSE(IO96B_CSR_REG(
+			CMD_RESPONSE_STATUS)),
+			STATUS_COMMAND_RESPONSE_READY_CLEAR);
+
+	return status;
+}
+
+int iossm_mb_send(uint32_t cmd_target_ip_type, uint32_t cmd_target_ip_instance_id,
+			uint32_t cmd_type, uint32_t cmd_opcode, uint32_t *args,
+			unsigned int len)
+{
+	unsigned int i;
+	uint32_t status = 0;
+	uint32_t cmd_req;
+	uint32_t cmd_param_reg;
+
+	cmd_target_ip_type = (cmd_target_ip_type & CMD_TARGET_IP_TYPE_MASK) <<
+				CMD_TARGET_IP_TYPE_OFFSET;
+	cmd_target_ip_instance_id = (cmd_target_ip_instance_id &
+				CMD_TARGET_IP_INSTANCE_ID_MASK) <<
+				CMD_TARGET_IP_INSTANCE_ID_OFFSET;
+	cmd_type = (cmd_type & CMD_TYPE_MASK) << CMD_TYPE_OFFSET;
+	cmd_opcode = (cmd_opcode & CMD_OPCODE_MASK) << CMD_OPCODE_OFFSET;
+	cmd_req = cmd_target_ip_type | cmd_target_ip_instance_id | cmd_type |
+			cmd_opcode;
+
+	/* send mailbox request */
+	IOSSM_MB_WRITE(IO96B_CSR_REG(CMD_REQ), cmd_req);
+	if (len != 0) {
+		cmd_param_reg = CMD_REQ;
+		for (i = 0; i < len; i++) {
+			cmd_param_reg = cmd_param_reg - CMD_PARAM_OFFSET;
+			IOSSM_MB_WRITE(IO96B_CSR_REG(cmd_param_reg), args[i]);
+		}
+	}
+
+	status = iossm_mb_read_response();
+	if (status != 0) {
+		return status;
+	}
+
+	return status;
+}
+
+int ddr_iossm_mailbox_cmd(uint32_t cmd_opcode)
+{
+	// IOSSM
+	uint32_t status = 0;
+	unsigned int i = 0;
+	uint32_t payload[IOSSM_CMD_MAX_WORD_SIZE] = {0U};
+
+	switch (cmd_opcode) {
+	case CMD_INIT:
+		status = iossm_mb_init();
+		break;
+
+	case OPCODE_GET_MEM_INTF_INFO:
+		status = iossm_mb_send(0, 0, MBOX_CMD_GET_SYS_INFO,
+		OPCODE_GET_MEM_INTF_INFO, payload, i);
+		break;
+
+	case OPCODE_GET_MEM_TECHNOLOGY:
+		status = iossm_mb_send(0, 0, MBOX_CMD_GET_MEM_INFO,
+		OPCODE_GET_MEM_TECHNOLOGY, payload, i);
+		break;
+
+	case OPCODE_GET_MEM_WIDTH_INFO:
+		status = iossm_mb_send(0, 0, MBOX_CMD_GET_MEM_INFO,
+		OPCODE_GET_MEM_WIDTH_INFO, payload, i);
+		break;
+
+	case OPCODE_ECC_ENABLE_STATUS:
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_ECC_ENABLE_STATUS,
+		payload, i);
+		break;
+
+	case OPCODE_ECC_INTERRUPT_MASK:
+		// payload[i] = CMD_PARAM_0 [16:0]: ECC_INTERRUPT_MASK
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_ECC_INTERRUPT_MASK,
+		payload, i);
+		break;
+
+	case OPCODE_ECC_SCRUB_MODE_0_START:
+		// payload[i] = CMD_PARAM_0 [15:0]: ECC_SCRUB_INTERVAL
+		//i++;
+		// payload[i] = CMD_PARAM_1 [11:0]: ECC_SCRUB_LEN
+		//i++;
+		// payload[i] = CMD_PARAM_2 [0:0]: ECC_SCRUB_FULL_MEM
+		//i++;
+		// payload[i]= CMD_PARAM_3 [31:0]: ECC_SCRUB_START_ADDR [31:0]
+		//i++;
+		// payload[i] = CMD_PARAM_4 [5:0]: ECC_SCRUB_START_ADDR [36:32]
+		//i++;
+		// payload[i] = CMD_PARAM_5 [31:0]: ECC_SCRUB_END_ADDR [31:0]
+		//i++;
+		// payload[i] = CMD_PARAM_6 [5:0]: ECC_SCRUB_END_ADDR [36:32]
+		//i++;
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_ECC_SCRUB_MODE_0_START,
+		payload, i);
+		break;
+
+	case OPCODE_ECC_SCRUB_MODE_1_START:
+		// payload[i] = CMD_PARAM_0 [15:0]: ECC_SCRUB_IDLE_CNT
+		//i++;
+		// payload[i] = CMD_PARAM_1 [11:0]: ECC_SCRUB_LEN
+		//i++;
+		// payload[i] = CMD_PARAM_2 [0:0]: ECC_SCRUB_FULL_MEM
+		//i++;
+		// payload[i] = CMD_PARAM_3 [31:0]: ECC_SCRUB_START_ADDR [31:0]
+		//i++;
+		// payload[i] = CMD_PARAM_4 [5:0]: ECC_SCRUB_START_ADDR [36:32]
+		//i++;
+		// payload[i] = CMD_PARAM_5 [31:0]: ECC_SCRUB_END_ADDR [31:0]
+		//i++;
+		// payload[i] = CMD_PARAM_6 [5:0]: ECC_SCRUB_END_ADDR [36:32]
+		//i++;
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_ECC_SCRUB_MODE_1_START,
+		payload, i);
+		break;
+
+	case OPCODE_BIST_RESULTS_STATUS:
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_BIST_RESULTS_STATUS,
+		payload, i);
+		break;
+
+	case OPCODE_BIST_MEM_INIT_START:
+		status = iossm_mb_send(0, 0,
+		MBOX_CMD_TRIG_CONTROLLER_OP, OPCODE_BIST_MEM_INIT_START,
+		payload, i);
+		break;
+
+	case OPCODE_TRIG_MEM_CAL:
+		status = iossm_mb_send(0, 0, MBOX_CMD_TRIG_MEM_CAL_OP,
+		OPCODE_TRIG_MEM_CAL, payload, i);
+		break;
+
+	default:
+		break;
+	}
+
+	if (status == -EPERM) {
+		assert(status);
+	}
+
+	return status;
+}
+
+int ddr_config_handoff(handoff *hoff_ptr)
+{
+	/* Populate DDR handoff data */
+	/* TODO: To add in DDR handoff configuration once available */
+	return 0;
+}
+
+// DDR firewall and non secure access
+void ddr_enable_ns_access(void)
+{
+	/* Please set the ddr non secure registers accordingly */
+
+	mmio_setbits_32(CCU_REG(DMI0_DMIUSMCTCR),
+			CCU_DMI_ALLOCEN | CCU_DMI_LOOKUPEN);
+	mmio_setbits_32(CCU_REG(DMI1_DMIUSMCTCR),
+			CCU_DMI_ALLOCEN | CCU_DMI_LOOKUPEN);
+
+	/* TODO: To add in CCU NCORE OCRAM bypass mask for non secure registers */
+	NOTICE("DDR non secure configured\n");
+}
+
+void ddr_enable_firewall(void)
+{
+	/* Please set the ddr firewall registers accordingly */
+	/* TODO: To add in CCU NCORE OCRAM bypass mask for firewall registers */
+	NOTICE("DDR firewall enabled\n");
+}
+
+bool is_ddr_init_in_progress(void)
+{
+	uint32_t reg = mmio_read_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_POR_0));
+
+	if (reg & SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_0_MASK) {
+		return true;
+	}
+	return false;
+}
+
+int ddr_init(void)
+{
+	// DDR driver initialization
+	int status = -EPERM;
+	uint32_t cmd_opcode = 0;
+
+	// Check and set Boot Scratch Register
+	if (is_ddr_init_in_progress()) {
+		return status;
+	}
+	mmio_write_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_POR_0), 0x01);
+
+	// Populate DDR handoff data
+	handoff reverse_handoff_ptr;
+
+	if (!socfpga_get_handoff(&reverse_handoff_ptr)) {
+		assert(status);
+	}
+	status = ddr_config_handoff(&reverse_handoff_ptr);
+	if (status == -EPERM) {
+		assert(status);
+	}
+
+	// CCU and firewall setup
+	ddr_enable_ns_access();
+	ddr_enable_firewall();
+
+	// DDR calibration check
+	status = ddr_calibration_check();
+	if (status == -EPERM) {
+		assert(status);
+	}
+
+	// DDR mailbox command
+	status = ddr_iossm_mailbox_cmd(cmd_opcode);
+	if (status != 0) {
+		assert(status);
+	}
+
+	// Check and set Boot Scratch Register
+	mmio_write_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_POR_0), 0x00);
+
+	NOTICE("DDR init successfully\n");
+	return status;
+}
diff --git a/plat/intel/soc/common/drivers/ddr/ddr.h b/plat/intel/soc/common/drivers/ddr/ddr.h
new file mode 100644
index 0000000..416b64e
--- /dev/null
+++ b/plat/intel/soc/common/drivers/ddr/ddr.h
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef DDR_H
+#define DDR_H
+
+#include <lib/mmio.h>
+#include "socfpga_handoff.h"
+
+/* MACRO DEFINATION */
+#define IO96B_0_REG_BASE				0x18400000
+#define IO96B_1_REG_BASE				0x18800000
+#define IO96B_CSR_BASE					0x05000000
+#define IO96B_CSR_REG(reg)				(IO96B_CSR_BASE + reg)
+
+#define IOSSM_CMD_MAX_WORD_SIZE				7U
+#define IOSSM_RESP_MAX_WORD_SIZE			4U
+
+#define CCU_REG_BASE					0x1C000000
+#define DMI0_DMIUSMCTCR					0x7300
+#define DMI1_DMIUSMCTCR					0x8300
+#define CCU_DMI_ALLOCEN					BIT(1)
+#define CCU_DMI_LOOKUPEN				BIT(2)
+#define CCU_REG(reg)					(CCU_REG_BASE + reg)
+
+// CMD_RESPONSE_STATUS Register
+#define CMD_RESPONSE_STATUS				0x45C
+#define CMD_RESPONSE_OFFSET				0x4
+#define CMD_RESPONSE_DATA_SHORT_MASK			GENMASK(31, 16)
+#define CMD_RESPONSE_DATA_SHORT_OFFSET			16
+#define STATUS_CMD_RESPONSE_ERROR_MASK			GENMASK(7, 5)
+#define STATUS_CMD_RESPONSE_ERROR_OFFSET		5
+#define STATUS_GENERAL_ERROR_MASK			GENMASK(4, 1)
+#define STATUS_GENERAL_ERROR_OFFSET			1
+#define STATUS_COMMAND_RESPONSE_READY			0x1
+#define STATUS_COMMAND_RESPONSE_READY_CLEAR		0x0
+#define STATUS_COMMAND_RESPONSE_READY_MASK		0x1
+#define STATUS_COMMAND_RESPONSE_READY_OFFSET		0
+#define STATUS_COMMAND_RESPONSE(x)			(((x) & \
+							STATUS_COMMAND_RESPONSE_READY_MASK) >> \
+							STATUS_COMMAND_RESPONSE_READY_OFFSET)
+
+// CMD_REQ Register
+#define CMD_STATUS					0x400
+#define CMD_PARAM					0x438
+#define CMD_REQ						0x43C
+#define CMD_PARAM_OFFSET				0x4
+#define CMD_TARGET_IP_TYPE_MASK				GENMASK(31, 29)
+#define CMD_TARGET_IP_TYPE_OFFSET			29
+#define CMD_TARGET_IP_INSTANCE_ID_MASK			GENMASK(28, 24)
+#define CMD_TARGET_IP_INSTANCE_ID_OFFSET		24
+#define CMD_TYPE_MASK					GENMASK(23, 16)
+#define CMD_TYPE_OFFSET					16
+#define CMD_OPCODE_MASK					GENMASK(15, 0)
+#define CMD_OPCODE_OFFSET				0
+
+#define CMD_INIT					0
+
+#define OPCODE_GET_MEM_INTF_INFO			0x0001
+#define OPCODE_GET_MEM_TECHNOLOGY			0x0002
+#define OPCODE_GET_MEM_WIDTH_INFO			0x0004
+#define OPCODE_TRIG_MEM_CAL				0x000A
+#define OPCODE_ECC_ENABLE_STATUS			0x0102
+#define OPCODE_ECC_INTERRUPT_MASK			0x0105
+#define OPCODE_ECC_SCRUB_MODE_0_START			0x0202
+#define OPCODE_ECC_SCRUB_MODE_1_START			0x0203
+#define OPCODE_BIST_RESULTS_STATUS			0x0302
+#define OPCODE_BIST_MEM_INIT_START			0x0303
+// Please update according to IOSSM mailbox spec
+#define MBOX_ID_IOSSM					0x00
+#define MBOX_CMD_GET_SYS_INFO				0x01
+// Please update according to IOSSM mailbox spec
+#define MBOX_CMD_GET_MEM_INFO				0x02
+#define MBOX_CMD_TRIG_CONTROLLER_OP			0x04
+#define MBOX_CMD_TRIG_MEM_CAL_OP			0x05
+#define MBOX_CMD_POKE_REG				0xFD
+#define MBOX_CMD_PEEK_REG				0xFE
+#define MBOX_CMD_GET_DEBUG_LOG				0xFF
+// Please update according to IOSSM mailbox spec
+#define MBOX_CMD_DIRECT					0x00
+
+#define SOCFPGA_SYSMGR_BOOT_SCRATCH_POR_0_MASK		0x01
+
+#define IOSSM_MB_WRITE(addr, data)			mmio_write_32(addr, data)
+
+/* FUNCTION DEFINATION */
+int ddr_calibration_check(void);
+
+int iossm_mb_init(void);
+
+int iossm_mb_read_response(void);
+
+int iossm_mb_send(uint32_t cmd_target_ip_type, uint32_t cmd_target_ip_instance_id,
+			uint32_t cmd_type, uint32_t cmd_opcode, uint32_t *args,
+			unsigned int len);
+
+int ddr_iossm_mailbox_cmd(uint32_t cmd);
+
+int ddr_init(void);
+
+int ddr_config_handoff(handoff *hoff_ptr);
+
+void ddr_enable_ns_access(void);
+
+void ddr_enable_firewall(void);
+
+bool is_ddr_init_in_progress(void);
+
+#endif
diff --git a/plat/intel/soc/common/drivers/nand/nand.c b/plat/intel/soc/common/drivers/nand/nand.c
new file mode 100644
index 0000000..c6acbe3
--- /dev/null
+++ b/plat/intel/soc/common/drivers/nand/nand.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/cadence/cdns_nand.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include "nand.h"
+
+#include "agilex5_pinmux.h"
+#include "combophy/combophy.h"
+
+/* Pinmux configuration */
+static void nand_pinmux_config(void)
+{
+	mmio_write_32(SOCFPGA_PINMUX(PIN0SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN1SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN2SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN3SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN4SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN5SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN6SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN7SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN8SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN9SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN10SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN11SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN12SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN13SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN14SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN16SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN17SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN18SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN19SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN20SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN21SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN22SEL), SOCFPGA_PINMUX_SEL_NAND);
+	mmio_write_32(SOCFPGA_PINMUX(PIN23SEL), SOCFPGA_PINMUX_SEL_NAND);
+}
+
+int nand_init(handoff *hoff_ptr)
+{
+	/* NAND pin mux configuration */
+	nand_pinmux_config();
+
+	return cdns_nand_host_init();
+}
diff --git a/plat/intel/soc/common/drivers/nand/nand.h b/plat/intel/soc/common/drivers/nand/nand.h
new file mode 100644
index 0000000..b060a72
--- /dev/null
+++ b/plat/intel/soc/common/drivers/nand/nand.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef DDR_H
+#define DDR_H
+
+#include <lib/mmio.h>
+#include "socfpga_handoff.h"
+
+/* FUNCTION DEFINATION */
+/*
+ * @brief Nand controller initialization function
+ *
+ * @hoff_ptr: Pointer to the hand-off data
+ * Return: 0 on success, a negative errno on failure
+ */
+int nand_init(handoff *hoff_ptr);
+
+#endif
diff --git a/plat/intel/soc/common/drivers/qspi/cadence_qspi.c b/plat/intel/soc/common/drivers/qspi/cadence_qspi.c
index cecf560..da8a8bd 100644
--- a/plat/intel/soc/common/drivers/qspi/cadence_qspi.c
+++ b/plat/intel/soc/common/drivers/qspi/cadence_qspi.c
@@ -13,6 +13,7 @@
 #include <drivers/console.h>
 
 #include "cadence_qspi.h"
+#include "socfpga_plat_def.h"
 
 #define LESS(a, b)   (((a) < (b)) ? (a) : (b))
 #define MORE(a, b)   (((a) > (b)) ? (a) : (b))
diff --git a/plat/intel/soc/common/drivers/qspi/cadence_qspi.h b/plat/intel/soc/common/drivers/qspi/cadence_qspi.h
index cfef585..104ab5f 100644
--- a/plat/intel/soc/common/drivers/qspi/cadence_qspi.h
+++ b/plat/intel/soc/common/drivers/qspi/cadence_qspi.h
@@ -10,8 +10,6 @@
 
 #define CAD_QSPI_MICRON_N25Q_SUPPORT		1
 
-#define CAD_QSPI_OFFSET				0xff8d2000
-
 #define CAD_INVALID				-1
 #define CAD_QSPI_ERROR				-2
 
@@ -38,8 +36,6 @@
 #define CAD_QSPI_CFG_SELCLKPHASE_CLR_MSK	0xfffffffb
 #define CAD_QSPI_CFG_SELCLKPOL_CLR_MSK		0xfffffffd
 
-#define CAD_QSPIDATA_OFST			0xff900000
-
 #define CAD_QSPI_DELAY				0xc
 #define CAD_QSPI_DELAY_CSSOT(x)			(((x) & 0xff) << 0)
 #define CAD_QSPI_DELAY_CSEOT(x)			(((x) & 0xff) << 8)
diff --git a/plat/intel/soc/common/drivers/sdmmc/sdmmc.c b/plat/intel/soc/common/drivers/sdmmc/sdmmc.c
new file mode 100644
index 0000000..8666f54
--- /dev/null
+++ b/plat/intel/soc/common/drivers/sdmmc/sdmmc.c
@@ -0,0 +1,769 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/cadence/cdns_combo_phy.h>
+#include <drivers/cadence/cdns_sdmmc.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+#include "agilex5_pinmux.h"
+#include "sdmmc.h"
+
+static const struct mmc_ops *ops;
+static unsigned int mmc_ocr_value;
+static struct mmc_csd_emmc mmc_csd;
+static struct sd_switch_status sd_switch_func_status;
+static unsigned char mmc_ext_csd[512] __aligned(16);
+static unsigned int mmc_flags;
+static struct mmc_device_info *mmc_dev_info;
+static unsigned int rca;
+static unsigned int scr[2]__aligned(16) = { 0 };
+
+extern const struct mmc_ops cdns_sdmmc_ops;
+extern struct cdns_sdmmc_params cdns_params;
+extern struct cdns_sdmmc_combo_phy sdmmc_combo_phy_reg;
+extern struct cdns_sdmmc_sdhc sdmmc_sdhc_reg;
+
+static bool is_cmd23_enabled(void)
+{
+	return ((mmc_flags & MMC_FLAG_CMD23) != 0U);
+}
+
+static bool is_sd_cmd6_enabled(void)
+{
+	return ((mmc_flags & MMC_FLAG_SD_CMD6) != 0U);
+}
+
+/* TODO: Will romove once ATF driver is developed */
+void sdmmc_pin_config(void)
+{
+	/* temp use base + addr. Official must change to common method */
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x00, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x04, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x08, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x0C, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x10, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x14, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x18, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x1C, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x20, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x24, 0x0);
+	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x28, 0x0);
+}
+
+static int sdmmc_send_cmd(unsigned int idx, unsigned int arg,
+			unsigned int r_type, unsigned int *r_data)
+{
+	struct mmc_cmd cmd;
+	int ret;
+
+	zeromem(&cmd, sizeof(struct mmc_cmd));
+
+	cmd.cmd_idx = idx;
+	cmd.cmd_arg = arg;
+	cmd.resp_type = r_type;
+
+	ret = ops->send_cmd(&cmd);
+
+	if ((ret == 0) && (r_data != NULL)) {
+		int i;
+
+		for (i = 0; i < 4; i++) {
+			*r_data = cmd.resp_data[i];
+			r_data++;
+		}
+	}
+
+	if (ret != 0) {
+		VERBOSE("Send command %u error: %d\n", idx, ret);
+	}
+
+	return ret;
+}
+
+static int sdmmc_device_state(void)
+{
+	int retries = DEFAULT_SDMMC_MAX_RETRIES;
+	unsigned int resp_data[4];
+
+	do {
+		int ret;
+
+		if (retries == 0) {
+			ERROR("CMD13 failed after %d retries\n",
+			      DEFAULT_SDMMC_MAX_RETRIES);
+			return -EIO;
+		}
+
+		ret = sdmmc_send_cmd(MMC_CMD(13), rca << RCA_SHIFT_OFFSET,
+				   MMC_RESPONSE_R1, &resp_data[0]);
+		if (ret != 0) {
+			retries--;
+			continue;
+		}
+
+		if ((resp_data[0] & STATUS_SWITCH_ERROR) != 0U) {
+			return -EIO;
+		}
+
+		retries--;
+	} while ((resp_data[0] & STATUS_READY_FOR_DATA) == 0U);
+
+	return MMC_GET_STATE(resp_data[0]);
+}
+
+static int sdmmc_set_ext_csd(unsigned int ext_cmd, unsigned int value)
+{
+	int ret;
+
+	ret = sdmmc_send_cmd(MMC_CMD(6),
+			   EXTCSD_WRITE_BYTES | EXTCSD_CMD(ext_cmd) |
+			   EXTCSD_VALUE(value) | EXTCSD_CMD_SET_NORMAL,
+			   MMC_RESPONSE_R1B, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret == MMC_STATE_PRG);
+
+	return 0;
+}
+
+static int sdmmc_mmc_sd_switch(unsigned int bus_width)
+{
+	int ret;
+	int retries = DEFAULT_SDMMC_MAX_RETRIES;
+	unsigned int bus_width_arg = 0;
+
+	/* CMD55: Application Specific Command */
+	ret = sdmmc_send_cmd(MMC_CMD(55), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R5, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = ops->prepare(0, (uintptr_t)&scr, sizeof(scr));
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* ACMD51: SEND_SCR */
+	do {
+		ret = sdmmc_send_cmd(MMC_ACMD(51), 0, MMC_RESPONSE_R1, NULL);
+		if ((ret != 0) && (retries == 0)) {
+			ERROR("ACMD51 failed after %d retries (ret=%d)\n",
+			      DEFAULT_SDMMC_MAX_RETRIES, ret);
+			return ret;
+		}
+
+		retries--;
+	} while (ret != 0);
+
+	ret = ops->read(0, (uintptr_t)&scr, sizeof(scr));
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (((scr[0] & SD_SCR_BUS_WIDTH_4) != 0U) &&
+	    (bus_width == MMC_BUS_WIDTH_4)) {
+		bus_width_arg = 2;
+	}
+
+	/* CMD55: Application Specific Command */
+	ret = sdmmc_send_cmd(MMC_CMD(55), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R5, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* ACMD6: SET_BUS_WIDTH */
+	ret = sdmmc_send_cmd(MMC_ACMD(6), bus_width_arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret == MMC_STATE_PRG);
+
+	return 0;
+}
+
+static int sdmmc_set_ios(unsigned int clk, unsigned int bus_width)
+{
+	int ret;
+	unsigned int width = bus_width;
+
+	if (mmc_dev_info->mmc_dev_type != MMC_IS_EMMC) {
+		if (width == MMC_BUS_WIDTH_8) {
+			WARN("Wrong bus config for SD-card, force to 4\n");
+			width = MMC_BUS_WIDTH_4;
+		}
+		ret = sdmmc_mmc_sd_switch(width);
+		if (ret != 0) {
+			return ret;
+		}
+	} else if (mmc_csd.spec_vers == 4U) {
+		ret = sdmmc_set_ext_csd(CMD_EXTCSD_BUS_WIDTH,
+				      (unsigned int)width);
+		if (ret != 0) {
+			return ret;
+		}
+	} else {
+		VERBOSE("Wrong MMC type or spec version\n");
+	}
+
+	return ops->set_ios(clk, width);
+}
+
+static int sdmmc_fill_device_info(void)
+{
+	unsigned long long c_size;
+	unsigned int speed_idx;
+	unsigned int nb_blocks;
+	unsigned int freq_unit;
+	int ret = 0;
+	struct mmc_csd_sd_v2 *csd_sd_v2;
+
+	switch (mmc_dev_info->mmc_dev_type) {
+	case MMC_IS_EMMC:
+		mmc_dev_info->block_size = MMC_BLOCK_SIZE;
+
+		ret = ops->prepare(0, (uintptr_t)&mmc_ext_csd,
+				   sizeof(mmc_ext_csd));
+		if (ret != 0) {
+			return ret;
+		}
+
+		/* MMC CMD8: SEND_EXT_CSD */
+		ret = sdmmc_send_cmd(MMC_CMD(8), 0, MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return ret;
+		}
+
+		ret = ops->read(0, (uintptr_t)&mmc_ext_csd,
+				sizeof(mmc_ext_csd));
+		if (ret != 0) {
+			return ret;
+		}
+
+		do {
+			ret = sdmmc_device_state();
+			if (ret < 0) {
+				return ret;
+			}
+		} while (ret != MMC_STATE_TRAN);
+
+		nb_blocks = (mmc_ext_csd[CMD_EXTCSD_SEC_CNT] << 0) |
+			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 1] << 8) |
+			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 2] << 16) |
+			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 3] << 24);
+
+		mmc_dev_info->device_size = (unsigned long long)nb_blocks *
+			mmc_dev_info->block_size;
+
+		break;
+
+	case MMC_IS_SD:
+		/*
+		 * Use the same mmc_csd struct, as required fields here
+		 * (READ_BL_LEN, C_SIZE, CSIZE_MULT) are common with eMMC.
+		 */
+		mmc_dev_info->block_size = BIT_32(mmc_csd.read_bl_len);
+
+		c_size = ((unsigned long long)mmc_csd.c_size_high << 2U) |
+			 (unsigned long long)mmc_csd.c_size_low;
+		assert(c_size != 0xFFFU);
+
+		mmc_dev_info->device_size = (c_size + 1U) *
+					    BIT_64(mmc_csd.c_size_mult + 2U) *
+					    mmc_dev_info->block_size;
+
+		break;
+
+	case MMC_IS_SD_HC:
+		assert(mmc_csd.csd_structure == 1U);
+
+		mmc_dev_info->block_size = MMC_BLOCK_SIZE;
+
+		/* Need to use mmc_csd_sd_v2 struct */
+		csd_sd_v2 = (struct mmc_csd_sd_v2 *)&mmc_csd;
+		c_size = ((unsigned long long)csd_sd_v2->c_size_high << 16) |
+			 (unsigned long long)csd_sd_v2->c_size_low;
+
+		mmc_dev_info->device_size = (c_size + 1U) << SDMMC_MULT_BY_512K_SHIFT;
+
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	speed_idx = (mmc_csd.tran_speed & CSD_TRAN_SPEED_MULT_MASK) >>
+			 CSD_TRAN_SPEED_MULT_SHIFT;
+
+	assert(speed_idx > 0U);
+
+	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
+		mmc_dev_info->max_bus_freq = tran_speed_base[speed_idx];
+	} else {
+		mmc_dev_info->max_bus_freq = sd_tran_speed_base[speed_idx];
+	}
+
+	freq_unit = mmc_csd.tran_speed & CSD_TRAN_SPEED_UNIT_MASK;
+	while (freq_unit != 0U) {
+		mmc_dev_info->max_bus_freq *= 10U;
+		--freq_unit;
+	}
+
+	mmc_dev_info->max_bus_freq *= 10000U;
+
+	return 0;
+}
+
+static int sdmmc_sd_switch(unsigned int mode, unsigned char group,
+		     unsigned char func)
+{
+	unsigned int group_shift = (group - 1U) * 4U;
+	unsigned int group_mask = GENMASK(group_shift + 3U,  group_shift);
+	unsigned int arg;
+	int ret;
+
+	ret = ops->prepare(0, (uintptr_t)&sd_switch_func_status,
+			   sizeof(sd_switch_func_status));
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* MMC CMD6: SWITCH_FUNC */
+	arg = mode | SD_SWITCH_ALL_GROUPS_MASK;
+	arg &= ~group_mask;
+	arg |= func << group_shift;
+	ret = sdmmc_send_cmd(MMC_CMD(6), arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return ops->read(0, (uintptr_t)&sd_switch_func_status,
+			 sizeof(sd_switch_func_status));
+}
+
+static int sdmmc_sd_send_op_cond(void)
+{
+	int n;
+	unsigned int resp_data[4];
+
+	for (n = 0; n < SEND_SDMMC_OP_COND_MAX_RETRIES; n++) {
+		int ret;
+
+		/* CMD55: Application Specific Command */
+		ret = sdmmc_send_cmd(MMC_CMD(55), 0, MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return ret;
+		}
+
+		/* ACMD41: SD_SEND_OP_COND */
+		ret = sdmmc_send_cmd(MMC_ACMD(41), OCR_HCS |
+			mmc_dev_info->ocr_voltage, MMC_RESPONSE_R3,
+			&resp_data[0]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((resp_data[0] & OCR_POWERUP) != 0U) {
+			mmc_ocr_value = resp_data[0];
+
+			if ((mmc_ocr_value & OCR_HCS) != 0U) {
+				mmc_dev_info->mmc_dev_type = MMC_IS_SD_HC;
+			} else {
+				mmc_dev_info->mmc_dev_type = MMC_IS_SD;
+			}
+
+			return 0;
+		}
+
+		mdelay(10);
+	}
+
+	ERROR("ACMD41 failed after %d retries\n", SEND_SDMMC_OP_COND_MAX_RETRIES);
+
+	return -EIO;
+}
+
+static int sdmmc_reset_to_idle(void)
+{
+	int ret;
+
+	/* CMD0: reset to IDLE */
+	ret = sdmmc_send_cmd(MMC_CMD(0), 0, 0, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	mdelay(2);
+
+	return 0;
+}
+
+static int sdmmc_send_op_cond(void)
+{
+	int ret, n;
+	unsigned int resp_data[4];
+
+	ret = sdmmc_reset_to_idle();
+	if (ret != 0) {
+		return ret;
+	}
+
+	for (n = 0; n < SEND_SDMMC_OP_COND_MAX_RETRIES; n++) {
+		ret = sdmmc_send_cmd(MMC_CMD(1), OCR_SECTOR_MODE |
+				   OCR_VDD_MIN_2V7 | OCR_VDD_MIN_1V7,
+				   MMC_RESPONSE_R3, &resp_data[0]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((resp_data[0] & OCR_POWERUP) != 0U) {
+			mmc_ocr_value = resp_data[0];
+			return 0;
+		}
+
+		mdelay(10);
+	}
+
+	ERROR("CMD1 failed after %d retries\n", SEND_SDMMC_OP_COND_MAX_RETRIES);
+
+	return -EIO;
+}
+
+static int sdmmc_enumerate(unsigned int clk, unsigned int bus_width)
+{
+	int ret;
+	unsigned int resp_data[4];
+
+	ops->init();
+
+	ret = sdmmc_reset_to_idle();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
+		ret = sdmmc_send_op_cond();
+	} else {
+		/* CMD8: Send Interface Condition Command */
+		ret = sdmmc_send_cmd(MMC_CMD(8), VHS_2_7_3_6_V | CMD8_CHECK_PATTERN,
+				   MMC_RESPONSE_R5, &resp_data[0]);
+
+		if ((ret == 0) && ((resp_data[0] & 0xffU) == CMD8_CHECK_PATTERN)) {
+			ret = sdmmc_sd_send_op_cond();
+		}
+	}
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* CMD2: Card Identification */
+	ret = sdmmc_send_cmd(MMC_CMD(2), 0, MMC_RESPONSE_R2, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* CMD3: Set Relative Address */
+	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
+		rca = MMC_FIX_RCA;
+		ret = sdmmc_send_cmd(MMC_CMD(3), rca << RCA_SHIFT_OFFSET,
+				   MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return ret;
+		}
+	} else {
+		ret = sdmmc_send_cmd(MMC_CMD(3), 0,
+				   MMC_RESPONSE_R6, &resp_data[0]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		rca = (resp_data[0] & 0xFFFF0000U) >> 16;
+	}
+
+	/* CMD9: CSD Register */
+	ret = sdmmc_send_cmd(MMC_CMD(9), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R2, &resp_data[0]);
+	if (ret != 0) {
+		return ret;
+	}
+
+	memcpy(&mmc_csd, &resp_data, sizeof(resp_data));
+
+	/* CMD7: Select Card */
+	ret = sdmmc_send_cmd(MMC_CMD(7), rca << RCA_SHIFT_OFFSET,
+			   MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return ret;
+		}
+	} while (ret != MMC_STATE_TRAN);
+
+	ret = sdmmc_set_ios(clk, bus_width);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = sdmmc_fill_device_info();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (is_sd_cmd6_enabled() &&
+	    (mmc_dev_info->mmc_dev_type == MMC_IS_SD_HC)) {
+		/* Try to switch to High Speed Mode */
+		ret = sdmmc_sd_switch(SD_SWITCH_FUNC_CHECK, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.support_g1 & BIT(9)) == 0U) {
+			/* High speed not supported, keep default speed */
+			return 0;
+		}
+
+		ret = sdmmc_sd_switch(SD_SWITCH_FUNC_SWITCH, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.sel_g2_g1 & 0x1U) == 0U) {
+			/* Cannot switch to high speed, keep default speed */
+			return 0;
+		}
+
+		mmc_dev_info->max_bus_freq = 50000000U;
+		ret = ops->set_ios(clk, bus_width);
+	}
+
+	return ret;
+}
+
+size_t sdmmc_read_blocks(int lba, uintptr_t buf, size_t size)
+{
+	int ret;
+	unsigned int cmd_idx, cmd_arg;
+
+	assert((ops != NULL) &&
+	       (ops->read != NULL) &&
+	       (size != 0U) &&
+	       ((size & MMC_BLOCK_MASK) == 0U));
+
+	ret = ops->prepare(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	if (is_cmd23_enabled()) {
+		/* Set block count */
+		ret = sdmmc_send_cmd(MMC_CMD(23), size / MMC_BLOCK_SIZE,
+				   MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+
+		cmd_idx = MMC_CMD(18);
+	} else {
+		if (size > MMC_BLOCK_SIZE) {
+			cmd_idx = MMC_CMD(18);
+		} else {
+			cmd_idx = MMC_CMD(17);
+		}
+	}
+
+	if (((mmc_ocr_value & OCR_ACCESS_MODE_MASK) == OCR_BYTE_MODE) &&
+	    (mmc_dev_info->mmc_dev_type != MMC_IS_SD_HC)) {
+		cmd_arg = lba * MMC_BLOCK_SIZE;
+	} else {
+		cmd_arg = lba;
+	}
+
+	ret = sdmmc_send_cmd(cmd_idx, cmd_arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return 0;
+	}
+
+	ret = ops->read(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	/* Wait buffer empty */
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return 0;
+		}
+	} while ((ret != MMC_STATE_TRAN) && (ret != MMC_STATE_DATA));
+
+	if (!is_cmd23_enabled() && (size > MMC_BLOCK_SIZE)) {
+		ret = sdmmc_send_cmd(MMC_CMD(12), 0, MMC_RESPONSE_R1B, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+	}
+
+	return size;
+}
+
+size_t sdmmc_write_blocks(int lba, const uintptr_t buf, size_t size)
+{
+	int ret;
+	unsigned int cmd_idx, cmd_arg;
+
+	assert((ops != NULL) &&
+	       (ops->write != NULL) &&
+	       (size != 0U) &&
+	       ((buf & MMC_BLOCK_MASK) == 0U) &&
+	       ((size & MMC_BLOCK_MASK) == 0U));
+
+	ret = ops->prepare(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	if (is_cmd23_enabled()) {
+		/* Set block count */
+		ret = sdmmc_send_cmd(MMC_CMD(23), size / MMC_BLOCK_SIZE,
+				   MMC_RESPONSE_R1, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+
+		cmd_idx = MMC_CMD(25);
+	} else {
+		if (size > MMC_BLOCK_SIZE) {
+			cmd_idx = MMC_CMD(25);
+		} else {
+			cmd_idx = MMC_CMD(24);
+		}
+	}
+
+	if ((mmc_ocr_value & OCR_ACCESS_MODE_MASK) == OCR_BYTE_MODE) {
+		cmd_arg = lba * MMC_BLOCK_SIZE;
+	} else {
+		cmd_arg = lba;
+	}
+
+	ret = sdmmc_send_cmd(cmd_idx, cmd_arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return 0;
+	}
+
+	ret = ops->write(lba, buf, size);
+	if (ret != 0) {
+		return 0;
+	}
+
+	/* Wait buffer empty */
+	do {
+		ret = sdmmc_device_state();
+		if (ret < 0) {
+			return 0;
+		}
+	} while ((ret != MMC_STATE_TRAN) && (ret != MMC_STATE_RCV));
+
+	if (!is_cmd23_enabled() && (size > MMC_BLOCK_SIZE)) {
+		ret = sdmmc_send_cmd(MMC_CMD(12), 0, MMC_RESPONSE_R1B, NULL);
+		if (ret != 0) {
+			return 0;
+		}
+	}
+
+	return size;
+}
+
+int sd_or_mmc_init(const struct mmc_ops *ops_ptr, unsigned int clk,
+	     unsigned int width, unsigned int flags,
+	     struct mmc_device_info *device_info)
+{
+	assert((ops_ptr != NULL) &&
+	       (ops_ptr->init != NULL) &&
+	       (ops_ptr->send_cmd != NULL) &&
+	       (ops_ptr->set_ios != NULL) &&
+	       (ops_ptr->prepare != NULL) &&
+	       (ops_ptr->read != NULL) &&
+	       (ops_ptr->write != NULL) &&
+	       (device_info != NULL) &&
+	       (clk != 0) &&
+	       ((width == MMC_BUS_WIDTH_1) ||
+		(width == MMC_BUS_WIDTH_4) ||
+		(width == MMC_BUS_WIDTH_8) ||
+		(width == MMC_BUS_WIDTH_DDR_4) ||
+		(width == MMC_BUS_WIDTH_DDR_8)));
+
+	ops = ops_ptr;
+	mmc_flags = flags;
+	mmc_dev_info = device_info;
+
+	return sdmmc_enumerate(clk, width);
+}
+
+int sdmmc_init(handoff *hoff_ptr, struct cdns_sdmmc_params *params, struct mmc_device_info *info)
+{
+	int result = 0;
+
+	/* SDMMC pin mux configuration */
+	sdmmc_pin_config();
+	cdns_set_sdmmc_var(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
+	result = cdns_sd_host_init(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
+	if (result < 0) {
+		return result;
+	}
+
+	assert((params != NULL) &&
+	       ((params->reg_base & MMC_BLOCK_MASK) == 0) &&
+	       ((params->desc_base & MMC_BLOCK_MASK) == 0) &&
+	       ((params->desc_size & MMC_BLOCK_MASK) == 0) &&
+		   ((params->reg_pinmux & MMC_BLOCK_MASK) == 0) &&
+		   ((params->reg_phy & MMC_BLOCK_MASK) == 0) &&
+	       (params->desc_size > 0) &&
+	       (params->clk_rate > 0) &&
+	       ((params->bus_width == MMC_BUS_WIDTH_1) ||
+		(params->bus_width == MMC_BUS_WIDTH_4) ||
+		(params->bus_width == MMC_BUS_WIDTH_8)));
+
+	memcpy(&cdns_params, params, sizeof(struct cdns_sdmmc_params));
+	cdns_params.cdn_sdmmc_dev_type = info->mmc_dev_type;
+	cdns_params.cdn_sdmmc_dev_mode = SD_DS;
+
+	result = sd_or_mmc_init(&cdns_sdmmc_ops, params->clk_rate, params->bus_width,
+		params->flags, info);
+
+	return result;
+}
diff --git a/plat/intel/soc/common/drivers/sdmmc/sdmmc.h b/plat/intel/soc/common/drivers/sdmmc/sdmmc.h
new file mode 100644
index 0000000..16c6b04
--- /dev/null
+++ b/plat/intel/soc/common/drivers/sdmmc/sdmmc.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SDMMC_H
+#define SDMMC_H
+
+#include <lib/mmio.h>
+#include "socfpga_handoff.h"
+
+#define PERIPHERAL_SDMMC_MASK			0x60
+#define PERIPHERAL_SDMMC_OFFSET			6
+
+#define DEFAULT_SDMMC_MAX_RETRIES		5
+#define SEND_SDMMC_OP_COND_MAX_RETRIES		100
+#define SDMMC_MULT_BY_512K_SHIFT		19
+
+static const unsigned char tran_speed_base[16] = {
+	0, 10, 12, 13, 15, 20, 26, 30, 35, 40, 45, 52, 55, 60, 70, 80
+};
+
+static const unsigned char sd_tran_speed_base[16] = {
+	0, 10, 12, 13, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 70, 80
+};
+
+
+/* FUNCTION DEFINATION */
+/*
+ * @brief SDMMC controller initialization function
+ *
+ * @hoff_ptr: Pointer to the hand-off data
+ * Return: 0 on success, a negative errno on failure
+ */
+int sdmmc_init(handoff *hoff_ptr, struct cdns_sdmmc_params *params,
+	     struct mmc_device_info *info);
+int sd_or_mmc_init(const struct mmc_ops *ops_ptr, unsigned int clk,
+	     unsigned int width, unsigned int flags,
+	     struct mmc_device_info *device_info);
+void sdmmc_pin_config(void);
+#endif
diff --git a/plat/intel/soc/common/drivers/wdt/watchdog.h b/plat/intel/soc/common/drivers/wdt/watchdog.h
index 2c72463..4ee4cff 100644
--- a/plat/intel/soc/common/drivers/wdt/watchdog.h
+++ b/plat/intel/soc/common/drivers/wdt/watchdog.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,7 +7,11 @@
 #ifndef CAD_WATCHDOG_H
 #define CAD_WATCHDOG_H
 
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+#define WDT_BASE			(0x10D00200)
+#else
 #define WDT_BASE			(0xFFD00200)
+#endif
 #define WDT_REG_SIZE_OFFSET		(0x4)
 #define WDT_MIN_CYCLES			(65536)
 #define WDT_PERIOD			(20)
diff --git a/plat/intel/soc/common/include/platform_def.h b/plat/intel/soc/common/include/platform_def.h
index 78deebc..49fc567 100644
--- a/plat/intel/soc/common/include/platform_def.h
+++ b/plat/intel/soc/common/include/platform_def.h
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -25,12 +25,6 @@
 /* sysmgr.boot_scratch_cold4 & 5 used for CPU release address for SPL */
 #define PLAT_CPU_RELEASE_ADDR			0xffd12210
 
-/*
- * sysmgr.boot_scratch_cold6 & 7 (64bit) are used to indicate L2 reset
- * is done and HPS should trigger warm reset via RMR_EL3.
- */
-#define L2_RESET_DONE_REG			0xFFD12218
-
 /* Magic word to indicate L2 reset is completed */
 #define L2_RESET_DONE_STATUS			0x1228E5E7
 
diff --git a/plat/intel/soc/common/include/socfpga_f2sdram_manager.h b/plat/intel/soc/common/include/socfpga_f2sdram_manager.h
index b30a11e..1bebfc9 100644
--- a/plat/intel/soc/common/include/socfpga_f2sdram_manager.h
+++ b/plat/intel/soc/common/include/socfpga_f2sdram_manager.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,7 @@
 #define SOCFPGA_F2SDRAMMGR_SIDEBANDMGR_FLAGOUTCLR0	0x54
 #define SOCFPGA_F2SDRAMMGR_SIDEBANDMGR_FLAGOUTSET0	0x50
 
+#define FLAGOUTCLR0_F2SDRAM0_ENABLE		(BIT(8))
 #define FLAGOUTSETCLR_F2SDRAM0_ENABLE		(BIT(1))
 #define FLAGOUTSETCLR_F2SDRAM1_ENABLE		(BIT(4))
 #define FLAGOUTSETCLR_F2SDRAM2_ENABLE		(BIT(7))
diff --git a/plat/intel/soc/common/include/socfpga_handoff.h b/plat/intel/soc/common/include/socfpga_handoff.h
index ba0f7f3..b2913c7 100644
--- a/plat/intel/soc/common/include/socfpga_handoff.h
+++ b/plat/intel/soc/common/include/socfpga_handoff.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,13 +7,15 @@
 #ifndef	HANDOFF_H
 #define	HANDOFF_H
 
-#define HANDOFF_MAGIC_HEADER		0x424f4f54	/* BOOT */
-#define HANDOFF_MAGIC_PINMUX_SEL	0x504d5558	/* PMUX */
-#define HANDOFF_MAGIC_IOCTLR		0x494f4354	/* IOCT */
-#define HANDOFF_MAGIC_FPGA		0x46504741	/* FPGA */
-#define HANDOFF_MAGIC_IODELAY		0x444c4159	/* DLAY */
-#define HANDOFF_MAGIC_CLOCK		0x434c4b53	/* CLKS */
-#define HANDOFF_MAGIC_MISC		0x4d495343	/* MISC */
+#define HANDOFF_MAGIC_HEADER			0x424f4f54	/* BOOT */
+#define HANDOFF_MAGIC_PINMUX_SEL		0x504d5558	/* PMUX */
+#define HANDOFF_MAGIC_IOCTLR			0x494f4354	/* IOCT */
+#define HANDOFF_MAGIC_FPGA				0x46504741	/* FPGA */
+#define HANDOFF_MAGIC_IODELAY			0x444c4159	/* DLAY */
+#define HANDOFF_MAGIC_CLOCK				0x434c4b53	/* CLKS */
+#define HANDOFF_MAGIC_MISC				0x4d495343	/* MISC */
+#define HANDOFF_MAGIC_PERIPHERAL		0x50455249	/* PERIPHERAL */
+#define HANDOFF_MAGIC_DDR				0x5344524d	/* DDR */
 
 #include <socfpga_plat_def.h>
 
@@ -39,8 +41,9 @@
 	uint32_t	pinmux_fpga_magic;
 	uint32_t	pinmux_fpga_length;
 	uint32_t	_pad_0x338_0x340[2];
-	uint32_t	pinmux_fpga_array[42];	/* offset, value */
-	uint32_t	_pad_0x3e8_0x3f0[2];
+	uint32_t	pinmux_fpga_array[44];	/* offset, value */
+	/* TODO: Temp remove due to add in extra handoff data */
+	// uint32_t	_pad_0x3e8_0x3f0[2];
 
 	/* pinmux configuration - io delay */
 	uint32_t	pinmux_delay_magic;
@@ -49,7 +52,6 @@
 	uint32_t	pinmux_iodelay_array[96];	/* offset, value */
 
 	/* clock configuration */
-
 #if PLATFORM_MODEL == PLAT_SOCFPGA_STRATIX10
 	uint32_t	clock_magic;
 	uint32_t	clock_length;
@@ -120,16 +122,68 @@
 	uint32_t	hps_osc_clk_h;
 	uint32_t	fpga_clk_hz;
 	uint32_t	_pad_0x604_0x610[3];
+#elif PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	uint32_t	clock_magic;
+	uint32_t	clock_length;
+	uint32_t	_pad_0x588_0x590[2];
+	uint32_t	main_pll_nocclk;
+	uint32_t	main_pll_nocdiv;
+	uint32_t	main_pll_pllglob;
+	uint32_t	main_pll_fdbck;
+	uint32_t	main_pll_pllc0;
+	uint32_t	main_pll_pllc1;
+	uint32_t	main_pll_pllc2;
+	uint32_t	main_pll_pllc3;
+	uint32_t	main_pll_pllm;
+	uint32_t	per_pll_emacctl;
+	uint32_t	per_pll_gpiodiv;
+	uint32_t	per_pll_pllglob;
+	uint32_t	per_pll_fdbck;
+	uint32_t	per_pll_pllc0;
+	uint32_t	per_pll_pllc1;
+	uint32_t	per_pll_pllc2;
+	uint32_t	per_pll_pllc3;
+	uint32_t	per_pll_pllm;
+	uint32_t	alt_emacactr;
+	uint32_t	alt_emacbctr;
+	uint32_t	alt_emacptpctr;
+	uint32_t	alt_gpiodbctr;
+	uint32_t	alt_sdmmcctr;
+	uint32_t	alt_s2fuser0ctr;
+	uint32_t	alt_s2fuser1ctr;
+	uint32_t	alt_psirefctr;
+	/* TODO: Temp added for clk manager. */
+	uint32_t	qspi_clk_khz;
+	uint32_t	hps_osc_clk_hz;
+	uint32_t	fpga_clk_hz;
+	/* TODO: Temp added for clk manager. */
+	uint32_t	ddr_reset_type;
+	/* TODO: Temp added for clk manager. */
+	uint32_t	hps_status_coldreset;
+	/* TODO: Temp remove due to add in extra handoff data */
+	//uint32_t	_pad_0x604_0x610[3];
 #endif
 	/* misc configuration */
 	uint32_t	misc_magic;
 	uint32_t	misc_length;
 	uint32_t	_pad_0x618_0x620[2];
+
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	/* peripheral configuration - select */
+	uint32_t	peripheral_pwr_gate_magic;
+	uint32_t	peripheral_pwr_gate_length;
+	uint32_t	_pad_0x08_0x0C[2];
+	uint32_t	peripheral_pwr_gate_array;	/* offset, value */
+
+	/* ddr configuration - select */
+	uint32_t	ddr_magic;
+	uint32_t	ddr_length;
+	uint32_t	_pad_0x1C_0x20[2];
+	uint32_t	ddr_array[4];	/* offset, value */
+#endif
 } handoff;
 
 int verify_handoff_image(handoff *hoff_ptr, handoff *reverse_hoff_ptr);
 int socfpga_get_handoff(handoff *hoff_ptr);
 
 #endif
-
-
diff --git a/plat/intel/soc/common/include/socfpga_mailbox.h b/plat/intel/soc/common/include/socfpga_mailbox.h
index 564b4ee..77d3af9 100644
--- a/plat/intel/soc/common/include/socfpga_mailbox.h
+++ b/plat/intel/soc/common/include/socfpga_mailbox.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,8 +9,11 @@
 
 #include <lib/utils_def.h>
 
-
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+#define MBOX_OFFSET					0x10a30000
+#else
 #define MBOX_OFFSET					0xffa30000
+#endif
 
 #define MBOX_ATF_CLIENT_ID				0x1U
 #define MBOX_MAX_JOB_ID					0xFU
@@ -193,9 +196,9 @@
 #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
+#define CONFIG_STATUS_WORD_SIZE			16U
+#define CONFIG_STATUS_FW_VER_OFFSET		1
+#define CONFIG_STATUS_FW_VER_MASK		0x00FFFFFF
 
 /* Data structure */
 
@@ -233,6 +236,7 @@
 			unsigned int *resp_len);
 
 void mailbox_reset_cold(void);
+void mailbox_reset_warm(uint32_t reset_type);
 void mailbox_clear_response(void);
 
 int intel_mailbox_get_config_status(uint32_t cmd, bool init_done);
diff --git a/plat/intel/soc/common/include/socfpga_noc.h b/plat/intel/soc/common/include/socfpga_noc.h
index e3c0f73..3fc3f81 100644
--- a/plat/intel/soc/common/include/socfpga_noc.h
+++ b/plat/intel/soc/common/include/socfpga_noc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2020-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -74,6 +74,10 @@
 #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_WATCHDOG4			0x007c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_PWRMGR			0x0080
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB1_RXECC			0x0084
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB1_TXECC			0x0088
 #define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_PROBES			0x0090
 #define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_QOS			0x0094
 
diff --git a/plat/intel/soc/common/include/socfpga_reset_manager.h b/plat/intel/soc/common/include/socfpga_reset_manager.h
index cce16ab..9d06a3d 100644
--- a/plat/intel/soc/common/include/socfpga_reset_manager.h
+++ b/plat/intel/soc/common/include/socfpga_reset_manager.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,10 @@
 
 #include "socfpga_plat_def.h"
 
+/* Status Response */
+#define RSTMGR_RET_OK				0
+#define RSTMGR_RET_ERROR			-1
+
 #define SOCFPGA_BRIDGE_ENABLE			BIT(0)
 #define SOCFPGA_BRIDGE_HAS_MASK			BIT(1)
 
@@ -22,28 +26,51 @@
 /* Register Mapping */
 
 #define SOCFPGA_RSTMGR_STAT			0x000
+#define SOCFPGA_RSTMGR_MISCSTAT			0x008
 #define SOCFPGA_RSTMGR_HDSKEN			0x010
 #define SOCFPGA_RSTMGR_HDSKREQ			0x014
 #define SOCFPGA_RSTMGR_HDSKACK			0x018
+#define SOCFPGA_RSTMGR_HDSKSTALL		0x01C
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 #define SOCFPGA_RSTMGR_MPUMODRST		0x020
+#endif
 #define SOCFPGA_RSTMGR_PER0MODRST		0x024
 #define SOCFPGA_RSTMGR_PER1MODRST		0x028
-#define SOCFPGA_RSTMGR_BRGMODRST		0x02c
+#define SOCFPGA_RSTMGR_BRGMODRST		0x02C
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 #define SOCFPGA_RSTMGR_COLDMODRST		0x034
+#endif
+#define SOCFPGA_RSTMGR_DBGMODRST		0x03C
+#define SOCFPGA_RSTMGR_BRGWARMMASK		0x04C
+#define SOCFPGA_RSTMGR_TSTSTA			0x05C
 #define SOCFPGA_RSTMGR_HDSKTIMEOUT		0x064
+#define SOCFPGA_RSTMGR_DBGHDSKTIMEOUT		0x06C
+#define SOCFPGA_RSTMGR_DBGRSTCMPLT		0x070
+#define SOCFPGA_RSTMGR_HPSRSTCMPLT		0x080
+#define SOCFPGA_RSTMGR_CPUINREST		0x090
+#define SOCFPGA_RSTMGR_CPURSTRELEASE		0x094
+#define SOCFPGA_RSTMGR_CPUBASELOW_0		0x098
+#define SOCFPGA_RSTMGR_CPUBASEHIGH_0		0x09C
+#define SOCFPGA_RSTMGR_CPUBASELOW_1		0x0A0
+#define SOCFPGA_RSTMGR_CPUBASEHIGH_1		0x0A4
+#define SOCFPGA_RSTMGR_CPUBASELOW_2		0x0A8
+#define SOCFPGA_RSTMGR_CPUBASEHIGH_2		0x0AC
+#define SOCFPGA_RSTMGR_CPUBASELOW_3		0x0B0
+#define SOCFPGA_RSTMGR_CPUBASEHIGH_3		0x0B4
 
 /* Field Mapping */
-
-#define RSTMGR_PER0MODRST_EMAC0			0x00000001
-#define RSTMGR_PER0MODRST_EMAC1			0x00000002
-#define RSTMGR_PER0MODRST_EMAC2			0x00000004
+/* PER0MODRST */
+#define RSTMGR_PER0MODRST_EMAC0			0x00000001	//TSN0
+#define RSTMGR_PER0MODRST_EMAC1			0x00000002	//TSN1
+#define RSTMGR_PER0MODRST_EMAC2			0x00000004	//TSN2
 #define RSTMGR_PER0MODRST_USB0			0x00000008
 #define RSTMGR_PER0MODRST_USB1			0x00000010
 #define RSTMGR_PER0MODRST_NAND			0x00000020
+#define RSTMGR_PER0MODRST_SOFTPHY		0x00000040
 #define RSTMGR_PER0MODRST_SDMMC			0x00000080
-#define RSTMGR_PER0MODRST_EMAC0OCP		0x00000100
-#define RSTMGR_PER0MODRST_EMAC1OCP		0x00000200
-#define RSTMGR_PER0MODRST_EMAC2OCP		0x00000400
+#define RSTMGR_PER0MODRST_EMAC0OCP		0x00000100	//TSN0ECC
+#define RSTMGR_PER0MODRST_EMAC1OCP		0x00000200	//TSN1ECC
+#define RSTMGR_PER0MODRST_EMAC2OCP		0x00000400	//TSN2ECC
 #define RSTMGR_PER0MODRST_USB0OCP		0x00000800
 #define RSTMGR_PER0MODRST_USB1OCP		0x00001000
 #define RSTMGR_PER0MODRST_NANDOCP		0x00002000
@@ -64,6 +91,7 @@
 #define RSTMGR_PER0MODRST_DMAIF6		0x40000000
 #define RSTMGR_PER0MODRST_DMAIF7		0x80000000
 
+/* PER1MODRST */
 #define RSTMGR_PER1MODRST_WATCHDOG0		0x00000001
 #define RSTMGR_PER1MODRST_WATCHDOG1		0x00000002
 #define RSTMGR_PER1MODRST_WATCHDOG2		0x00000004
@@ -77,31 +105,121 @@
 #define RSTMGR_PER1MODRST_I2C2			0x00000400
 #define RSTMGR_PER1MODRST_I2C3			0x00000800
 #define RSTMGR_PER1MODRST_I2C4			0x00001000
+#define RSTMGR_PER1MODRST_I3C0			0x00002000
+#define RSTMGR_PER1MODRST_I3C1			0x00004000
 #define RSTMGR_PER1MODRST_UART0			0x00010000
 #define RSTMGR_PER1MODRST_UART1			0x00020000
 #define RSTMGR_PER1MODRST_GPIO0			0x01000000
 #define RSTMGR_PER1MODRST_GPIO1			0x02000000
+#define RSTMGR_PER1MODRST_WATCHDOG4		0x04000000
 
+/* HDSKEN */
+#define RSTMGR_HDSKEN_EMIF_FLUSH		0x00000001
 #define RSTMGR_HDSKEN_FPGAHSEN			0x00000004
 #define RSTMGR_HDSKEN_ETRSTALLEN		0x00000008
-#define RSTMGR_HDSKEN_L2FLUSHEN			0x00000100
+#define RSTMGR_HDSKEN_LWS2F_FLUSH		0x00000200
+#define RSTMGR_HDSKEN_S2F_FLUSH			0x00000400
+#define RSTMGR_HDSKEN_F2SDRAM_FLUSH		0x00000800
+#define RSTMGR_HDSKEN_F2S_FLUSH			0x00001000
 #define RSTMGR_HDSKEN_L3NOC_DBG			0x00010000
 #define RSTMGR_HDSKEN_DEBUG_L3NOC		0x00020000
-#define RSTMGR_HDSKEN_SDRSELFREFEN		0x00000001
 
-#define RSTMGR_HDSKEQ_FPGAHSREQ			0x4
+/* HDSKREQ */
+#define RSTMGR_HDSKREQ_EMIFFLUSHREQ		0x00000001
+#define RSTMGR_HDSKREQ_ETRSTALLREQ		0x00000008
+#define RSTMGR_HDSKREQ_LWS2F_FLUSH		0x00000200
+#define RSTMGR_HDSKREQ_S2F_FLUSH		0x00000400
+#define RSTMGR_HDSKREQ_F2SDRAM_FLUSH		0x00000800
+#define RSTMGR_HDSKREQ_F2S_FLUSH		0x00001000
+#define RSTMGR_HDSKREQ_L3NOC_DBG		0x00010000
+#define RSTMGR_HDSKREQ_DEBUG_L3NOC		0x00020000
+#define RSTMGR_HDSKREQ_FPGAHSREQ		0x00000004
+#define RSTMGR_HDSKREQ_LWSOC2FPGAREQ		0x00000200
+#define RSTMGR_HDSKREQ_SOC2FPGAREQ		0x00000400
+#define RSTMGR_HDSKREQ_F2SDRAM0REQ		0x00000800
+#define RSTMGR_HDSKREQ_FPGA2SOCREQ		0x00001000
 
-#define RSTMGR_BRGMODRST_SOC2FPGA		0x1
-#define RSTMGR_BRGMODRST_LWHPS2FPGA		0x2
-#define RSTMGR_BRGMODRST_FPGA2SOC		0x4
-#define RSTMGR_BRGMODRST_F2SSDRAM0		0x8
+/* HDSKACK */
+#define RSTMGR_HDSKACK_EMIFFLUSHREQ		0x00000001
+#define RSTMGR_HDSKACK_FPGAHSREQ		0x00000004
+#define RSTMGR_HDSKACK_ETRSTALLREQ		0x00000008
+#define RSTMGR_HDSKACK_LWS2F_FLUSH		0x00000200
+#define RSTMGR_HDSKACK_S2F_FLUSH		0x00000400
+#define RSTMGR_HDSKACK_F2SDRAM_FLUSH		0x00000800
+#define RSTMGR_HDSKACK_F2S_FLUSH		0x00001000
+#define RSTMGR_HDSKACK_L3NOC_DBG		0x00010000
+#define RSTMGR_HDSKACK_DEBUG_L3NOC		0x00020000
+#define RSTMGR_HDSKACK_FPGAHSACK		0x00000004
+#define RSTMGR_HDSKACK_LWSOC2FPGAACK		0x00000200
+#define RSTMGR_HDSKACK_SOC2FPGAACK		0x00000400
+#define RSTMGR_HDSKACK_F2SDRAM0ACK		0x00000800
+#define RSTMGR_HDSKACK_FPGA2SOCACK		0x00001000
+#define RSTMGR_HDSKACK_FPGAHSACK_DASRT		0x00000000
+#define RSTMGR_HDSKACK_F2SDRAM0ACK_DASRT	0x00000000
+#define RSTMGR_HDSKACK_FPGA2SOCACK_DASRT	0x00000000
+
+/* HDSKSTALL */
+#define RSTMGR_HDSKACK_ETRSTALLWARMRST		0x00000001
+
+/* BRGMODRST */
+#define RSTMGR_BRGMODRST_SOC2FPGA		0x00000001
+#define RSTMGR_BRGMODRST_LWHPS2FPGA		0x00000002
+#define RSTMGR_BRGMODRST_FPGA2SOC		0x00000004
+#define RSTMGR_BRGMODRST_F2SSDRAM0		0x00000008
+#if PLATFORM_MODEL == PLAT_SOCFPGA_STRATIX10
 #define RSTMGR_BRGMODRST_F2SSDRAM1		0x10
 #define RSTMGR_BRGMODRST_F2SSDRAM2		0x20
-#define RSTMGR_BRGMODRST_MPFE			0x40
 #define RSTMGR_BRGMODRST_DDRSCH			0x40
+#elif PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+#define RSTMGR_BRGMODRST_F2SSDRAM1		0x10
+#define RSTMGR_BRGMODRST_F2SSDRAM2		0x20
+#endif
+
+#define RSTMGR_BRGMODRST_MPFE			0x40
+
+/* DBGMODRST */
+#define RSTMGR_DBGMODRST_DBG_RST		0x00000001
 
-#define RSTMGR_HDSKREQ_FPGAHSREQ		(BIT(2))
-#define RSTMGR_HDSKACK_FPGAHSACK_MASK		(BIT(2))
+/* BRGMODRSTMASK */
+#define RSTMGR_BRGMODRSTMASK_SOC2FPGA		0x00000001
+#define RSTMGR_BRGMODRSTMASK_LWHPS2FPGA		0x00000002
+#define RSTMGR_BRGMODRSTMASK_FPGA2SOC		0x00000004
+#define RSTMGR_BRGMODRSTMASK_F2SDRAM0		0x00000008
+#define RSTMGR_BRGMODRSTMASK_MPFE		0x00000040
+
+/* TSTSTA */
+#define RSTMGR_TSTSTA_RSTST			0x0000001F
+
+/* HDSKTIMEOUT */
+#define RSTMGR_HDSKTIMEOUT_VAL			0xFFFFFFFF
+
+/* DBGHDSKTIMEOUT */
+#define RSTMGR_DBGHDSKTIMEOUT_VAL		0xFFFFFFFF
+
+/* DBGRSTCMPLT */
+#define RSTMGR_DBGRSTCMPLT_VAL			0xFFFFFFFF
+
+/* HPSRSTCMPLT */
+#define RSTMGR_DBGRSTCMPLT_VAL			0xFFFFFFFF
+
+/* CPUINRESET */
+#define RSTMGR_CPUINRESET_CPU0			0x00000001
+#define RSTMGR_CPUINRESET_CPU1			0x00000002
+#define RSTMGR_CPUINRESET_CPU2			0x00000004
+#define RSTMGR_CPUINRESET_CPU3			0x00000008
+
+/* CPUSTRELEASE */
+#define RSTMGR_CPUSTRELEASE_CPUx		0x10D11094
+
+/* CPUxRESETBASE */
+#define RSTMGR_CPUxRESETBASELOW_CPU0		0x10D11098
+#define RSTMGR_CPUxRESETBASEHIGH_CPU0		0x10D1109C
+#define RSTMGR_CPUxRESETBASELOW_CPU1		0x10D110A0
+#define RSTMGR_CPUxRESETBASEHIGH_CPU1		0x10D110A4
+#define RSTMGR_CPUxRESETBASELOW_CPU2		0x10D110A8
+#define RSTMGR_CPUxRESETBASEHIGH_CPU2		0x10D110AC
+#define RSTMGR_CPUxRESETBASELOW_CPU3		0x10D110B0
+#define RSTMGR_CPUxRESETBASEHIGH_CPU3		0x10D110B4
 
 /* Definitions */
 
@@ -109,17 +227,28 @@
 #define RSTMGR_HDSKEN_SET			0x010D
 
 /* Macros */
+#define SOCFPGA_RSTMGR(_reg)			(SOCFPGA_RSTMGR_REG_BASE + (SOCFPGA_RSTMGR_##_reg))
+#define RSTMGR_FIELD(_reg, _field)		(RSTMGR_##_reg##MODRST_##_field)
 
-#define SOCFPGA_RSTMGR(_reg)		(SOCFPGA_RSTMGR_REG_BASE \
-					+ (SOCFPGA_RSTMGR_##_reg))
-#define RSTMGR_FIELD(_reg, _field)	(RSTMGR_##_reg##MODRST_##_field)
+/* Reset type to SDM from PSCI */
+// Temp add macro here for reset type
+#define SOCFPGA_RESET_TYPE_COLD			0
+#define SOCFPGA_RESET_TYPE_WARM			1
 
 /* Function Declarations */
 
 void deassert_peripheral_reset(void);
 void config_hps_hs_before_warm_reset(void);
 
+int socfpga_bridges_reset(uint32_t mask);
 int socfpga_bridges_enable(uint32_t mask);
 int socfpga_bridges_disable(uint32_t mask);
 
+int socfpga_cpurstrelease(unsigned int cpu_id);
+int socfpga_cpu_reset_base(unsigned int cpu_id);
+
+/* SMP: Func proto */
+void bl31_plat_set_secondary_cpu_entrypoint(unsigned int cpu_id);
+void bl31_plat_set_secondary_cpu_off(void);
+
 #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 6708edb..0668301 100644
--- a/plat/intel/soc/common/include/socfpga_sip_svc.h
+++ b/plat/intel/soc/common/include/socfpga_sip_svc.h
@@ -167,6 +167,7 @@
 #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
+#define INTEL_SIP_SMC_V2_RSU_UPDATE_ADDR			0xC2000405
 
 /* V2: Mailbox function identifier */
 #define INTEL_SIP_SMC_V2_MAILBOX_SEND_COMMAND			0xC2000420
@@ -185,7 +186,7 @@
 /*
  * Increase if there is new SMC function ID being added
  */
-#define SIP_SVC_VERSION_MINOR					1
+#define SIP_SVC_VERSION_MINOR					2
 
 
 /* Structure Definitions */
@@ -219,6 +220,9 @@
 uint32_t intel_secure_reg_update(uint64_t reg_addr, uint32_t mask,
 				 uint32_t val, uint32_t *retval);
 
+/* Set RSU update address*/
+uint32_t intel_rsu_update(uint64_t update_address);
+
 /* Miscellaneous HPS services */
 uint32_t intel_hps_set_bridges(uint64_t enable, uint64_t mask);
 
diff --git a/plat/intel/soc/common/include/socfpga_system_manager.h b/plat/intel/soc/common/include/socfpga_system_manager.h
index 8d9ba70..f860f57 100644
--- a/plat/intel/soc/common/include/socfpga_system_manager.h
+++ b/plat/intel/soc/common/include/socfpga_system_manager.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/intel/soc/common/include/socfpga_vab.h b/plat/intel/soc/common/include/socfpga_vab.h
new file mode 100644
index 0000000..f6081df
--- /dev/null
+++ b/plat/intel/soc/common/include/socfpga_vab.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2020-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SOCFPGA_VAB_H
+#define SOCFPGA_VAB_H
+
+
+#include <stdlib.h>
+#include "socfpga_fcs.h"
+
+struct fcs_hps_vab_certificate_data {
+	uint32_t vab_cert_magic_num;			/* offset 0x10 */
+	uint32_t flags;
+	uint8_t rsvd0_1[8];
+	uint8_t fcs_sha384[FCS_SHA384_WORD_SIZE];	/* offset 0x20 */
+};
+
+struct fcs_hps_vab_certificate_header {
+	uint32_t cert_magic_num;			/* offset 0 */
+	uint32_t cert_data_sz;
+	uint32_t cert_ver;
+	uint32_t cert_type;
+	struct fcs_hps_vab_certificate_data d;		/* offset 0x10 */
+	/* keychain starts at offset 0x50 */
+};
+
+/* Macros */
+#define IS_BYTE_ALIGNED(x, a)		(((x) & ((typeof(x))(a) - 1)) == 0)
+#define BYTE_ALIGN(x, a)		__ALIGN_MASK((x), (typeof(x))(a)-1)
+#define __ALIGN_MASK(x, mask)		(((x)+(mask))&~(mask))
+#define VAB_CERT_HEADER_SIZE		sizeof(struct fcs_hps_vab_certificate_header)
+#define VAB_CERT_MAGIC_OFFSET		offsetof(struct fcs_hps_vab_certificate_header, d)
+#define VAB_CERT_FIT_SHA384_OFFSET	offsetof(struct fcs_hps_vab_certificate_data, fcs_sha384[0])
+#define SDM_CERT_MAGIC_NUM		0x25D04E7F
+#define CHUNKSZ_PER_WD_RESET		(256 * 1024)
+
+/* SHA related return Macro */
+#define ENOVABIMG		1 /* VAB certificate not available */
+#define EIMGERR		2 /* Image format/size not valid */
+#define ETIMEOUT		3 /* Execution timeout */
+#define EPROCESS		4 /* Process error */
+#define EKEYREJECTED		5/* Key was rejected by service */
+
+/* Function Definitions */
+static size_t get_img_size(uint8_t *img_buf, size_t img_buf_sz);
+int socfpga_vendor_authentication(void **p_image, size_t *p_size);
+static uint32_t get_unaligned_le32(const void *p);
+void sha384_csum_wd(const unsigned char *input, unsigned int ilen,
+unsigned char *output, unsigned int chunk_sz);
+
+#endif
diff --git a/plat/intel/soc/common/soc/socfpga_emac.c b/plat/intel/soc/common/soc/socfpga_emac.c
index 02ff89e..49c92c3 100644
--- a/plat/intel/soc/common/soc/socfpga_emac.c
+++ b/plat/intel/soc/common/soc/socfpga_emac.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2023, Intel Corporation. All rights reserved.
+ * Copyright (c) 2020-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/intel/soc/common/soc/socfpga_firewall.c b/plat/intel/soc/common/soc/socfpga_firewall.c
index fc3889c..6247df3 100644
--- a/plat/intel/soc/common/soc/socfpga_firewall.c
+++ b/plat/intel/soc/common/soc/socfpga_firewall.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,7 +20,11 @@
 void enable_ns_peripheral_access(void)
 {
 	mmio_write_32(SOCFPGA_L4_PER_SCR(NAND_REGISTER), DISABLE_L4_FIREWALL);
+#if	((PLATFORM_MODEL == PLAT_SOCFPGA_STRATIX10) || \
+	(PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX) || \
+	(PLATFORM_MODEL == PLAT_SOCFPGA_N5X))
 	mmio_write_32(SOCFPGA_L4_PER_SCR(NAND_DATA), DISABLE_L4_FIREWALL);
+#endif
 
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(NAND_ECC), DISABLE_L4_FIREWALL);
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(NAND_READ_ECC), DISABLE_L4_FIREWALL);
@@ -87,9 +91,19 @@
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(WATCHDOG1), DISABLE_L4_FIREWALL);
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(WATCHDOG2), DISABLE_L4_FIREWALL);
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(WATCHDOG3), DISABLE_L4_FIREWALL);
+#if	PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	mmio_write_32(SOCFPGA_L4_SYS_SCR(WATCHDOG4), DISABLE_L4_FIREWALL);
+#endif
 
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(DAP), DISABLE_L4_FIREWALL);
 
+#if	PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	mmio_write_32(SOCFPGA_L4_SYS_SCR(PWRMGR), DISABLE_L4_FIREWALL);
+
+	mmio_write_32(SOCFPGA_L4_SYS_SCR(USB1_RXECC), DISABLE_L4_FIREWALL);
+	mmio_write_32(SOCFPGA_L4_SYS_SCR(USB1_TXECC), DISABLE_L4_FIREWALL);
+#endif
+
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(L4_NOC_PROBES), DISABLE_L4_FIREWALL);
 
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(L4_NOC_QOS), DISABLE_L4_FIREWALL);
diff --git a/plat/intel/soc/common/soc/socfpga_handoff.c b/plat/intel/soc/common/soc/socfpga_handoff.c
index a3146b4..526c6e1 100644
--- a/plat/intel/soc/common/soc/socfpga_handoff.c
+++ b/plat/intel/soc/common/soc/socfpga_handoff.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,16 +29,34 @@
 	for (i = 0; i < sizeof(handoff) / 4; i++)
 		buffer[i] = SWAP_UINT32(buffer[i]);
 
-	if (reverse_hoff_ptr->header_magic != HANDOFF_MAGIC_HEADER)
+	if (reverse_hoff_ptr->header_magic != HANDOFF_MAGIC_HEADER) {
 		return -1;
-	if (reverse_hoff_ptr->pinmux_sel_magic != HANDOFF_MAGIC_PINMUX_SEL)
+	}
+	if (reverse_hoff_ptr->pinmux_sel_magic != HANDOFF_MAGIC_PINMUX_SEL) {
 		return -1;
-	if (reverse_hoff_ptr->pinmux_io_magic != HANDOFF_MAGIC_IOCTLR)
+	}
+	if (reverse_hoff_ptr->pinmux_io_magic != HANDOFF_MAGIC_IOCTLR) {
 		return -1;
-	if (reverse_hoff_ptr->pinmux_fpga_magic != HANDOFF_MAGIC_FPGA)
+	}
+	if (reverse_hoff_ptr->pinmux_fpga_magic != HANDOFF_MAGIC_FPGA) {
 		return -1;
-	if (reverse_hoff_ptr->pinmux_delay_magic != HANDOFF_MAGIC_IODELAY)
+	}
+	if (reverse_hoff_ptr->pinmux_delay_magic != HANDOFF_MAGIC_IODELAY) {
 		return -1;
+	}
+	if (reverse_hoff_ptr->clock_magic != HANDOFF_MAGIC_CLOCK) {
+		return -1;
+	}
+
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	if (reverse_hoff_ptr->peripheral_pwr_gate_magic != HANDOFF_MAGIC_PERIPHERAL) {
+		return -1;
+	}
+
+	if (reverse_hoff_ptr->ddr_magic != HANDOFF_MAGIC_DDR) {
+		return -1;
+	}
+#endif
 
 	return 0;
 }
diff --git a/plat/intel/soc/common/soc/socfpga_mailbox.c b/plat/intel/soc/common/soc/socfpga_mailbox.c
index 525ac2b..d93fc8a 100644
--- a/plat/intel/soc/common/soc/socfpga_mailbox.c
+++ b/plat/intel/soc/common/soc/socfpga_mailbox.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2023, Intel Corporation. All rights reserved.
+ * Copyright (c) 2020-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -523,10 +523,20 @@
 void mailbox_reset_cold(void)
 {
 	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
-	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_REBOOT_HPS, NULL, 0U,
-				CMD_CASUAL, NULL, NULL);
+
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_REBOOT_HPS, 0U, 0U,
+				 CMD_CASUAL, NULL, NULL);
 }
 
+void mailbox_reset_warm(uint32_t reset_type)
+{
+	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
+
+	reset_type = 0x01; // Warm reset header data must be 1
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_REBOOT_HPS, &reset_type, 1U,
+				 CMD_CASUAL, NULL, NULL);
+}
+
 int mailbox_rsu_get_spt_offset(uint32_t *resp_buf, unsigned int resp_buf_len)
 {
 	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_GET_SUBPARTITION_TABLE,
@@ -679,9 +689,10 @@
 				&resp_len);
 }
 
-int mailbox_seu_err_status(uint32_t *resp_buf, uint32_t resp_buf_len)
+int mailbox_seu_err_status(uint32_t *resp_buf, unsigned int resp_buf_len)
 {
+
 	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_SEU_ERR_READ, NULL, 0U,
 				CMD_CASUAL, resp_buf,
-				&resp_buf_len);;
+				&resp_buf_len);
 }
diff --git a/plat/intel/soc/common/soc/socfpga_reset_manager.c b/plat/intel/soc/common/soc/socfpga_reset_manager.c
index a546638..7db86c7 100644
--- a/plat/intel/soc/common/soc/socfpga_reset_manager.c
+++ b/plat/intel/soc/common/soc/socfpga_reset_manager.c
@@ -4,6 +4,7 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
 #include <errno.h>
 #include <common/debug.h>
 #include <drivers/delay_timer.h>
@@ -23,6 +24,7 @@
 			RSTMGR_FIELD(PER1, WATCHDOG1) |
 			RSTMGR_FIELD(PER1, WATCHDOG2) |
 			RSTMGR_FIELD(PER1, WATCHDOG3) |
+			RSTMGR_FIELD(PER1, WATCHDOG4) |
 			RSTMGR_FIELD(PER1, L4SYSTIMER0) |
 			RSTMGR_FIELD(PER1, L4SYSTIMER1) |
 			RSTMGR_FIELD(PER1, SPTIMER0) |
@@ -32,12 +34,15 @@
 			RSTMGR_FIELD(PER1, I2C2) |
 			RSTMGR_FIELD(PER1, I2C3) |
 			RSTMGR_FIELD(PER1, I2C4) |
+			RSTMGR_FIELD(PER1, I3C0) |
+			RSTMGR_FIELD(PER1, I3C1) |
 			RSTMGR_FIELD(PER1, UART0) |
 			RSTMGR_FIELD(PER1, UART1) |
 			RSTMGR_FIELD(PER1, GPIO0) |
 			RSTMGR_FIELD(PER1, GPIO1));
 
 	mmio_clrbits_32(SOCFPGA_RSTMGR(PER0MODRST),
+			RSTMGR_FIELD(PER0, SOFTPHY) |
 			RSTMGR_FIELD(PER0, EMAC0OCP) |
 			RSTMGR_FIELD(PER0, EMAC1OCP) |
 			RSTMGR_FIELD(PER0, EMAC2OCP) |
@@ -80,10 +85,10 @@
 {
 	uint32_t or_mask = 0;
 
-	or_mask |= RSTMGR_HDSKEN_SDRSELFREFEN;
+	or_mask |= RSTMGR_HDSKEN_EMIF_FLUSH;
 	or_mask |= RSTMGR_HDSKEN_FPGAHSEN;
 	or_mask |= RSTMGR_HDSKEN_ETRSTALLEN;
-	or_mask |= RSTMGR_HDSKEN_L2FLUSHEN;
+	or_mask |= RSTMGR_HDSKEN_LWS2F_FLUSH;
 	or_mask |= RSTMGR_HDSKEN_L3NOC_DBG;
 	or_mask |= RSTMGR_HDSKEN_DEBUG_L3NOC;
 
@@ -104,6 +109,27 @@
 	return -ETIMEDOUT;
 }
 
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+static int poll_idle_status_by_counter(uint32_t addr, uint32_t mask,
+					uint32_t match, uint32_t delay_ms)
+{
+	int time_out = delay_ms;
+
+	while (time_out-- > 0) {
+
+		if ((mmio_read_32(addr) & mask) == match) {
+			return 0;
+		}
+
+		/* ToDo: Shall use udelay for product release */
+		for (int i = 0; i < 2000; i++) {
+			/* dummy delay */
+		}
+	}
+	return -ETIMEDOUT;
+}
+#endif
+
 static int poll_idle_status_by_clkcycles(uint32_t addr, uint32_t mask,
 					 uint32_t match, uint32_t delay_clk_cycles)
 {
@@ -185,6 +211,39 @@
 		*f2s_respempty |= FLAGINSTATUS_F2SDRAM2_RESPEMPTY;
 		*f2s_cmdidle |= FLAGINSTATUS_F2SDRAM2_CMDIDLE;
 	}
+#elif PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	if (mask & FPGA2SOC_MASK) {
+		*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 |= FLAGINSTATUS_F2SDRAM0_IDLEACK;
+		*f2s_respempty |= FLAGINSTATUS_F2SDRAM0_RESPEMPTY;
+	}
+	if (mask & F2SDRAM0_MASK) {
+		*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 |= FLAGINSTATUS_F2SDRAM0_IDLEACK;
+		*f2s_respempty |= FLAGINSTATUS_F2SDRAM0_RESPEMPTY;
+	}
+	if (mask & F2SDRAM1_MASK) {
+		*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 |= FLAGINSTATUS_F2SDRAM1_IDLEACK;
+		*f2s_respempty |= FLAGINSTATUS_F2SDRAM1_RESPEMPTY;
+	}
+	if (mask & F2SDRAM2_MASK) {
+		*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 |= FLAGINSTATUS_F2SDRAM2_IDLEACK;
+		*f2s_respempty |= FLAGINSTATUS_F2SDRAM2_RESPEMPTY;
+	}
 #else
 	if ((mask & FPGA2SOC_MASK) != 0U) {
 		*brg_mask |= RSTMGR_FIELD(BRG, FPGA2SOC);
@@ -198,6 +257,153 @@
 #endif
 }
 
+int socfpga_bridges_reset(uint32_t mask)
+{
+	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;
+	uint32_t f2s_cmdidle = 0;
+
+	/* Reset s2f bridge */
+	socfpga_s2f_bridge_mask(mask, &brg_mask, &noc_mask);
+	if (brg_mask) {
+		if (mask & SOC2FPGA_MASK) {
+			/* Request handshake with SOC2FPGA bridge to clear traffic */
+			mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+					RSTMGR_HDSKREQ_S2F_FLUSH);
+
+			/* Wait for bridge to idle status */
+			ret = poll_idle_status(SOCFPGA_RSTMGR(HDSKACK),
+					RSTMGR_HDSKACK_S2F_FLUSH,
+					RSTMGR_HDSKACK_S2F_FLUSH, 300);
+		}
+
+		if (mask & LWHPS2FPGA_MASK) {
+			/* Request handshake with LWSOC2FPGA bridge to clear traffic */
+			mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+					RSTMGR_HDSKREQ_LWS2F_FLUSH);
+
+			/* Wait for bridge to idle status */
+			ret = poll_idle_status(SOCFPGA_RSTMGR(HDSKACK),
+					RSTMGR_HDSKACK_LWS2F_FLUSH,
+					RSTMGR_HDSKACK_LWS2F_FLUSH, 300);
+		}
+
+		if (ret < 0) {
+			ERROR("S2F Bridge reset: Timeout waiting for idle ack\n");
+			assert(false);
+		}
+
+		/* Assert reset to bridge */
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+				brg_mask);
+
+		/* Clear idle requests to bridge */
+		if (mask & SOC2FPGA_MASK) {
+			mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+					RSTMGR_HDSKREQ_S2F_FLUSH);
+		}
+
+		if (mask & LWHPS2FPGA_MASK) {
+			mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+					RSTMGR_HDSKREQ_LWS2F_FLUSH);
+		}
+
+		/* When FPGA reconfig is complete */
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
+	}
+
+	/* Reset f2s bridge */
+	socfpga_f2s_bridge_mask(mask, &brg_mask, &f2s_idlereq,
+					&f2s_force_drain, &f2s_en,
+					&f2s_idleack, &f2s_respempty,
+					&f2s_cmdidle);
+
+	if (brg_mask) {
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKEN),
+				RSTMGR_HDSKEN_FPGAHSEN);
+
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+				RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		ret = poll_idle_status(SOCFPGA_RSTMGR(HDSKACK),
+				RSTMGR_HDSKACK_FPGAHSREQ,
+				RSTMGR_HDSKACK_FPGAHSREQ, 300);
+
+		if (ret < 0) {
+			ERROR("F2S Bridge disable: Timeout waiting for idle req\n");
+			assert(false);
+		}
+
+		/* Disable f2s bridge */
+		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) {
+				idle_status = mmio_read_32(SOCFPGA_F2SDRAMMGR(
+					SIDEBANDMGR_FLAGINSTATUS0));
+				if (idle_status & f2s_respempty) {
+					break;
+				}
+			}
+			udelay(1000);
+		} while (timeout-- > 0);
+
+		/* Assert reset to f2s bridge */
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+				brg_mask);
+
+		/* Clear idle request to FPGA */
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+				RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Clear idle request to MPFE */
+		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTCLR0),
+				f2s_idlereq);
+
+		/* When FPGA reconfig is complete */
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
+
+		/* Enable f2s bridge */
+		mmio_clrbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+			f2s_idlereq);
+
+		ret = poll_idle_status(SOCFPGA_F2SDRAMMGR(
+			SIDEBANDMGR_FLAGINSTATUS0), f2s_idleack, 0, 300);
+		if (ret < 0) {
+			ERROR("F2S bridge enable: Timeout waiting for idle ack");
+			assert(false);
+		}
+
+		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_enable(uint32_t mask)
 {
 	int ret = 0;
@@ -209,9 +415,87 @@
 	uint32_t f2s_idleack = 0;
 	uint32_t f2s_respempty = 0;
 	uint32_t f2s_cmdidle = 0;
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	uint32_t delay = 0;
+#endif
 
 	/* Enable s2f bridge */
 	socfpga_s2f_bridge_mask(mask, &brg_mask, &noc_mask);
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	/* Enable SOC2FPGA bridge */
+	if (brg_mask & RSTMGR_BRGMODRSTMASK_SOC2FPGA) {
+		/* Write Reset Manager hdskreq[soc2fpga_flush_req] = 1 */
+		NOTICE("Set S2F hdskreq ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_SOC2FPGAREQ);
+
+		/* Read Reset Manager hdskack[soc2fpga] = 1 */
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_SOC2FPGAACK, RSTMGR_HDSKACK_SOC2FPGAACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("S2F bridge enable: Timeout hdskack\n");
+		}
+
+		/* Write Reset Manager hdskreq[soc2fpga_flush_req] = 0 */
+		NOTICE("Clear S2F hdskreq ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_SOC2FPGAREQ);
+
+		/* Write Reset Manager brgmodrst[soc2fpga] = 1 */
+		NOTICE("Assert S2F ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_SOC2FPGA);
+
+		/* ToDo: Shall use udelay for product release */
+		for (delay = 0; delay < 1000; delay++) {
+			/* dummy delay */
+		}
+
+		/* Write Reset Manager brgmodrst[soc2fpga] = 0 */
+		NOTICE("Deassert S2F ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_SOC2FPGA);
+	}
+
+	/* Enable LWSOC2FPGA bridge */
+	if (brg_mask & RSTMGR_BRGMODRSTMASK_LWHPS2FPGA) {
+		/* Write Reset Manager hdskreq[lwsoc2fpga_flush_req] = 1 */
+		NOTICE("Set LWS2F hdskreq ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_LWSOC2FPGAREQ);
+
+		/* Read Reset Manager hdskack[lwsoc2fpga] = 1 */
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_LWSOC2FPGAACK, RSTMGR_HDSKACK_LWSOC2FPGAACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("LWS2F bridge enable: Timeout hdskack\n");
+		}
+
+		/* Write Reset Manager hdskreq[lwsoc2fpga_flush_req] = 0 */
+		NOTICE("Clear LWS2F hdskreq ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_LWSOC2FPGAREQ);
+
+		/* Write Reset Manager brgmodrst[lwsoc2fpga] = 1 */
+		NOTICE("Assert LWS2F ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_LWHPS2FPGA);
+
+		/* ToDo: Shall use udelay for product release */
+		for (delay = 0; delay < 1000; delay++) {
+			/* dummy delay */
+		}
+
+		/* Write Reset Manager brgmodrst[lwsoc2fpga] = 0 */
+		NOTICE("Deassert LWS2F ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_LWHPS2FPGA);
+	}
+#else
 	if (brg_mask != 0U) {
 		/* Clear idle request */
 		mmio_setbits_32(SOCFPGA_SYSMGR(NOC_IDLEREQ_CLR),
@@ -224,15 +508,186 @@
 		ret = poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLEACK),
 				       noc_mask, 0, 300);
 		if (ret < 0) {
-			ERROR("S2F bridge enable: "
-					"Timeout waiting for idle ack\n");
+			ERROR("S2F bridge enable: Timeout idle ack\n");
 		}
 	}
+#endif
 
 	/* Enable f2s bridge */
 	socfpga_f2s_bridge_mask(mask, &brg_mask, &f2s_idlereq,
 				&f2s_force_drain, &f2s_en,
 				&f2s_idleack, &f2s_respempty, &f2s_cmdidle);
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	/* Enable FPGA2SOC bridge */
+	if (brg_mask & RSTMGR_BRGMODRSTMASK_FPGA2SOC) {
+		/* Write Reset Manager hdsken[fpgahsen] = 1 */
+		NOTICE("Set FPGA hdsken(fpgahsen) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKEN), RSTMGR_HDSKEN_FPGAHSEN);
+
+		/* Write Reset Manager hdskreq[fpgahsreq] = 1 */
+		NOTICE("Set FPGA hdskreq(fpgahsreq) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Read Reset Manager hdskack[fpgahsack] = 1 */
+		NOTICE("Get FPGA hdskack(fpgahsack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGAHSACK, RSTMGR_HDSKACK_FPGAHSACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("FPGA bridge fpga handshake fpgahsreq: Timeout\n");
+		}
+
+		/* Write Reset Manager hdskreq[f2s_flush_req] = 1 */
+		NOTICE("Set F2S hdskreq(f2s_flush_req) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_FPGA2SOCREQ);
+
+		/* Read Reset Manager hdskack[f2s_flush_ack] = 1 */
+		NOTICE("Get F2S hdskack(f2s_flush_ack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGA2SOCACK, RSTMGR_HDSKACK_FPGA2SOCACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2S bridge fpga handshake f2sdram_flush_req: Timeout\n");
+		}
+
+		/* Write Reset Manager hdskreq[fpgahsreq] = 1 */
+		NOTICE("Clear FPGA hdskreq(fpgahsreq) ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Write Reset Manager hdskreq[f2s_flush_req] = 1 */
+		NOTICE("Clear F2S hdskreq(f2s_flush_req) ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_FPGA2SOCREQ);
+
+		/* Read Reset Manager hdskack[f2s_flush_ack] = 0 */
+		NOTICE("Get F2SDRAM hdskack(f2s_flush_ack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGA2SOCACK, RSTMGR_HDSKACK_FPGA2SOCACK_DASRT,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2S bridge fpga handshake f2s_flush_ack: Timeout\n");
+		}
+
+		/* Read Reset Manager hdskack[fpgahsack] = 0 */
+		NOTICE("Get FPGA hdskack(fpgahsack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGAHSACK, RSTMGR_HDSKACK_FPGAHSACK_DASRT,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2S bridge fpga handshake fpgahsack: Timeout\n");
+		}
+
+		/* Write Reset Manager brgmodrst[fpga2soc] = 1 */
+		NOTICE("Assert F2S ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST), RSTMGR_BRGMODRST_FPGA2SOC);
+
+		/* ToDo: Shall use udelay for product release */
+		for (delay = 0; delay < 1000; delay++) {
+			/* dummy delay */
+		}
+
+		/* Write Reset Manager brgmodrst[fpga2soc] = 0 */
+		NOTICE("Deassert F2S ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), RSTMGR_BRGMODRST_FPGA2SOC);
+
+		/* Write System Manager f2s bridge control register[f2soc_enable] = 1 */
+		NOTICE("Deassert F2S f2soc_enable ...\n");
+		mmio_setbits_32(SOCFPGA_SYSMGR(F2S_BRIDGE_CTRL),
+			SYSMGR_F2S_BRIDGE_CTRL_EN);
+	}
+
+	/* Enable FPGA2SDRAM bridge */
+	if (brg_mask & RSTMGR_BRGMODRSTMASK_F2SDRAM0) {
+		/* Write Reset Manager hdsken[fpgahsen] = 1 */
+		NOTICE("Set F2SDRAM hdsken(fpgahsen) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKEN), RSTMGR_HDSKEN_FPGAHSEN);
+
+		/* Write Reset Manager hdskreq[fpgahsreq] = 1 */
+		NOTICE("Set F2SDRAM hdskreq(fpgahsreq) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Read Reset Manager hdskack[fpgahsack] = 1 */
+		NOTICE("Get F2SDRAM hdskack(fpgahsack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGAHSACK, RSTMGR_HDSKACK_FPGAHSACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2SDRAM bridge fpga handshake fpgahsreq: Timeout\n");
+		}
+
+		/* Write Reset Manager hdskreq[f2sdram_flush_req] = 1 */
+		NOTICE("Set F2SDRAM hdskreq(f2sdram_flush_req) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_F2SDRAM0REQ);
+
+		/* Read Reset Manager hdskack[f2sdram_flush_ack] = 1 */
+		NOTICE("Get F2SDRAM hdskack(f2sdram_flush_ack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_F2SDRAM0ACK, RSTMGR_HDSKACK_F2SDRAM0ACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2SDRAM bridge fpga handshake f2sdram_flush_req: Timeout\n");
+		}
+
+		/* Write Reset Manager hdskreq[fpgahsreq] = 1 */
+		NOTICE("Clear F2SDRAM hdskreq(fpgahsreq) ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Write Reset Manager hdskreq[f2sdram_flush_req] = 1 */
+		NOTICE("Clear F2SDRAM hdskreq(f2sdram_flush_req) ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_F2SDRAM0REQ);
+
+		/* Read Reset Manager hdskack[f2sdram_flush_ack] = 0 */
+		NOTICE("Get F2SDRAM hdskack(f2sdram_flush_ack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_F2SDRAM0ACK, RSTMGR_HDSKACK_F2SDRAM0ACK_DASRT,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2SDRAM bridge fpga handshake f2sdram_flush_ack: Timeout\n");
+		}
+
+		/* Read Reset Manager hdskack[fpgahsack] = 0 */
+		NOTICE("Get F2SDRAM hdskack(fpgahsack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGAHSACK, RSTMGR_HDSKACK_FPGAHSACK_DASRT,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2SDRAM bridge fpga handshake fpgahsack: Timeout\n");
+		}
+
+		/* Write Reset Manager brgmodrst[fpga2sdram] = 1 */
+		NOTICE("Assert F2SDRAM ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_F2SSDRAM0);
+
+		/* ToDo: Shall use udelay for product release */
+		for (delay = 0; delay < 1000; delay++) {
+			/* dummy delay */
+		}
+
+		/* Write Reset Manager brgmodrst[fpga2sdram] = 0 */
+		NOTICE("Deassert F2SDRAM ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_F2SSDRAM0);
+
+		/*
+		 * Clear fpga2sdram_manager_main_SidebandManager_FlagOutClr0
+		 * f2s_ready_latency_enable
+		 */
+		NOTICE("Clear F2SDRAM f2s_ready_latency_enable ...\n");
+		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTCLR0),
+			FLAGOUTCLR0_F2SDRAM0_ENABLE);
+	}
+#else
 	if (brg_mask != 0U) {
 		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
 
@@ -243,8 +698,7 @@
 				       f2s_idleack, 0, 300);
 
 		if (ret < 0) {
-			ERROR("F2S bridge enable: "
-			      "Timeout waiting for idle ack");
+			ERROR("F2S bridge enable: Timeout idle ack");
 		}
 
 		/* Clear the force drain */
@@ -256,7 +710,7 @@
 				f2s_en);
 		udelay(5);
 	}
-
+#endif
 	return ret;
 }
 
@@ -329,15 +783,13 @@
 		ret = poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLEACK),
 				       noc_mask, noc_mask, 300);
 		if (ret < 0) {
-			ERROR("S2F Bridge disable: "
-			      "Timeout waiting for idle ack\n");
+			ERROR("S2F Bridge disable: Timeout idle ack\n");
 		}
 
 		ret = poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLESTATUS),
 				       noc_mask, noc_mask, 300);
 		if (ret < 0) {
-			ERROR("S2F Bridge disable: "
-			      "Timeout waiting for idle status\n");
+			ERROR("S2F Bridge disable: Timeout idle status\n");
 		}
 
 		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
@@ -365,8 +817,8 @@
 				RSTMGR_HDSKREQ_FPGAHSREQ);
 
 		ret = poll_idle_status_by_clkcycles(SOCFPGA_RSTMGR(HDSKACK),
-						    RSTMGR_HDSKACK_FPGAHSACK_MASK,
-						    RSTMGR_HDSKACK_FPGAHSACK_MASK, 1000);
+						    RSTMGR_HDSKACK_FPGAHSREQ,
+						    RSTMGR_HDSKACK_FPGAHSREQ, 1000);
 
 		/* DISABLE F2S Bridge */
 		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTCLR0),
@@ -394,3 +846,68 @@
 
 	return ret;
 }
+
+/* CPUxRESETBASELOW */
+int socfpga_cpu_reset_base(unsigned int cpu_id)
+{
+	int ret = 0;
+	uint32_t entrypoint = 0;
+
+	ret = socfpga_cpurstrelease(cpu_id);
+
+	if (ret < 0) {
+		return RSTMGR_RET_ERROR;
+	}
+
+	if (ret == RSTMGR_RET_OK) {
+
+		switch (cpu_id) {
+		case 0:
+			entrypoint = mmio_read_32(SOCFPGA_RSTMGR_CPUBASELOW_0);
+			entrypoint |= mmio_read_32(SOCFPGA_RSTMGR_CPUBASEHIGH_0) << 24;
+		break;
+
+		case 1:
+			entrypoint = mmio_read_32(SOCFPGA_RSTMGR_CPUBASELOW_1);
+			entrypoint |= mmio_read_32(SOCFPGA_RSTMGR_CPUBASEHIGH_1) << 24;
+		break;
+
+		case 2:
+			entrypoint = mmio_read_32(SOCFPGA_RSTMGR_CPUBASELOW_2);
+			entrypoint |= mmio_read_32(SOCFPGA_RSTMGR_CPUBASEHIGH_2) << 24;
+		break;
+
+		case 3:
+			entrypoint = mmio_read_32(SOCFPGA_RSTMGR_CPUBASELOW_3);
+			entrypoint |= mmio_read_32(SOCFPGA_RSTMGR_CPUBASEHIGH_3) << 24;
+		break;
+
+		default:
+		break;
+		}
+
+		mmio_write_64(PLAT_SEC_ENTRY, entrypoint);
+	}
+
+	return RSTMGR_RET_OK;
+}
+
+/* CPURSTRELEASE */
+int socfpga_cpurstrelease(unsigned int cpu_id)
+{
+	unsigned int timeout = 0;
+
+	do {
+		/* Read response queue status to ensure it is empty */
+		uint32_t cpurstrelease_status;
+
+		cpurstrelease_status = mmio_read_32(SOCFPGA_RSTMGR(CPURSTRELEASE));
+
+		if ((cpurstrelease_status & RSTMGR_CPUSTRELEASE_CPUx) == cpu_id) {
+			return RSTMGR_RET_OK;
+		}
+		udelay(1000);
+	} while (timeout-- > 0);
+
+	return RSTMGR_RET_ERROR;
+}
diff --git a/plat/intel/soc/common/socfpga_psci.c b/plat/intel/soc/common/socfpga_psci.c
index 3b96dfc..5ffd512 100644
--- a/plat/intel/soc/common/socfpga_psci.c
+++ b/plat/intel/soc/common/socfpga_psci.c
@@ -1,12 +1,14 @@
 /*
  * Copyright (c) 2019-2023, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <arch_helpers.h>
 #include <common/debug.h>
-#ifndef SOCFPGA_GIC_V3
+
+#ifndef GICV3_SUPPORT_GIC600
 #include <drivers/arm/gicv2.h>
 #else
 #include <drivers/arm/gicv3.h>
@@ -14,13 +16,16 @@
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
 #include <plat/common/platform.h>
-
 #include "socfpga_mailbox.h"
 #include "socfpga_plat_def.h"
 #include "socfpga_reset_manager.h"
 #include "socfpga_sip_svc.h"
 #include "socfpga_system_manager.h"
 
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+void socfpga_wakeup_secondary_cpu(unsigned int cpu_id);
+extern void plat_secondary_cold_boot_setup(void);
+#endif
 
 /*******************************************************************************
  * plat handler called when a CPU is about to enter standby.
@@ -43,13 +48,18 @@
 int socfpga_pwr_domain_on(u_register_t mpidr)
 {
 	unsigned int cpu_id = plat_core_pos_by_mpidr(mpidr);
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	/* TODO: Add in CPU FUSE from SDM */
+#else
 	uint32_t psci_boot = 0x00;
 
 	VERBOSE("%s: mpidr: 0x%lx\n", __func__, mpidr);
+#endif
 
 	if (cpu_id == -1)
 		return PSCI_E_INTERN_FAIL;
 
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 	if (cpu_id == 0x00) {
 		psci_boot = mmio_read_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_8));
 		psci_boot |= 0x20000; /* bit 17 */
@@ -57,9 +67,16 @@
 	}
 
 	mmio_write_64(PLAT_CPUID_RELEASE, cpu_id);
+#endif
 
 	/* release core reset */
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	bl31_plat_set_secondary_cpu_entrypoint(cpu_id);
+#else
 	mmio_setbits_32(SOCFPGA_RSTMGR(MPUMODRST), 1 << cpu_id);
+	mmio_write_64(PLAT_CPUID_RELEASE, cpu_id);
+#endif
+
 	return PSCI_E_SUCCESS;
 }
 
@@ -74,7 +91,12 @@
 			__func__, i, target_state->pwr_domain_state[i]);
 
 	/* Prevent interrupts from spuriously waking up this cpu */
+#ifdef GICV3_SUPPORT_GIC600
+	gicv3_cpuif_disable(plat_my_core_pos());
+#else
 	gicv2_cpuif_disable();
+#endif
+
 }
 
 /*******************************************************************************
@@ -83,15 +105,18 @@
  ******************************************************************************/
 void socfpga_pwr_domain_suspend(const psci_power_state_t *target_state)
 {
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 	unsigned int cpu_id = plat_my_core_pos();
+#endif
 
 	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]);
 
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 	/* assert core reset */
 	mmio_setbits_32(SOCFPGA_RSTMGR(MPUMODRST), 1 << cpu_id);
-
+#endif
 }
 
 /*******************************************************************************
@@ -105,12 +130,18 @@
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
 
+	/* Enable the gic cpu interface */
+#ifdef GICV3_SUPPORT_GIC600
+	gicv3_rdistif_init(plat_my_core_pos());
+	gicv3_cpuif_enable(plat_my_core_pos());
+#else
 	/* Program the gic per-cpu distributor or re-distributor interface */
 	gicv2_pcpu_distif_init();
 	gicv2_set_pe_target_mask(plat_my_core_pos());
 
 	/* Enable the gic cpu interface */
 	gicv2_cpuif_enable();
+#endif
 }
 
 /*******************************************************************************
@@ -122,14 +153,18 @@
  ******************************************************************************/
 void socfpga_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
 {
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 	unsigned int cpu_id = plat_my_core_pos();
+#endif
 
 	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]);
 
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 	/* release core reset */
 	mmio_clrbits_32(SOCFPGA_RSTMGR(MPUMODRST), 1 << cpu_id);
+#endif
 }
 
 /*******************************************************************************
@@ -163,11 +198,20 @@
 static int socfpga_system_reset2(int is_vendor, int reset_type,
 					u_register_t cookie)
 {
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	mailbox_reset_warm(reset_type);
+#else
 	if (cold_reset_for_ecc_dbe()) {
 		mailbox_reset_cold();
 	}
+#endif
+
 	/* disable cpuif */
+#ifdef GICV3_SUPPORT_GIC600
+	gicv3_cpuif_disable(plat_my_core_pos());
+#else
 	gicv2_cpuif_disable();
+#endif
 
 	/* Store magic number */
 	mmio_write_32(L2_RESET_DONE_REG, L2_RESET_DONE_STATUS);
@@ -178,8 +222,10 @@
 	/* Enable handshakes */
 	mmio_setbits_32(SOCFPGA_RSTMGR(HDSKEN), RSTMGR_HDSKEN_SET);
 
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 	/* Reset L2 module */
 	mmio_setbits_32(SOCFPGA_RSTMGR(COLDMODRST), 0x100);
+#endif
 
 	while (1)
 		wfi();
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index ff050e4..c6530cf 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -335,6 +335,7 @@
 	return 0;
 #endif
 
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 	switch (reg_addr) {
 	case(0xF8011100):	/* ECCCTRL1 */
 	case(0xF8011104):	/* ECCCTRL2 */
@@ -386,7 +387,41 @@
 	case(0xFFD12220):	/* BOOT_SCRATCH_COLD8 */
 	case(0xFFD12224):	/* BOOT_SCRATCH_COLD9 */
 		return 0;
+#else
+	switch (reg_addr) {
+
+	case(0xF8011104):	/* ECCCTRL2 */
+	case(0xFFD12028):	/* SDMMCGRP_CTRL */
+	case(0xFFD120C4):	/* NOC_IDLEREQ_SET */
+	case(0xFFD120C8):	/* NOC_IDLEREQ_CLR */
+	case(0xFFD120D0):	/* NOC_IDLEACK */
+
 
+	case(SOCFPGA_MEMCTRL(ECCCTRL1)):	/* ECCCTRL1 */
+	case(SOCFPGA_MEMCTRL(ERRINTEN)):	/* ERRINTEN */
+	case(SOCFPGA_MEMCTRL(ERRINTENS)):	/* ERRINTENS */
+	case(SOCFPGA_MEMCTRL(ERRINTENR)):	/* ERRINTENR */
+	case(SOCFPGA_MEMCTRL(INTMODE)):	/* INTMODE */
+	case(SOCFPGA_MEMCTRL(INTSTAT)):	/* INTSTAT */
+	case(SOCFPGA_MEMCTRL(DIAGINTTEST)):	/* DIAGINTTEST */
+	case(SOCFPGA_MEMCTRL(DERRADDRA)):	/* DERRADDRA */
+
+	case(SOCFPGA_SYSMGR(EMAC_0)):	/* EMAC0 */
+	case(SOCFPGA_SYSMGR(EMAC_1)):	/* EMAC1 */
+	case(SOCFPGA_SYSMGR(EMAC_2)):	/* EMAC2 */
+	case(SOCFPGA_SYSMGR(ECC_INTMASK_VALUE)):	/* ECC_INT_MASK_VALUE */
+	case(SOCFPGA_SYSMGR(ECC_INTMASK_SET)):	/* ECC_INT_MASK_SET */
+	case(SOCFPGA_SYSMGR(ECC_INTMASK_CLR)):	/* ECC_INT_MASK_CLEAR */
+	case(SOCFPGA_SYSMGR(ECC_INTMASK_SERR)):	/* ECC_INTSTATUS_SERR */
+	case(SOCFPGA_SYSMGR(ECC_INTMASK_DERR)):	/* ECC_INTSTATUS_DERR */
+	case(SOCFPGA_SYSMGR(NOC_TIMEOUT)):	/* NOC_TIMEOUT */
+	case(SOCFPGA_SYSMGR(NOC_IDLESTATUS)):	/* NOC_IDLESTATUS */
+	case(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_0)):	/* BOOT_SCRATCH_COLD0 */
+	case(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_1)):	/* BOOT_SCRATCH_COLD1 */
+	case(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_8)):	/* BOOT_SCRATCH_COLD8 */
+	case(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_9)):	/* BOOT_SCRATCH_COLD9 */
+		return 0;
+#endif
 	default:
 		break;
 	}
@@ -442,7 +477,7 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
-static uint32_t intel_rsu_update(uint64_t update_address)
+uint32_t intel_rsu_update(uint64_t update_address)
 {
 	if (update_address > SIZE_MAX) {
 		return INTEL_SIP_SMC_STATUS_REJECTED;
diff --git a/plat/intel/soc/common/socfpga_sip_svc_v2.c b/plat/intel/soc/common/socfpga_sip_svc_v2.c
index 791c714..e9996d3 100644
--- a/plat/intel/soc/common/socfpga_sip_svc_v2.c
+++ b/plat/intel/soc/common/socfpga_sip_svc_v2.c
@@ -158,6 +158,10 @@
 		status = intel_hps_set_bridges(x2, x3);
 		SMC_RET2(handle, status, x1);
 
+	case INTEL_SIP_SMC_V2_RSU_UPDATE_ADDR:
+		status = intel_rsu_update(x2);
+		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);
diff --git a/plat/intel/soc/common/socfpga_storage.c b/plat/intel/soc/common/socfpga_storage.c
index 79e15d7..e80f074 100644
--- a/plat/intel/soc/common/socfpga_storage.c
+++ b/plat/intel/soc/common/socfpga_storage.c
@@ -9,29 +9,37 @@
 #include <assert.h>
 #include <common/debug.h>
 #include <common/tbbr/tbbr_img_def.h>
+#include <drivers/cadence/cdns_nand.h>
+#include <drivers/cadence/cdns_sdmmc.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 <drivers/io/io_mtd.h>
 #include <drivers/io/io_storage.h>
 #include <drivers/mmc.h>
 #include <drivers/partition/partition.h>
 #include <lib/mmio.h>
 #include <tools_share/firmware_image_package.h>
 
+#include "drivers/sdmmc/sdmmc.h"
 #include "socfpga_private.h"
 
+
 #define PLAT_FIP_BASE		(0)
 #define PLAT_FIP_MAX_SIZE	(0x1000000)
 #define PLAT_MMC_DATA_BASE	(0xffe3c000)
 #define PLAT_MMC_DATA_SIZE	(0x2000)
 #define PLAT_QSPI_DATA_BASE	(0x3C00000)
 #define PLAT_QSPI_DATA_SIZE	(0x1000000)
-
+#define PLAT_NAND_DATA_BASE	(0x0200000)
+#define PLAT_NAND_DATA_SIZE	(0x1000000)
 
 static const io_dev_connector_t *fip_dev_con;
 static const io_dev_connector_t *boot_dev_con;
 
+static io_mtd_dev_spec_t nand_dev_spec;
+
 static uintptr_t fip_dev_handle;
 static uintptr_t boot_dev_handle;
 
@@ -136,17 +144,27 @@
 	case BOOT_SOURCE_SDMMC:
 		register_io_dev = &register_io_dev_block;
 		boot_dev_spec.buffer.offset	= PLAT_MMC_DATA_BASE;
-		boot_dev_spec.buffer.length	= MMC_BLOCK_SIZE;
-		boot_dev_spec.ops.read		= mmc_read_blocks;
-		boot_dev_spec.ops.write		= mmc_write_blocks;
+		boot_dev_spec.buffer.length	= SOCFPGA_MMC_BLOCK_SIZE;
+		boot_dev_spec.ops.read		= SDMMC_READ_BLOCKS;
+		boot_dev_spec.ops.write		= SDMMC_WRITE_BLOCKS;
 		boot_dev_spec.block_size	= MMC_BLOCK_SIZE;
 		break;
 
 	case BOOT_SOURCE_QSPI:
 		register_io_dev = &register_io_dev_memmap;
-		fip_spec.offset = fip_spec.offset + PLAT_QSPI_DATA_BASE;
+		fip_spec.offset = PLAT_QSPI_DATA_BASE;
 		break;
 
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	case BOOT_SOURCE_NAND:
+		register_io_dev = &register_io_dev_mtd;
+		nand_dev_spec.ops.init = cdns_nand_init_mtd;
+		nand_dev_spec.ops.read = cdns_nand_read;
+		nand_dev_spec.ops.write = NULL;
+		fip_spec.offset = PLAT_NAND_DATA_BASE;
+		break;
+#endif
+
 	default:
 		ERROR("Unsupported boot source\n");
 		panic();
@@ -159,8 +177,13 @@
 	result = register_io_dev_fip(&fip_dev_con);
 	assert(result == 0);
 
-	result = io_dev_open(boot_dev_con, (uintptr_t)&boot_dev_spec,
-			&boot_dev_handle);
+	if (boot_source == BOOT_SOURCE_NAND) {
+		result = io_dev_open(boot_dev_con, (uintptr_t)&nand_dev_spec,
+								&boot_dev_handle);
+	} else {
+		result = io_dev_open(boot_dev_con, (uintptr_t)&boot_dev_spec,
+								&boot_dev_handle);
+	}
 	assert(result == 0);
 
 	result = io_dev_open(fip_dev_con, (uintptr_t)NULL, &fip_dev_handle);
diff --git a/plat/intel/soc/common/socfpga_topology.c b/plat/intel/soc/common/socfpga_topology.c
index ca1a91e..28c9557 100644
--- a/plat/intel/soc/common/socfpga_topology.c
+++ b/plat/intel/soc/common/socfpga_topology.c
@@ -33,8 +33,8 @@
 	if (mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK))
 		return -1;
 
-	cluster_id = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
-	cpu_id = (mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK;
+	cluster_id = (mpidr >> PLAT_CLUSTER_ID_MPIDR_AFF_SHIFT) & MPIDR_AFFLVL_MASK;
+	cpu_id = (mpidr >> PLAT_CPU_ID_MPIDR_AFF_SHIFT) & MPIDR_AFFLVL_MASK;
 
 	if (cluster_id >= PLATFORM_CLUSTER_COUNT)
 		return -1;
diff --git a/plat/intel/soc/common/socfpga_vab.c b/plat/intel/soc/common/socfpga_vab.c
new file mode 100644
index 0000000..e16610c
--- /dev/null
+++ b/plat/intel/soc/common/socfpga_vab.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/tbbr/tbbr_img_def.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <tools_share/firmware_image_package.h>
+
+#include "socfpga_mailbox.h"
+#include "socfpga_vab.h"
+
+static size_t get_img_size(uint8_t *img_buf, size_t img_buf_sz)
+{
+	uint8_t *img_buf_end = img_buf + img_buf_sz;
+	uint32_t cert_sz = get_unaligned_le32(img_buf_end - sizeof(uint32_t));
+	uint8_t *p = img_buf_end - cert_sz - sizeof(uint32_t);
+
+	/* Ensure p is pointing within the img_buf */
+	if (p < img_buf || p > (img_buf_end - VAB_CERT_HEADER_SIZE))
+		return 0;
+
+	if (get_unaligned_le32(p) == SDM_CERT_MAGIC_NUM)
+		return (size_t)(p - img_buf);
+
+	return 0;
+}
+
+
+
+int socfpga_vendor_authentication(void **p_image, size_t *p_size)
+{
+	int retry_count = 20;
+	uint8_t hash384[FCS_SHA384_WORD_SIZE];
+	uint64_t img_addr, mbox_data_addr;
+	uint32_t img_sz, mbox_data_sz;
+	uint8_t *cert_hash_ptr, *mbox_relocate_data_addr;
+	uint32_t resp = 0, resp_len = 1;
+	int ret = 0;
+
+	img_addr = (uintptr_t)*p_image;
+	img_sz = get_img_size((uint8_t *)img_addr, *p_size);
+
+	if (!img_sz) {
+		NOTICE("VAB certificate not found in image!\n");
+		return -ENOVABIMG;
+	}
+
+	if (!IS_BYTE_ALIGNED(img_sz, sizeof(uint32_t))) {
+		NOTICE("Image size (%d bytes) not aliged to 4 bytes!\n", img_sz);
+		return -EIMGERR;
+	}
+
+	/* Generate HASH384 from the image */
+	/* TODO: This part need to cross check !!!!!! */
+	sha384_csum_wd((uint8_t *)img_addr, img_sz, hash384, CHUNKSZ_PER_WD_RESET);
+	cert_hash_ptr = (uint8_t *)(img_addr + img_sz +
+	VAB_CERT_MAGIC_OFFSET + VAB_CERT_FIT_SHA384_OFFSET);
+
+	/*
+	 * Compare the SHA384 found in certificate against the SHA384
+	 * calculated from image
+	 */
+	if (memcmp(hash384, cert_hash_ptr, FCS_SHA384_WORD_SIZE)) {
+		NOTICE("SHA384 does not match!\n");
+		return -EKEYREJECTED;
+	}
+
+
+	mbox_data_addr = img_addr + img_sz - sizeof(uint32_t);
+	/* Size in word (32bits) */
+	mbox_data_sz = (BYTE_ALIGN(*p_size - img_sz, sizeof(uint32_t))) >> 2;
+
+	NOTICE("mbox_data_addr = %lx    mbox_data_sz = %d\n", mbox_data_addr, mbox_data_sz);
+
+	/* TODO: This part need to cross check !!!!!! */
+	// mbox_relocate_data_addr = (uint8_t *)malloc(mbox_data_sz * sizeof(uint32_t));
+	// if (!mbox_relocate_data_addr) {
+		// NOTICE("Cannot allocate memory for VAB certificate relocation!\n");
+		// return -ENOMEM;
+	// }
+
+	memcpy(mbox_relocate_data_addr, (uint8_t *)mbox_data_addr, mbox_data_sz * sizeof(uint32_t));
+	*(uint32_t *)mbox_relocate_data_addr = 0;
+
+	do {
+		/* Invoke SMC call to ATF to send the VAB certificate to SDM */
+		ret  = mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_VAB_SRC_CERT,
+(uint32_t *)mbox_relocate_data_addr, mbox_data_sz, 0, &resp, &resp_len);
+
+		/* If SDM is not available, just delay 50ms and retry again */
+		/* 0x1FF = The device is busy */
+		if (ret == MBOX_RESP_ERR(0x1FF)) {
+			mdelay(50);
+		} else {
+			break;
+		}
+	} while (--retry_count);
+
+	/* Free the relocate certificate memory space */
+	zeromem((void *)&mbox_relocate_data_addr, sizeof(uint32_t));
+
+
+	/* Exclude the size of the VAB certificate from image size */
+	*p_size = img_sz;
+
+	if (ret) {
+		/*
+		 * Unsupported mailbox command or device not in the
+		 * owned/secure state
+		 */
+		 /* 0x85 = Not allowed under current security setting */
+		if (ret == MBOX_RESP_ERR(0x85)) {
+			/* SDM bypass authentication */
+			NOTICE("Image Authentication bypassed at address\n");
+			return 0;
+		}
+		NOTICE("VAB certificate authentication failed in SDM\n");
+		/* 0x1FF = The device is busy */
+		if (ret == MBOX_RESP_ERR(0x1FF)) {
+			NOTICE("Operation timed out\n");
+			return -ETIMEOUT;
+		} else if (ret == MBOX_WRONG_ID) {
+			NOTICE("No such process\n");
+			return -EPROCESS;
+		}
+	} else {
+		/* If Certificate Process Status has error */
+		if (resp) {
+			NOTICE("VAB certificate execution format error\n");
+			return -EIMGERR;
+		}
+	}
+
+	NOTICE("Image Authentication bypassed at address\n");
+	return ret;
+
+}
+
+static uint32_t get_unaligned_le32(const void *p)
+{
+	/* TODO: Temp for testing */
+	//return le32_to_cpup((__le32 *)p);
+	return 0;
+}
+
+void sha384_csum_wd(const unsigned char *input, unsigned int ilen,
+		unsigned char *output, unsigned int chunk_sz)
+{
+	/* TODO: Update sha384 start, update and finish */
+}
diff --git a/plat/intel/soc/n5x/include/socfpga_plat_def.h b/plat/intel/soc/n5x/include/socfpga_plat_def.h
index 197bbca..a06bbc4 100644
--- a/plat/intel/soc/n5x/include/socfpga_plat_def.h
+++ b/plat/intel/soc/n5x/include/socfpga_plat_def.h
@@ -88,6 +88,18 @@
 #define PLAT_SYS_COUNTER_FREQ_IN_TICKS	(400000000)
 #define PLAT_HZ_CONVERT_TO_MHZ	(1000000)
 
+/*******************************************************************************
+ * SDMMC related pointer function
+ ******************************************************************************/
+#define SDMMC_READ_BLOCKS	mmc_read_blocks
+#define SDMMC_WRITE_BLOCKS	mmc_write_blocks
+
+/*******************************************************************************
+ * sysmgr.boot_scratch_cold6 & 7 (64bit) are used to indicate L2 reset
+ * is done and HPS should trigger warm reset via RMR_EL3.
+ ******************************************************************************/
+#define L2_RESET_DONE_REG			0xFFD12218
+
 /* Platform specific system counter */
 #define PLAT_SYS_COUNTER_FREQ_IN_MHZ	get_cpu_clk()
 
diff --git a/plat/intel/soc/stratix10/include/socfpga_plat_def.h b/plat/intel/soc/stratix10/include/socfpga_plat_def.h
index 8a5d4a4..7c9f15a 100644
--- a/plat/intel/soc/stratix10/include/socfpga_plat_def.h
+++ b/plat/intel/soc/stratix10/include/socfpga_plat_def.h
@@ -86,6 +86,18 @@
 #define PLAT_SYS_COUNTER_FREQ_IN_TICKS	(400000000)
 #define PLAT_HZ_CONVERT_TO_MHZ	(1000000)
 
+/*******************************************************************************
+ * SDMMC related pointer function
+ ******************************************************************************/
+#define SDMMC_READ_BLOCKS	mmc_read_blocks
+#define SDMMC_WRITE_BLOCKS	mmc_write_blocks
+
+/*******************************************************************************
+ * sysmgr.boot_scratch_cold6 & 7 (64bit) are used to indicate L2 reset
+ * is done and HPS should trigger warm reset via RMR_EL3.
+ ******************************************************************************/
+#define L2_RESET_DONE_REG			0xFFD12218
+
 /* Platform specific system counter */
 #define PLAT_SYS_COUNTER_FREQ_IN_MHZ	get_cpu_clk()
 
diff --git a/plat/mediatek/drivers/apusys/devapc/apusys_dapc_v1.h b/plat/mediatek/drivers/apusys/devapc/apusys_dapc_v1.h
index 2f5d47b..1b77942 100644
--- a/plat/mediatek/drivers/apusys/devapc/apusys_dapc_v1.h
+++ b/plat/mediatek/drivers/apusys/devapc/apusys_dapc_v1.h
@@ -155,19 +155,4 @@
 			   FORBIDDEN,     NO_PROTECTION, FORBIDDEN, FORBIDDEN, \
 			   FORBIDDEN,     FORBIDDEN,     FORBIDDEN, FORBIDDEN, \
 			   FORBIDDEN,     FORBIDDEN,     FORBIDDEN, FORBIDDEN)
-
-#define SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT_D3_SEC_RW(domain)			 \
-	APUSYS_APC_AO_ATTR(domain,						 \
-			   NO_PROTECTION, FORBIDDEN,     FORBIDDEN, SEC_RW_ONLY, \
-			   FORBIDDEN,     NO_PROTECTION, FORBIDDEN, FORBIDDEN,   \
-			   FORBIDDEN,     FORBIDDEN,     FORBIDDEN, FORBIDDEN,   \
-			   FORBIDDEN,     FORBIDDEN,     FORBIDDEN, FORBIDDEN)
-
-#define SLAVE_FORBID_EXCEPT_D0_D3_SEC_RW_D5_NO_PROTECT(domain)		       \
-	APUSYS_APC_AO_ATTR(domain,					       \
-			   SEC_RW_ONLY, FORBIDDEN,     FORBIDDEN, SEC_RW_ONLY, \
-			   FORBIDDEN,   NO_PROTECTION, FORBIDDEN, FORBIDDEN,   \
-			   FORBIDDEN,   FORBIDDEN,     FORBIDDEN, FORBIDDEN,   \
-			   FORBIDDEN,   FORBIDDEN,     FORBIDDEN, FORBIDDEN)
-
 #endif /* APUSYS_DAPC_V1_H */
diff --git a/plat/mediatek/drivers/apusys/mt8188/apusys_devapc_def.h b/plat/mediatek/drivers/apusys/mt8188/apusys_devapc_def.h
index e74b022..47a2a94 100644
--- a/plat/mediatek/drivers/apusys/mt8188/apusys_devapc_def.h
+++ b/plat/mediatek/drivers/apusys/mt8188/apusys_devapc_def.h
@@ -29,14 +29,14 @@
 #define SLAVE_AO_BCRM			SLAVE_FORBID_EXCEPT_D5_NO_PROTECT
 #define SLAVE_AO_DAPC_WRAP		SLAVE_FORBID_EXCEPT_D5_NO_PROTECT
 #define SLAVE_AO_DAPC_CON		SLAVE_FORBID_EXCEPT_D0_SEC_RW_D5_NO_PROTECT
-#define SLAVE_RCX_ACX_BULK		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT_D3_SEC_RW
-#define SLAVE_ACX0_BCRM			SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT_D3_SEC_RW
+#define SLAVE_RCX_ACX_BULK		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
+#define SLAVE_ACX0_BCRM			SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
 #define SLAVE_RPCTOP_LITE_ACX0		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
-#define SLAVE_ACX1_BCRM			SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT_D3_SEC_RW
+#define SLAVE_ACX1_BCRM			SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
 #define SLAVE_RPCTOP_LITE_ACX1		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
-#define SLAVE_RCX_TO_ACX0_0		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT_D3_SEC_RW
+#define SLAVE_RCX_TO_ACX0_0		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
 #define SLAVE_RCX_TO_ACX0_1		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
-#define SLAVE_SAE_TO_ACX0_0		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT_D3_SEC_RW
+#define SLAVE_SAE_TO_ACX0_0		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
 #define SLAVE_SAE_TO_ACX0_1		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
 #define SLAVE_RCX_TO_ACX1_0		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
 #define SLAVE_RCX_TO_ACX1_1		SLAVE_FORBID_EXCEPT_D0_D5_NO_PROTECT
@@ -79,7 +79,7 @@
 #define SLAVE_SENSOR_WRAP_ACX1_DLA1	SLAVE_FORBID_EXCEPT_D5_NO_PROTECT
 #define SLAVE_SENSOR_WRAP_ACX1_VPU0	SLAVE_FORBID_EXCEPT_D5_NO_PROTECT
 #define SLAVE_REVISER			SLAVE_FORBID_EXCEPT_D0_SEC_RW
-#define SLAVE_NOC			SLAVE_FORBID_EXCEPT_D0_D3_SEC_RW_D5_NO_PROTECT
+#define SLAVE_NOC			SLAVE_FORBID_EXCEPT_D0_SEC_RW
 #define SLAVE_BCRM			SLAVE_FORBID_EXCEPT_D5_NO_PROTECT
 #define SLAVE_DAPC_WRAP			SLAVE_FORBID_EXCEPT_D5_NO_PROTECT
 #define SLAVE_DAPC_CON			SLAVE_FORBID_EXCEPT_D0_SEC_RW_D5_NO_PROTECT
diff --git a/plat/mediatek/drivers/gic600/mt_gic_v3.c b/plat/mediatek/drivers/gic600/mt_gic_v3.c
index cca5d0a..85f9e37 100644
--- a/plat/mediatek/drivers/gic600/mt_gic_v3.c
+++ b/plat/mediatek/drivers/gic600/mt_gic_v3.c
@@ -42,13 +42,19 @@
 
 struct gic_chip_data {
 	/* All cores share the same configuration */
+	unsigned int saved_ctlr;
 	unsigned int saved_group;
 	unsigned int saved_enable;
 	unsigned int saved_conf0;
 	unsigned int saved_conf1;
 	unsigned int saved_grpmod;
+	unsigned int saved_ispendr;
+	unsigned int saved_isactiver;
+	unsigned int saved_nsacr;
 	/* Per-core sgi */
 	unsigned int saved_sgi[PLATFORM_CORE_COUNT];
+	/* Per-core priority */
+	unsigned int saved_prio[PLATFORM_CORE_COUNT][GICR_NUM_REGS(IPRIORITYR)];
 };
 
 static struct gic_chip_data gic_data;
@@ -94,53 +100,84 @@
 
 void mt_gic_rdistif_save(void)
 {
-	unsigned int proc_num;
+	unsigned int i, proc_num;
 	uintptr_t gicr_base;
 
 	proc_num = plat_my_core_pos();
 	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
 
+	/*
+	 * Wait for any write to GICR_CTLR to complete before trying to save any
+	 * state.
+	 */
+	gicr_wait_for_pending_write(gicr_base);
+
+	gic_data.saved_ctlr = mmio_read_32(gicr_base + GICR_CTLR);
 	gic_data.saved_group = mmio_read_32(gicr_base + GICR_IGROUPR0);
 	gic_data.saved_enable = mmio_read_32(gicr_base + GICR_ISENABLER0);
 	gic_data.saved_conf0 = mmio_read_32(gicr_base + GICR_ICFGR0);
 	gic_data.saved_conf1 = mmio_read_32(gicr_base + GICR_ICFGR1);
 	gic_data.saved_grpmod = mmio_read_32(gicr_base + GICR_IGRPMODR0);
+	gic_data.saved_ispendr = mmio_read_32(gicr_base + GICR_ISPENDR0);
+	gic_data.saved_isactiver = mmio_read_32(gicr_base + GICR_ISACTIVER0);
+	gic_data.saved_nsacr = mmio_read_32(gicr_base + GICR_NSACR);
+
+	for (i = 0U; i < 8U; ++i)
+		gic_data.saved_prio[proc_num][i] = gicr_ipriorityr_read(gicr_base, i);
 
 	rdist_has_saved[proc_num] = 1;
 }
 
 void mt_gic_rdistif_restore(void)
 {
-	unsigned int proc_num;
+	unsigned int i, proc_num;
 	uintptr_t gicr_base;
 
 	proc_num = plat_my_core_pos();
 	if (rdist_has_saved[proc_num] == 1) {
 		gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+
 		mmio_write_32(gicr_base + GICR_IGROUPR0, gic_data.saved_group);
-		mmio_write_32(gicr_base + GICR_ISENABLER0,
-			gic_data.saved_enable);
+		mmio_write_32(gicr_base + GICR_IGRPMODR0, gic_data.saved_grpmod);
+		mmio_write_32(gicr_base + GICR_NSACR, gic_data.saved_nsacr);
 		mmio_write_32(gicr_base + GICR_ICFGR0, gic_data.saved_conf0);
 		mmio_write_32(gicr_base + GICR_ICFGR1, gic_data.saved_conf1);
-		mmio_write_32(gicr_base + GICR_IGRPMODR0,
-			gic_data.saved_grpmod);
+
+		for (i = 0U; i < 8U; ++i)
+			gicr_ipriorityr_write(gicr_base, i, gic_data.saved_prio[proc_num][i]);
+
+		mmio_write_32(gicr_base + GICR_ISPENDR0, gic_data.saved_ispendr);
+		mmio_write_32(gicr_base + GICR_ISACTIVER0, gic_data.saved_isactiver);
+		mmio_write_32(gicr_base + GICR_ISENABLER0, gic_data.saved_enable);
+		mmio_write_32(gicr_base + GICR_CTLR, gic_data.saved_ctlr);
+
+		gicr_wait_for_pending_write(gicr_base);
 	}
 }
 
 void mt_gic_rdistif_restore_all(void)
 {
-	unsigned int proc_num;
+	unsigned int i, proc_num;
 	uintptr_t gicr_base;
 
 	for (proc_num = 0; proc_num < PLATFORM_CORE_COUNT; proc_num++) {
 		gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+
 		mmio_write_32(gicr_base + GICR_IGROUPR0, gic_data.saved_group);
-		mmio_write_32(gicr_base + GICR_ISENABLER0,
-			gic_data.saved_enable);
+		mmio_write_32(gicr_base + GICR_IGRPMODR0, gic_data.saved_grpmod);
+		mmio_write_32(gicr_base + GICR_NSACR, gic_data.saved_nsacr);
 		mmio_write_32(gicr_base + GICR_ICFGR0, gic_data.saved_conf0);
 		mmio_write_32(gicr_base + GICR_ICFGR1, gic_data.saved_conf1);
-		mmio_write_32(gicr_base + GICR_IGRPMODR0,
-			gic_data.saved_grpmod);
+
+		for (i = 0U; i < 8U; ++i)
+			gicr_ipriorityr_write(gicr_base, i, gic_data.saved_prio[proc_num][i]);
+
+		mmio_write_32(gicr_base + GICR_ISPENDR0, gic_data.saved_ispendr);
+		mmio_write_32(gicr_base + GICR_ISACTIVER0, gic_data.saved_isactiver);
+		mmio_write_32(gicr_base + GICR_ISENABLER0, gic_data.saved_enable);
+		mmio_write_32(gicr_base + GICR_CTLR, gic_data.saved_ctlr);
+
+		gicr_wait_for_pending_write(gicr_base);
 	}
 }
 
diff --git a/plat/qemu/common/qemu_sdei.c b/plat/qemu/common/qemu_sdei.c
new file mode 100644
index 0000000..820567e
--- /dev/null
+++ b/plat/qemu/common/qemu_sdei.c
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2023, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* SDEI configuration for ARM platforms */
+
+#include <bl31/ehf.h>
+#include <common/debug.h>
+#include <lib/utils_def.h>
+#include <services/sdei.h>
+#include <platform_def.h>
+
+/* Private event mappings */
+static sdei_ev_map_t qemu_sdei_private[] = {
+	SDEI_DEFINE_EVENT_0(PLAT_SDEI_SGI_PRIVATE),
+};
+
+/* Shared event mappings */
+static sdei_ev_map_t qemu_sdei_shared[] = {
+};
+
+void plat_sdei_setup(void)
+{
+	INFO("SDEI platform setup\n");
+}
+
+/* Export Arm SDEI events */
+REGISTER_SDEI_MAP(qemu_sdei_private, qemu_sdei_shared);
diff --git a/plat/qemu/qemu/include/platform_def.h b/plat/qemu/qemu/include/platform_def.h
index 98b8254..93a3ce8 100644
--- a/plat/qemu/qemu/include/platform_def.h
+++ b/plat/qemu/qemu/include/platform_def.h
@@ -244,8 +244,7 @@
  * interrupts.
  *****************************************************************************/
 #define PLATFORM_G1S_PROPS(grp)						\
-	INTR_PROP_DESC(QEMU_IRQ_SEC_SGI_0, GIC_HIGHEST_SEC_PRIORITY,	\
-					   grp, GIC_INTR_CFG_EDGE),	\
+	DESC_G1S_IRQ_SEC_SGI_0(grp)					\
 	INTR_PROP_DESC(QEMU_IRQ_SEC_SGI_1, GIC_HIGHEST_SEC_PRIORITY,	\
 					   grp, GIC_INTR_CFG_EDGE),	\
 	INTR_PROP_DESC(QEMU_IRQ_SEC_SGI_2, GIC_HIGHEST_SEC_PRIORITY,	\
@@ -261,7 +260,19 @@
 	INTR_PROP_DESC(QEMU_IRQ_SEC_SGI_7, GIC_HIGHEST_SEC_PRIORITY,	\
 					   grp, GIC_INTR_CFG_EDGE)
 
-#define PLATFORM_G0_PROPS(grp)
+#if SDEI_SUPPORT
+#define DESC_G0_IRQ_SEC_SGI(grp)					\
+	INTR_PROP_DESC(QEMU_IRQ_SEC_SGI_0, PLAT_SDEI_NORMAL_PRI, (grp), \
+					   GIC_INTR_CFG_EDGE)
+#define DESC_G1S_IRQ_SEC_SGI_0(grp)
+#else
+#define DESC_G0_IRQ_SEC_SGI(grp)
+#define DESC_G1S_IRQ_SEC_SGI_0(grp)					\
+	INTR_PROP_DESC(QEMU_IRQ_SEC_SGI_0, PLAT_SDEI_NORMAL_PRI, (grp),	\
+					   GIC_INTR_CFG_EDGE),
+#endif
+
+#define PLATFORM_G0_PROPS(grp)		DESC_G0_IRQ_SEC_SGI(grp)
 
 /*
  * DT related constants
@@ -270,6 +281,14 @@
 #define PLAT_QEMU_DT_MAX_SIZE		0x100000
 
 /*
+ * Platforms macros to support SDEI
+ */
+#define PLAT_PRI_BITS			U(3)
+#define PLAT_SDEI_CRITICAL_PRI		0x60
+#define PLAT_SDEI_NORMAL_PRI		0x70
+#define PLAT_SDEI_SGI_PRIVATE		QEMU_IRQ_SEC_SGI_0
+
+/*
  * System counter
  */
 #define SYS_COUNTER_FREQ_IN_TICKS	((1000 * 1000 * 1000) / 16)
diff --git a/plat/qemu/qemu/platform.mk b/plat/qemu/qemu/platform.mk
index 3a0e1c0..a10ab65 100644
--- a/plat/qemu/qemu/platform.mk
+++ b/plat/qemu/qemu/platform.mk
@@ -12,7 +12,7 @@
 # Qemu Cortex-A15 model does not implement the virtualization extension.
 # For this reason, we cannot set ARM_CORTEX_A15=yes and must define all
 # the ARMv7 build directives.
-MARCH32_DIRECTIVE 	:= 	-mcpu=cortex-a15
+MARCH_DIRECTIVE 	:= 	-mcpu=cortex-a15
 $(eval $(call add_define,ARMV7_SUPPORTS_LARGE_PAGE_ADDRESSING))
 $(eval $(call add_define,ARMV7_SUPPORTS_GENERIC_TIMER))
 $(eval $(call add_define,ARMV7_SUPPORTS_VFP))
@@ -65,6 +65,7 @@
 				lib/cpus/aarch64/cortex_a76.S		\
 				lib/cpus/aarch64/neoverse_n_common.S	\
 				lib/cpus/aarch64/neoverse_n1.S		\
+				lib/cpus/aarch64/neoverse_v1.S		\
 				lib/cpus/aarch64/qemu_max.S
 else
 QEMU_CPU_LIBS		:=	lib/cpus/${ARCH}/cortex_a15.S
@@ -230,6 +231,10 @@
 				${PLAT_QEMU_COMMON_PATH}/qemu_bl31_setup.c		\
 				${QEMU_GIC_SOURCES}
 
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES		+=	plat/qemu/common/qemu_sdei.c
+endif
+
 # Pointer Authentication sources
 ifeq (${ENABLE_PAUTH}, 1)
 PLAT_BL_COMMON_SOURCES	+=	plat/arm/common/aarch64/arm_pauth.c	\
@@ -323,3 +328,7 @@
 ifneq (${BL33},)
 all: qemu_fw.bios qemu_fw.rom
 endif
+
+ifeq (${EL3_EXCEPTION_HANDLING},1)
+BL31_SOURCES		+=	plat/common/aarch64/plat_ehf.c
+endif
diff --git a/plat/qemu/qemu_sbsa/platform.mk b/plat/qemu/qemu_sbsa/platform.mk
index 60d6b7e..3dfefd0 100644
--- a/plat/qemu/qemu_sbsa/platform.mk
+++ b/plat/qemu/qemu_sbsa/platform.mk
@@ -51,6 +51,7 @@
 				lib/cpus/aarch64/cortex_a72.S			\
 				lib/cpus/aarch64/neoverse_n_common.S		\
 				lib/cpus/aarch64/neoverse_n1.S			\
+				lib/cpus/aarch64/neoverse_v1.S			\
 				lib/cpus/aarch64/qemu_max.S
 
 include lib/xlat_tables_v2/xlat_tables.mk
diff --git a/plat/qemu/qemu_sbsa/sbsa_sip_svc.c b/plat/qemu/qemu_sbsa/sbsa_sip_svc.c
index 37460d7..05ebec4 100644
--- a/plat/qemu/qemu_sbsa/sbsa_sip_svc.c
+++ b/plat/qemu/qemu_sbsa/sbsa_sip_svc.c
@@ -26,8 +26,10 @@
  * need version of whole 'virtual hardware platform'.
  */
 #define SIP_SVC_VERSION  SIP_FUNCTION_ID(1)
-
 #define SIP_SVC_GET_GIC  SIP_FUNCTION_ID(100)
+#define SIP_SVC_GET_GIC_ITS SIP_FUNCTION_ID(101)
+
+static uint64_t gic_its_addr;
 
 void sbsa_set_gic_bases(const uintptr_t gicd_base, const uintptr_t gicr_base);
 uintptr_t sbsa_get_gicd(void);
@@ -45,9 +47,12 @@
 	 * QEMU gives us this DeviceTree node:
 	 *
 	 * intc {
-		reg = < 0x00 0x40060000 0x00 0x10000
-			0x00 0x40080000 0x00 0x4000000>;
-	};
+	 *	 reg = < 0x00 0x40060000 0x00 0x10000
+	 *		 0x00 0x40080000 0x00 0x4000000>;
+	 *       its {
+	 *               reg = <0x00 0x44081000 0x00 0x20000>;
+	 *       };
+	 * };
 	 */
 	node = fdt_path_offset(dtb, "/intc");
 	if (node < 0) {
@@ -74,6 +79,18 @@
 	INFO("GICR base = 0x%lx\n", gicr_base);
 
 	sbsa_set_gic_bases(gicd_base, gicr_base);
+
+	node = fdt_path_offset(dtb, "/intc/its");
+	if (node < 0) {
+		return;
+	}
+
+	err = fdt_get_reg_props_by_index(dtb, node, 0, &gic_its_addr, NULL);
+	if (err < 0) {
+		ERROR("Failed to read GICI reg property of GIC node\n");
+		return;
+	}
+	INFO("GICI base = 0x%lx\n", gic_its_addr);
 }
 
 void read_platform_version(void *dtb)
@@ -143,6 +160,9 @@
 	case SIP_SVC_GET_GIC:
 		SMC_RET3(handle, NULL, sbsa_get_gicd(), sbsa_get_gicr());
 
+	case SIP_SVC_GET_GIC_ITS:
+		SMC_RET2(handle, NULL, gic_its_addr);
+
 	default:
 		ERROR("%s: unhandled SMC (0x%x) (function id: %d)\n", __func__, smc_fid,
 		      smc_fid - SIP_FUNCTION);
diff --git a/plat/qti/common/src/qti_pm.c b/plat/qti/common/src/qti_pm.c
index 487a56e..1113efc 100644
--- a/plat/qti/common/src/qti_pm.c
+++ b/plat/qti/common/src/qti_pm.c
@@ -260,6 +260,10 @@
 		    state_id & QTI_LOCAL_PSTATE_MASK;
 		state_id >>= QTI_LOCAL_PSTATE_WIDTH;
 	}
+
+#if PSCI_OS_INIT_MODE
+	req_state->last_at_pwrlvl = PLAT_MAX_PWR_LVL;
+#endif
 }
 
 /*
diff --git a/plat/qti/mdm9607/platform.mk b/plat/qti/mdm9607/platform.mk
new file mode 100644
index 0000000..4c6938c
--- /dev/null
+++ b/plat/qti/mdm9607/platform.mk
@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ARM_ARCH_MAJOR		:= 7
+ARM_CORTEX_A7		:= yes
+
+BL31_BASE		?= 0x87e00000
+PRELOADED_BL33_BASE	?= 0x82900000
+QTI_UART_NUM		?= 5
+
+include plat/qti/msm8916/platform.mk
diff --git a/plat/qti/mdm9607/sp_min/sp_min-mdm9607.mk b/plat/qti/mdm9607/sp_min/sp_min-mdm9607.mk
new file mode 100644
index 0000000..28a6f01
--- /dev/null
+++ b/plat/qti/mdm9607/sp_min/sp_min-mdm9607.mk
@@ -0,0 +1,7 @@
+#
+# Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include plat/qti/msm8916/sp_min/sp_min-msm8916.mk
diff --git a/plat/qti/msm8909/platform.mk b/plat/qti/msm8909/platform.mk
new file mode 100644
index 0000000..8a88aa5
--- /dev/null
+++ b/plat/qti/msm8909/platform.mk
@@ -0,0 +1,13 @@
+#
+# Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ARM_ARCH_MAJOR	:= 7
+ARM_CORTEX_A7	:= yes
+
+BL31_BASE	?= 0x87e80000
+QTI_UART_NUM	?= 1
+
+include plat/qti/msm8916/platform.mk
diff --git a/plat/qti/msm8909/sp_min/sp_min-msm8909.mk b/plat/qti/msm8909/sp_min/sp_min-msm8909.mk
new file mode 100644
index 0000000..28a6f01
--- /dev/null
+++ b/plat/qti/msm8909/sp_min/sp_min-msm8909.mk
@@ -0,0 +1,7 @@
+#
+# Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include plat/qti/msm8916/sp_min/sp_min-msm8916.mk
diff --git a/plat/qti/msm8916/aarch32/msm8916_helpers.S b/plat/qti/msm8916/aarch32/msm8916_helpers.S
new file mode 100644
index 0000000..dc35043
--- /dev/null
+++ b/plat/qti/msm8916/aarch32/msm8916_helpers.S
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2021-2023, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <platform_def.h>
+
+#include <msm8916_mmap.h>
+
+#if PLATFORM_CORE_COUNT > 1
+#define APCS_TCM_START_ADDR	0x10
+#else
+#define APCS_TCM_START_ADDR	0x34
+#endif
+#define APCS_TCM_REDIRECT_EN_0	BIT_32(0)
+
+	.globl	plat_crash_console_init
+	.globl	plat_crash_console_putc
+	.globl	plat_crash_console_flush
+	.globl	plat_panic_handler
+	.globl	plat_my_core_pos
+	.globl	plat_get_my_entrypoint
+	.globl	plat_reset_handler
+	.globl	platform_mem_init
+	.globl	msm8916_entry_point
+
+	/* -------------------------------------------------
+	 * int plat_crash_console_init(void)
+	 * Initialize the crash console.
+	 * Out: r0 - 1 on success, 0 on error
+	 * Clobber list : r0 - r4
+	 * -------------------------------------------------
+	 */
+func plat_crash_console_init
+	ldr	r1, =BLSP_UART_BASE
+	mov	r0, #1
+	b	console_uartdm_core_init
+endfunc plat_crash_console_init
+
+	/* -------------------------------------------------
+	 * int plat_crash_console_putc(int c)
+	 * Print a character on the crash console.
+	 * In : r0 - character to be printed
+	 * Out: r0 - printed character on success
+	 * Clobber list : r1, r2
+	 * -------------------------------------------------
+	 */
+func plat_crash_console_putc
+	ldr	r1, =BLSP_UART_BASE
+	b	console_uartdm_core_putc
+endfunc plat_crash_console_putc
+
+	/* -------------------------------------------------
+	 * void plat_crash_console_flush(void)
+	 * Force a write of all buffered data that has not
+	 * been output.
+	 * Clobber list : r1, r2
+	 * -------------------------------------------------
+	 */
+func plat_crash_console_flush
+	ldr	r1, =BLSP_UART_BASE
+	b	console_uartdm_core_flush
+endfunc plat_crash_console_flush
+
+	/* -------------------------------------------------
+	 * void plat_panic_handler(void) __dead
+	 * Called when an unrecoverable error occurs.
+	 * -------------------------------------------------
+	 */
+func plat_panic_handler
+	/* Try to shutdown/reset */
+	ldr	r0, =MPM_PS_HOLD
+	mov	r1, #0
+	str	r1, [r0]
+1:	b	1b
+endfunc plat_panic_handler
+
+	/* -------------------------------------------------
+	 * unsigned int plat_my_core_pos(void)
+	 * Out: r0 - index of the calling CPU
+	 * -------------------------------------------------
+	 */
+func plat_my_core_pos
+	.if PLATFORM_CORE_COUNT > 1
+		ldcopr	r1, MPIDR
+		and	r0, r1, #MPIDR_CPU_MASK
+		.if PLATFORM_CLUSTER_COUNT > 1
+			and	r1, r1, #MPIDR_CLUSTER_MASK
+			orr	r0, r0, r1, LSR #(MPIDR_AFFINITY_BITS - \
+						  PLATFORM_CPU_PER_CLUSTER_SHIFT)
+		.endif
+	.else
+		/* There is just a single core so always 0 */
+		mov r0, #0
+	.endif
+	bx	lr
+endfunc plat_my_core_pos
+
+	/* -------------------------------------------------
+	 * uintptr_t plat_get_my_entrypoint(void)
+	 * Distinguish cold and warm boot and return warm boot
+	 * entry address if available.
+	 * Out: r0 - warm boot entry point or 0 on cold boot
+	 * -------------------------------------------------
+	 */
+func plat_get_my_entrypoint
+	ldr	r0, =msm8916_entry_point
+	ldr	r0, [r0]
+	cmp	r0, #0
+	bxne	lr
+
+	/*
+	 * Cold boot: Disable TCM redirect to L2 cache as early as
+	 * possible to avoid crashes when making use of the cache.
+	 */
+	ldr	r1, =APCS_CFG(0)
+	ldr	r2, [r1, #APCS_TCM_START_ADDR]
+	and	r2, r2, #~APCS_TCM_REDIRECT_EN_0
+	str	r2, [r1, #APCS_TCM_START_ADDR]
+	bx	lr
+endfunc plat_get_my_entrypoint
+
+	/* -------------------------------------------------
+	 * void platform_mem_init(void)
+	 * Performs additional memory initialization early
+	 * in the boot process.
+	 * -------------------------------------------------
+	 */
+func platform_mem_init
+	/* Nothing to do here, all memory is already initialized */
+	bx	lr
+endfunc platform_mem_init
+
+	.data
+	.align	3
+
+	/* -------------------------------------------------
+	 * Warm boot entry point for CPU. Set by PSCI code.
+	 * -------------------------------------------------
+	 */
+msm8916_entry_point:
+	.word	0
diff --git a/plat/qti/msm8916/aarch32/uartdm_console.S b/plat/qti/msm8916/aarch32/uartdm_console.S
new file mode 100644
index 0000000..a19776a
--- /dev/null
+++ b/plat/qti/msm8916/aarch32/uartdm_console.S
@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) 2021-2023, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * Based on aarch32/skeleton_console.S:
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include <console_macros.S>
+
+/* UART DM registers */
+#define UART_DM_DMEN		0x03c		/* DMA / data packing */
+#define UART_DM_SR		0x0a4		/* status register */
+#define UART_DM_CR		0x0a8		/* command register */
+#define UART_DM_TF		0x100		/* transmit FIFO */
+
+#define UART_DM_DMEN_TX_SC	BIT_32(4)	/* TX single character mode */
+
+#define UART_DM_SR_TXRDY	BIT_32(2)	/* TX FIFO has space */
+#define UART_DM_SR_TXEMT	BIT_32(3)	/* TX FIFO is empty */
+
+#define UART_DM_CR_RESET_RX	(U(0x01) << 4)	/* reset receiver */
+#define UART_DM_CR_RESET_TX	(U(0x02) << 4)	/* reset transmitter */
+#define UART_DM_CR_TX_ENABLE	BIT_32(2)	/* enable transmitter */
+
+	.globl	console_uartdm_register
+	.globl	console_uartdm_core_init
+	.globl	console_uartdm_putc
+	.globl	console_uartdm_core_putc
+	.globl	console_uartdm_flush
+	.globl	console_uartdm_core_flush
+
+	/* -----------------------------------------------------------
+	 * int console_uartdm_register(console_t *console,
+	 * 	uintptr_t base_addr)
+	 * Function to initialize and register the console. The caller
+	 * needs to pass an empty console_t structure in which *MUST*
+	 * be allocated in persistent memory (e.g. a global or static
+	 * local variable, *NOT* on the stack).
+	 * In : r0 - pointer to empty console_t structure
+	 *      r1 - base address
+	 * Out: r0 - 1 on success, 0 on error
+	 * Clobber list : r0 - r7
+	 * -----------------------------------------------------------
+	 */
+func console_uartdm_register
+	str	r1, [r0, #CONSOLE_T_BASE]
+	mov	r7, lr
+	bl	console_uartdm_core_init
+	mov	lr, r7
+
+	/* Register the new console */
+	finish_console_register uartdm putc=1, flush=1
+endfunc console_uartdm_register
+
+	/* -----------------------------------------------------------
+	 * void console_uartdm_core_init(unused, uintptr_t base_addr)
+	 * Function to initialize the console.
+	 * In : r0 - unused
+	 *      r1 - base address
+	 * Out: void
+	 * Clobber list : r1, r2, r3
+	 * -----------------------------------------------------------
+	 */
+func console_uartdm_core_init
+	/*
+	 * Try to flush remaining characters from the TX FIFO before resetting
+	 * the transmitter. Unfortunately there is no good way to check if
+	 * the transmitter is actually enabled (and will finish eventually),
+	 * so use a timeout to avoid looping forever.
+	 */
+	mov	r2, #65536
+1:
+	ldr	r3, [r1, #UART_DM_SR]
+	tst	r3, #UART_DM_SR_TXEMT
+	bne	2f
+	subs	r2, r2, #1
+	bne	1b
+	/* Timeout */
+
+2:	/* Reset receiver */
+	mov	r3, #UART_DM_CR_RESET_RX
+	str	r3, [r1, #UART_DM_CR]
+
+	/* Reset transmitter */
+	mov	r3, #UART_DM_CR_RESET_TX
+	str	r3, [r1, #UART_DM_CR]
+
+	/*
+	 * Disable BAM/DMA modes but enable single-character mode for TX.
+	 * The single character mode allows simplifying the putc implementation
+	 * since characters can be written directly to the FIFO instead of
+	 * having to initiate a new transfer and waiting for its completion.
+	 */
+	mov	r3, #UART_DM_DMEN_TX_SC
+	str	r3, [r1, #UART_DM_DMEN]
+
+	/* Enable transmitter */
+	mov	r3, #UART_DM_CR_TX_ENABLE
+	str	r3, [r1, #UART_DM_CR]
+
+	bx	lr
+endfunc console_uartdm_core_init
+
+	/* -----------------------------------------------------------
+	 * int console_uartdm_putc(int c, console_t *console)
+	 * Function to output a character over the console.
+	 * In : r0 - character to be printed
+	 *      r1 - pointer to console_t struct
+	 * Out: r0 - printed character on success, < 0 on error.
+	 * Clobber list : r0, r1, r2
+	 * -----------------------------------------------------------
+	 */
+func console_uartdm_putc
+	ldr	r1, [r1, #CONSOLE_T_BASE]
+	b	console_uartdm_core_putc
+endfunc console_uartdm_putc
+
+	/* -----------------------------------------------------------
+	 * int console_uartdm_core_putc(int c, uintptr_t base_addr)
+	 * Function to output a character over the console.
+	 * In : r0 - character to be printed
+	 *      r1 - base address
+	 * Out: r0 - printed character on success, < 0 on error.
+	 * Clobber list : r2
+	 * -----------------------------------------------------------
+	 */
+func console_uartdm_core_putc
+	cmp	r0, #'\n'
+	bne	2f
+
+1:	/* Loop until TX FIFO has space */
+	ldr	r2, [r1, #UART_DM_SR]
+	tst	r2, #UART_DM_SR_TXRDY
+	beq	1b
+
+	/* Prepend '\r' to '\n' */
+	mov	r2, #'\r'
+	str	r2, [r1, #UART_DM_TF]
+
+2:	/* Loop until TX FIFO has space */
+	ldr	r2, [r1, #UART_DM_SR]
+	tst	r2, #UART_DM_SR_TXRDY
+	beq	2b
+
+	/* Write character to FIFO */
+	str	r0, [r1, #UART_DM_TF]
+	bx	lr
+endfunc console_uartdm_core_putc
+
+	/* -----------------------------------------------------------
+	 * void console_uartdm_flush(console_t *console)
+	 * Function to force a write of all buffered data
+	 * that has not been output.
+	 * In : r0 - pointer to console_t struct
+	 * Out: void
+	 * Clobber list : r0, r1, r2, r3, r4, r5
+	 * -----------------------------------------------------------
+	 */
+func console_uartdm_flush
+	ldr	r1, [r0, #CONSOLE_T_BASE]
+	b	console_uartdm_core_flush
+endfunc console_uartdm_flush
+
+	/* -----------------------------------------------------------
+	 * void console_uartdm_core_flush(unused, uintptr_t base_addr)
+	 * Function to force a write of all buffered data
+	 * that has not been output.
+	 * In : r0 - unused
+	 *      r1 - base address
+	 * Out: void
+	 * Clobber list : r2
+	 * -----------------------------------------------------------
+	 */
+func console_uartdm_core_flush
+1:	/* Loop until TX FIFO is empty */
+	ldr	r2, [r1, #UART_DM_SR]
+	tst	r2, #UART_DM_SR_TXEMT
+	beq	1b
+	bx	lr
+endfunc console_uartdm_core_flush
diff --git a/plat/qti/msm8916/aarch64/msm8916_helpers.S b/plat/qti/msm8916/aarch64/msm8916_helpers.S
index 528c5a4..de9438a 100644
--- a/plat/qti/msm8916/aarch64/msm8916_helpers.S
+++ b/plat/qti/msm8916/aarch64/msm8916_helpers.S
@@ -6,10 +6,15 @@
 
 #include <arch.h>
 #include <asm_macros.S>
+#include <platform_def.h>
 
 #include <msm8916_mmap.h>
 
+#if PLATFORM_CORE_COUNT > 1
 #define APCS_TCM_START_ADDR	0x10
+#else
+#define APCS_TCM_START_ADDR	0x34
+#endif
 #define APCS_TCM_REDIRECT_EN_0	BIT_32(0)
 
 	.globl	plat_crash_console_init
@@ -30,7 +35,7 @@
 	 * -------------------------------------------------
 	 */
 func plat_crash_console_init
-	mov	x1, #BLSP_UART2_BASE
+	mov_imm	x1, BLSP_UART_BASE
 	mov	x0, #1
 	b	console_uartdm_core_init
 endfunc plat_crash_console_init
@@ -44,7 +49,7 @@
 	 * -------------------------------------------------
 	 */
 func plat_crash_console_putc
-	mov	x1, #BLSP_UART2_BASE
+	mov_imm	x1, BLSP_UART_BASE
 	b	console_uartdm_core_putc
 endfunc plat_crash_console_putc
 
@@ -56,7 +61,7 @@
 	 * -------------------------------------------------
 	 */
 func plat_crash_console_flush
-	mov	x1, #BLSP_UART2_BASE
+	mov_imm	x1, BLSP_UART_BASE
 	b	console_uartdm_core_flush
 endfunc plat_crash_console_flush
 
@@ -78,9 +83,18 @@
 	 * -------------------------------------------------
 	 */
 func plat_my_core_pos
-	/* There is just a single cluster so this is very simple */
-	mrs	x0, mpidr_el1
-	and	x0, x0, #MPIDR_CPU_MASK
+	.if PLATFORM_CORE_COUNT > 1
+		mrs	x1, mpidr_el1
+		and	x0, x1, #MPIDR_CPU_MASK
+		.if PLATFORM_CLUSTER_COUNT > 1
+			and	x1, x1, #MPIDR_CLUSTER_MASK
+			orr	x0, x0, x1, LSR #(MPIDR_AFFINITY_BITS - \
+						  PLATFORM_CPU_PER_CLUSTER_SHIFT)
+		.endif
+	.else
+		/* There is just a single core so always 0 */
+		mov	x0, #0
+	.endif
 	ret
 endfunc plat_my_core_pos
 
@@ -93,43 +107,42 @@
 	 */
 func plat_get_my_entrypoint
 	ldr	x0, msm8916_entry_point
+	cbz	x0, 1f
 	ret
-endfunc plat_get_my_entrypoint
-
-	/* -------------------------------------------------
-	 * void plat_reset_handler(void)
-	 * Perform additional initialization after reset.
-	 * Clobber list : x0 - x18, x30
-	 * -------------------------------------------------
+1:
+	/*
+	 * Cold boot: Disable TCM redirect to L2 cache as early as
+	 * possible to avoid crashes when making use of the cache.
 	 */
-func plat_reset_handler
+	mov_imm	x1, APCS_CFG(0)
+	ldr	w2, [x1, #APCS_TCM_START_ADDR]
+	and	w2, w2, #~APCS_TCM_REDIRECT_EN_0
+	str	w2, [x1, #APCS_TCM_START_ADDR]
+
 	/*
-	 * Check if the CPU is running at the correct address.
-	 * During cold boot the CPU enters here at the wrong address
-	 * using the "boot remapper". (It remaps the BL31_BASE to
-	 * the CPU reset address 0x0).
+	 * After reset the CPU always starts executing at the fixed reset
+	 * address (0x0), which does not match the link address of BL31.
+	 * The "boot remapper" redirects all memory accesses to the real
+	 * physical address in DRAM.
+	 *
+	 * For warm boots, this is already handled by loading the real
+	 * entry point address above.
+	 *
+	 * For cold boots, check if the CPU is using the boot remapper,
+	 * i.e. if bl31_entrypoint appears to be at the reset address (0x0).
 	 */
-	mov	x0, #BL31_BASE
 	adr	x1, bl31_entrypoint
-	cmp	x0, x1
-	b.ne	_remapped_cold_boot
-	/* Already running at correct address, just return directly */
-	ret
+	cbnz	x1, 2f
 
-_remapped_cold_boot:
 	/*
-	 * The previous boot stage seems to use the L2 cache as TCM.
-	 * Disable the TCM redirect before enabling caches to avoid
-	 * strange crashes.
+	 * Add the real BL31_BASE offset to the return address in the link
+	 * register so the CPU will continue at the real address after return.
 	 */
-	mov	x2, #APCS_CFG
-	ldr	w3, [x2, #APCS_TCM_START_ADDR]
-	and	w3, w3, #~APCS_TCM_REDIRECT_EN_0
-	str	w3, [x2, #APCS_TCM_START_ADDR]
-
-	/* Enter BL31 again at the real address */
-	br	x0
-endfunc plat_reset_handler
+	mov_imm	x1, BL31_BASE
+	add	lr, lr, x1
+2:
+	ret
+endfunc plat_get_my_entrypoint
 
 	/* -------------------------------------------------
 	 * void platform_mem_init(void)
diff --git a/plat/qti/msm8916/include/msm8916_mmap.h b/plat/qti/msm8916/include/msm8916_mmap.h
index d201536..20c5a57 100644
--- a/plat/qti/msm8916/include/msm8916_mmap.h
+++ b/plat/qti/msm8916/include/msm8916_mmap.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Stephan Gerhold <stephan@gerhold.net>
+ * Copyright (c) 2021-2023, Stephan Gerhold <stephan@gerhold.net>
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -22,20 +22,30 @@
 
 #define APPS_SMMU_BASE		(PCNOC_BASE + 0x1e00000)
 #define APPS_SMMU_QCOM		(APPS_SMMU_BASE + 0xf0000)
+#define GPU_SMMU_BASE		(PCNOC_BASE + 0x1f00000)
 
-#define BLSP_UART1_BASE		(PCNOC_BASE + 0x78af000)
-#define BLSP_UART2_BASE		(PCNOC_BASE + 0x78b0000)
+#define BLSP1_BASE		(PCNOC_BASE + 0x7880000)
+#define BLSP1_UART_BASE(n)	(BLSP1_BASE + 0x2f000 + (((n) - 1) * 0x1000))
+#define BLSP_UART_BASE		BLSP1_UART_BASE(QTI_UART_NUM)
 
 #define APCS_QGIC2_BASE		(APCS_BASE + 0x00000)
 #define APCS_QGIC2_GICD		(APCS_QGIC2_BASE + 0x0000)
 #define APCS_QGIC2_GICC		(APCS_QGIC2_BASE + 0x2000)
 #define APCS_BANKED_ACS		(APCS_BASE + 0x08000)
 #define APCS_BANKED_SAW2	(APCS_BASE + 0x09000)
-#define APCS_CFG		(APCS_BASE + 0x10000)
-#define APCS_GLB		(APCS_BASE + 0x11000)
-#define APCS_L2_SAW2		(APCS_BASE + 0x12000)
-#define APCS_QTMR		(APCS_BASE + 0x20000)
-#define APCS_ALIAS_ACS(cpu)	(APCS_BASE + 0x88000 + ((cpu) * 0x10000))
-#define APCS_ALIAS_SAW2(cpu)	(APCS_BASE + 0x89000 + ((cpu) * 0x10000))
+
+#define _APCS_CLUSTER(cluster)	(APCS_BASE + ((cluster) * 0x100000))
+#define _APCS_CPU(cluster, cpu)	(_APCS_CLUSTER(cluster) + ((cpu) * 0x10000))
+#define APCS_CFG(cluster)	(_APCS_CLUSTER(cluster) + 0x10000)
+#define APCS_GLB(cluster)	(_APCS_CLUSTER(cluster) + 0x11000)
+#define APCS_L2_SAW2(cluster)	(_APCS_CLUSTER(cluster) + 0x12000)
+#define APCS_QTMR(cluster)	(_APCS_CLUSTER(cluster) + 0x20000)
+#define APCS_ALIAS_ACS(cluster, cpu)	(_APCS_CPU(cluster, cpu) + 0x88000)
+#define APCS_ALIAS_SAW2(cluster, cpu)	(_APCS_CPU(cluster, cpu) + 0x89000)
+
+/* Only on platforms with multiple clusters (e.g. MSM8939) */
+#define APCS_CCI_BASE		(APCS_BASE + 0x1c0000)
+#define APCS_CCI_SAW2		(APCS_BASE + 0x1d2000)
+#define APCS_CCI_ACS		(APCS_BASE + 0x1d4000)
 
 #endif /* MSM8916_MMAP_H */
diff --git a/plat/qti/msm8916/include/platform_def.h b/plat/qti/msm8916/include/platform_def.h
index 6d5ff2b..a5baacd 100644
--- a/plat/qti/msm8916/include/platform_def.h
+++ b/plat/qti/msm8916/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Stephan Gerhold <stephan@gerhold.net>
+ * Copyright (c) 2021-2023, Stephan Gerhold <stephan@gerhold.net>
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,6 +8,7 @@
 
 #include <plat/common/common_def.h>
 
+#ifdef __aarch64__
 /*
  * There is at least 1 MiB available for BL31. However, at the moment the
  * "msm8916_entry_point" variable in the data section is read through the
@@ -18,15 +19,26 @@
  */
 #define BL31_LIMIT			(BL31_BASE + SZ_128K)
 #define BL31_PROGBITS_LIMIT		(BL31_BASE + SZ_64K)
+#endif
+#define BL32_LIMIT			(BL32_BASE + SZ_128K)
 
 #define CACHE_WRITEBACK_GRANULE		U(64)
 #define PLATFORM_STACK_SIZE		SZ_4K
 
-/* CPU topology: single cluster with 4 cores */
+/* CPU topology: one or two clusters with 4 cores each */
+#ifdef PLAT_msm8939
+#define PLATFORM_CLUSTER_COUNT		U(2)
+#else
 #define PLATFORM_CLUSTER_COUNT		U(1)
-#define PLATFORM_MAX_CPUS_PER_CLUSTER	U(4)
+#endif
+#if defined(PLAT_mdm9607)
+#define PLATFORM_CPU_PER_CLUSTER_SHIFT	U(0)	/* 1 */
+#else
+#define PLATFORM_CPU_PER_CLUSTER_SHIFT	U(2)	/* 4 */
+#endif
+#define PLATFORM_CPUS_PER_CLUSTER	(1 << PLATFORM_CPU_PER_CLUSTER_SHIFT)
 #define PLATFORM_CORE_COUNT		(PLATFORM_CLUSTER_COUNT * \
-					 PLATFORM_MAX_CPUS_PER_CLUSTER)
+					 PLATFORM_CPUS_PER_CLUSTER)
 
 /* Power management */
 #define PLATFORM_SYSTEM_COUNT		U(1)
@@ -44,8 +56,9 @@
 #define PLAT_PHY_ADDR_SPACE_SIZE	(ULL(1) << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(ULL(1) << 32)
 
-/* Timer frequency */
+/* Timer */
 #define PLAT_SYSCNT_FREQ		19200000
+#define IRQ_SEC_PHY_TIMER		(16 + 2)	/* PPI #2 */
 
 /*
  * The Qualcomm QGIC2 implementation seems to have PIDR0-4 and PIDR4-7
@@ -54,4 +67,9 @@
  */
 #define GICD_PIDR2_GICV2		U(0xFD8)
 
+/* TSP */
+#define TSP_IRQ_SEC_PHY_TIMER		IRQ_SEC_PHY_TIMER
+#define TSP_SEC_MEM_BASE		BL32_BASE
+#define TSP_SEC_MEM_SIZE		(BL32_LIMIT - BL32_BASE)
+
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/qti/msm8916/msm8916_bl31_setup.c b/plat/qti/msm8916/msm8916_bl31_setup.c
index 8cba5c5..c588020 100644
--- a/plat/qti/msm8916/msm8916_bl31_setup.c
+++ b/plat/qti/msm8916/msm8916_bl31_setup.c
@@ -8,25 +8,10 @@
 
 #include <arch.h>
 #include <common/debug.h>
-#include <drivers/console.h>
-#include <drivers/generic_delay_timer.h>
-#include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_mmu_helpers.h>
-#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
-#include "msm8916_gicv2.h"
-#include <msm8916_mmap.h>
-#include <platform_def.h>
-#include <uartdm_console.h>
-
-static const mmap_region_t msm8916_mmap[] = {
-	MAP_REGION_FLAT(PCNOC_BASE, PCNOC_SIZE,
-			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER),
-	MAP_REGION_FLAT(APCS_BASE, APCS_SIZE,
-			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER),
-	{},
-};
+#include "msm8916_config.h"
+#include "msm8916_setup.h"
 
 static struct {
 	entry_point_info_t bl32;
@@ -44,160 +29,24 @@
 	.bl33.spsr = SPSR_64(MODE_EL2, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS),
 };
 
-static console_t console;
-
-unsigned int plat_get_syscnt_freq2(void)
-{
-	return PLAT_SYSCNT_FREQ;
-}
-
-#define CLK_ENABLE			BIT_32(0)
-#define CLK_OFF				BIT_32(31)
-
-#define GPIO_BLSP_UART2_TX		4
-#define GPIO_BLSP_UART2_RX		5
-#define GPIO_CFG_FUNC_BLSP_UART2	(U(0x2) << 2)
-#define GPIO_CFG_DRV_STRENGTH_16MA	(U(0x7) << 6)
-
-#define GCC_BLSP1_AHB_CBCR		(GCC_BASE + 0x01008)
-#define GCC_BLSP1_UART2_APPS_CBCR	(GCC_BASE + 0x0302c)
-#define GCC_APCS_CLOCK_BRANCH_ENA_VOTE	(GCC_BASE + 0x45004)
-#define BLSP1_AHB_CLK_ENA		BIT_32(10)
-
-/*
- * The previous boot stage seems to disable most of the UART setup before exit
- * so it must be enabled here again before the UART console can be used.
- */
-static void msm8916_enable_blsp_uart2(void)
-{
-	/* Route GPIOs to BLSP UART2 */
-	mmio_write_32(TLMM_GPIO_CFG(GPIO_BLSP_UART2_TX),
-		      GPIO_CFG_FUNC_BLSP_UART2 | GPIO_CFG_DRV_STRENGTH_16MA);
-	mmio_write_32(TLMM_GPIO_CFG(GPIO_BLSP_UART2_RX),
-		      GPIO_CFG_FUNC_BLSP_UART2 | GPIO_CFG_DRV_STRENGTH_16MA);
-
-	/* Enable AHB clock */
-	mmio_setbits_32(GCC_APCS_CLOCK_BRANCH_ENA_VOTE, BLSP1_AHB_CLK_ENA);
-	while (mmio_read_32(GCC_BLSP1_AHB_CBCR) & CLK_OFF)
-		;
-
-	/* Enable BLSP UART2 clock */
-	mmio_setbits_32(GCC_BLSP1_UART2_APPS_CBCR, CLK_ENABLE);
-	while (mmio_read_32(GCC_BLSP1_UART2_APPS_CBCR) & CLK_OFF)
-		;
-}
-
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 				u_register_t arg2, u_register_t arg3)
 {
-	/* Initialize the debug console as early as possible */
-	msm8916_enable_blsp_uart2();
-	console_uartdm_register(&console, BLSP_UART2_BASE);
+	msm8916_early_platform_setup();
+	msm8916_configure_early();
 }
 
 void bl31_plat_arch_setup(void)
 {
-	mmap_add_region(BL31_BASE, BL31_BASE, BL31_END - BL31_BASE,
-			MT_RW_DATA | MT_SECURE);
-	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE,
-			BL_CODE_END - BL_CODE_BASE,
-			MT_CODE | MT_SECURE);
-	mmap_add_region(BL_RO_DATA_BASE, BL_RO_DATA_BASE,
-			BL_RO_DATA_END - BL_RO_DATA_BASE,
-			MT_RO_DATA | MT_SECURE);
-	mmap_add_region(BL_COHERENT_RAM_BASE, BL_COHERENT_RAM_BASE,
-			BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE,
-			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER);
-
-	mmap_add(msm8916_mmap);
-	init_xlat_tables();
+	msm8916_plat_arch_setup(BL31_BASE, BL31_END - BL31_BASE);
 	enable_mmu_el3(0);
 }
 
-static void msm8916_configure_timer(void)
-{
-	/* Set timer frequency */
-	mmio_write_32(APCS_QTMR + CNTCTLBASE_CNTFRQ, plat_get_syscnt_freq2());
-
-	/* Make all timer frames available to non-secure world */
-	mmio_write_32(APCS_QTMR + CNTNSAR, GENMASK_32(7, 0));
-}
-
-/*
- * The APCS register regions always start with a SECURE register that should
- * be cleared to 0 to only allow secure access. Since BL31 handles most of
- * the CPU power management, most of them can be cleared to secure access only.
- */
-#define APCS_GLB_SECURE_STS_NS		BIT_32(0)
-#define APCS_GLB_SECURE_PWR_NS		BIT_32(1)
-#define APCS_BOOT_START_ADDR_SEC	(APCS_CFG + 0x04)
-#define REMAP_EN			BIT_32(0)
-#define APCS_AA64NAA32_REG		(APCS_CFG + 0x0c)
-
-static void msm8916_configure_cpu_pm(void)
-{
-	unsigned int cpu;
-
-	/* Disallow non-secure access to boot remapper / TCM registers */
-	mmio_write_32(APCS_CFG, 0);
-
-	/*
-	 * Disallow non-secure access to power management registers.
-	 * However, allow STS and PWR since those also seem to control access
-	 * to CPU frequency related registers (e.g. APCS_CMD_RCGR). If these
-	 * bits are not set, CPU frequency control fails in the non-secure world.
-	 */
-	mmio_write_32(APCS_GLB, APCS_GLB_SECURE_STS_NS | APCS_GLB_SECURE_PWR_NS);
-
-	/* Disallow non-secure access to L2 SAW2 */
-	mmio_write_32(APCS_L2_SAW2, 0);
-
-	/* Disallow non-secure access to CPU ACS and SAW2 */
-	for (cpu = 0; cpu < PLATFORM_CORE_COUNT; cpu++) {
-		mmio_write_32(APCS_ALIAS_ACS(cpu), 0);
-		mmio_write_32(APCS_ALIAS_SAW2(cpu), 0);
-	}
-
-	/* Make sure all further warm boots end up in BL31 and aarch64 state */
-	CASSERT((BL31_BASE & 0xffff) == 0, assert_bl31_base_64k_aligned);
-	mmio_write_32(APCS_BOOT_START_ADDR_SEC, BL31_BASE | REMAP_EN);
-	mmio_write_32(APCS_AA64NAA32_REG, 1);
-}
-
-/*
- * MSM8916 has a special "interrupt aggregation logic" in the APPS SMMU,
- * which allows routing context bank interrupts to one of 3 interrupt numbers
- * ("TZ/HYP/NS"). Route all interrupts to the non-secure interrupt number
- * by default to avoid special setup on the non-secure side.
- */
-#define GCC_SMMU_CFG_CBCR			(GCC_BASE + 0x12038)
-#define GCC_APCS_SMMU_CLOCK_BRANCH_ENA_VOTE	(GCC_BASE + 0x4500c)
-#define SMMU_CFG_CLK_ENA			BIT_32(12)
-#define APPS_SMMU_INTR_SEL_NS			(APPS_SMMU_QCOM + 0x2000)
-#define APPS_SMMU_INTR_SEL_NS_EN_ALL		U(0xffffffff)
-
-static void msm8916_configure_smmu(void)
-{
-	/* Enable SMMU configuration clock to enable register access */
-	mmio_setbits_32(GCC_APCS_SMMU_CLOCK_BRANCH_ENA_VOTE, SMMU_CFG_CLK_ENA);
-	while (mmio_read_32(GCC_SMMU_CFG_CBCR) & CLK_OFF)
-		;
-
-	/* Route all context bank interrupts to non-secure interrupt */
-	mmio_write_32(APPS_SMMU_INTR_SEL_NS, APPS_SMMU_INTR_SEL_NS_EN_ALL);
-
-	/* Disable configuration clock again */
-	mmio_clrbits_32(GCC_APCS_SMMU_CLOCK_BRANCH_ENA_VOTE, SMMU_CFG_CLK_ENA);
-}
-
 void bl31_platform_setup(void)
 {
 	INFO("BL31: Platform setup start\n");
-	generic_delay_timer_init();
-	msm8916_configure_timer();
-	msm8916_gicv2_init();
-	msm8916_configure_cpu_pm();
-	msm8916_configure_smmu();
+	msm8916_platform_setup();
+	msm8916_configure();
 	INFO("BL31: Platform setup done\n");
 }
 
diff --git a/plat/qti/msm8916/msm8916_config.c b/plat/qti/msm8916/msm8916_config.c
new file mode 100644
index 0000000..0ac604b
--- /dev/null
+++ b/plat/qti/msm8916/msm8916_config.c
@@ -0,0 +1,195 @@
+/*
+ * Copyright (c) 2021-2023, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch.h>
+#include <drivers/arm/cci.h>
+#include <lib/mmio.h>
+
+#include "msm8916_config.h"
+#include "msm8916_gicv2.h"
+#include <msm8916_mmap.h>
+#include <platform_def.h>
+
+static const int cci_map[] = { 3, 4 };
+
+void msm8916_configure_early(void)
+{
+	if (PLATFORM_CLUSTER_COUNT > 1) {
+		cci_init(APCS_CCI_BASE, cci_map, ARRAY_SIZE(cci_map));
+		cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+	}
+}
+
+static void msm8916_configure_timer(uintptr_t base)
+{
+	/* Set timer frequency */
+	mmio_write_32(base + CNTCTLBASE_CNTFRQ, PLAT_SYSCNT_FREQ);
+
+	/* Make all timer frames available to non-secure world */
+	mmio_write_32(base + CNTNSAR, GENMASK_32(7, 0));
+}
+
+/*
+ * The APCS register regions always start with a SECURE register that should
+ * be cleared to 0 to only allow secure access. Since BL31 handles most of
+ * the CPU power management, most of them can be cleared to secure access only.
+ */
+#define APCS_GLB_SECURE_STS_NS		BIT_32(0)
+#define APCS_GLB_SECURE_PWR_NS		BIT_32(1)
+#if PLATFORM_CORE_COUNT > 1
+#define APCS_BOOT_START_ADDR_SEC	0x04
+#define APCS_AA64NAA32_REG		0x0c
+#else
+#define APCS_BOOT_START_ADDR_SEC	0x18
+#endif
+#define REMAP_EN			BIT_32(0)
+
+static void msm8916_configure_apcs_cluster(unsigned int cluster)
+{
+	uintptr_t cfg = APCS_CFG(cluster);
+	unsigned int cpu;
+
+	/* Disallow non-secure access to boot remapper / TCM registers */
+	mmio_write_32(cfg, 0);
+
+	/*
+	 * Disallow non-secure access to power management registers.
+	 * However, allow STS and PWR since those also seem to control access
+	 * to CPU frequency related registers (e.g. APCS_CMD_RCGR). If these
+	 * bits are not set, CPU frequency control fails in the non-secure world.
+	 */
+	mmio_write_32(APCS_GLB(cluster),
+		      APCS_GLB_SECURE_STS_NS | APCS_GLB_SECURE_PWR_NS);
+
+	if (PLATFORM_CORE_COUNT > 1) {
+		/* Disallow non-secure access to L2 SAW2 */
+		mmio_write_32(APCS_L2_SAW2(cluster), 0);
+
+		/* Disallow non-secure access to CPU ACS and SAW2 */
+		for (cpu = 0; cpu < PLATFORM_CPUS_PER_CLUSTER; cpu++) {
+			mmio_write_32(APCS_ALIAS_ACS(cluster, cpu), 0);
+			mmio_write_32(APCS_ALIAS_SAW2(cluster, cpu), 0);
+		}
+	} else {
+		/* There is just one core so no aliases exist */
+		mmio_write_32(APCS_BANKED_ACS, 0);
+		mmio_write_32(APCS_BANKED_SAW2, 0);
+	}
+
+#ifdef __aarch64__
+	/* Make sure all further warm boots end up in BL31 and aarch64 state */
+	CASSERT((BL31_BASE & 0xffff) == 0, assert_bl31_base_64k_aligned);
+	mmio_write_32(cfg + APCS_BOOT_START_ADDR_SEC, BL31_BASE | REMAP_EN);
+	mmio_write_32(cfg + APCS_AA64NAA32_REG, 1);
+#else
+	/* Make sure all further warm boots end up in BL32 */
+	CASSERT((BL32_BASE & 0xffff) == 0, assert_bl32_base_64k_aligned);
+	mmio_write_32(cfg + APCS_BOOT_START_ADDR_SEC, BL32_BASE | REMAP_EN);
+#endif
+
+	msm8916_configure_timer(APCS_QTMR(cluster));
+}
+
+static void msm8916_configure_apcs(void)
+{
+	unsigned int cluster;
+
+	for (cluster = 0; cluster < PLATFORM_CLUSTER_COUNT; cluster++) {
+		msm8916_configure_apcs_cluster(cluster);
+	}
+
+	if (PLATFORM_CLUSTER_COUNT > 1) {
+		/* Disallow non-secure access to CCI ACS and SAW2 */
+		mmio_write_32(APCS_CCI_ACS, 0);
+		mmio_write_32(APCS_CCI_SAW2, 0);
+	}
+}
+
+/*
+ * MSM8916 has a special "interrupt aggregation logic" in the APPS SMMU,
+ * which allows routing context bank interrupts to one of 3 interrupt numbers
+ * ("TZ/HYP/NS"). Route all interrupts to the non-secure interrupt number
+ * by default to avoid special setup on the non-secure side.
+ */
+#define CLK_OFF					BIT_32(31)
+#define GCC_APSS_TCU_CBCR			(GCC_BASE + 0x12018)
+#define GCC_GFX_TCU_CBCR			(GCC_BASE + 0x12020)
+#define GCC_SMMU_CFG_CBCR			(GCC_BASE + 0x12038)
+#define GCC_RPM_SMMU_CLOCK_BRANCH_ENA_VOTE	(GCC_BASE + 0x3600c)
+#define GCC_APCS_SMMU_CLOCK_BRANCH_ENA_VOTE	(GCC_BASE + 0x4500c)
+#define APSS_TCU_CLK_ENA			BIT_32(1)
+#define GFX_TCU_CLK_ENA				BIT_32(2)
+#define GFX_TBU_CLK_ENA				BIT_32(3)
+#define SMMU_CFG_CLK_ENA			BIT_32(12)
+#define APPS_SMMU_INTR_SEL_NS			(APPS_SMMU_QCOM + 0x2000)
+#define APPS_SMMU_INTR_SEL_NS_EN_ALL		U(0xffffffff)
+
+#define SMMU_SACR				0x010
+#define SMMU_SACR_CACHE_LOCK			BIT_32(26)
+#define SMMU_IDR7				0x03c
+#define SMMU_IDR7_MINOR(val)			(((val) >> 0) & 0xf)
+#define SMMU_IDR7_MAJOR(val)			(((val) >> 4) & 0xf)
+
+static void msm8916_smmu_cache_unlock(uintptr_t smmu_base, uintptr_t clk_cbcr)
+{
+	uint32_t version;
+
+	/* Wait for clock */
+	while (mmio_read_32(clk_cbcr) & CLK_OFF) {
+	}
+
+	version = mmio_read_32(smmu_base + SMMU_IDR7);
+	VERBOSE("SMMU(0x%lx) r%dp%d\n", smmu_base,
+		SMMU_IDR7_MAJOR(version), SMMU_IDR7_MINOR(version));
+
+	/* For SMMU r2p0+ clear CACHE_LOCK to allow writes to CBn_ACTLR */
+	if (SMMU_IDR7_MAJOR(version) >= 2) {
+		mmio_clrbits_32(smmu_base + SMMU_SACR, SMMU_SACR_CACHE_LOCK);
+	}
+}
+
+static void msm8916_configure_smmu(void)
+{
+	uint32_t ena_bits = APSS_TCU_CLK_ENA | SMMU_CFG_CLK_ENA;
+
+	/* Single core (MDM) platforms do not have a GPU */
+	if (PLATFORM_CORE_COUNT > 1) {
+		ena_bits |= GFX_TCU_CLK_ENA | GFX_TBU_CLK_ENA;
+	}
+
+	/* Enable SMMU clocks to enable register access */
+	mmio_write_32(GCC_APCS_SMMU_CLOCK_BRANCH_ENA_VOTE, ena_bits);
+
+	/* Wait for configuration clock */
+	while (mmio_read_32(GCC_SMMU_CFG_CBCR) & CLK_OFF) {
+	}
+
+	/* Route all context bank interrupts to non-secure interrupt */
+	mmio_write_32(APPS_SMMU_INTR_SEL_NS, APPS_SMMU_INTR_SEL_NS_EN_ALL);
+
+	/* Clear sACR.CACHE_LOCK bit if needed for MMU-500 r2p0+ */
+	msm8916_smmu_cache_unlock(APPS_SMMU_BASE, GCC_APSS_TCU_CBCR);
+	if (PLATFORM_CORE_COUNT > 1) {
+		msm8916_smmu_cache_unlock(GPU_SMMU_BASE, GCC_GFX_TCU_CBCR);
+	}
+
+	/*
+	 * Keep APCS vote for SMMU clocks for rest of booting process, but make
+	 * sure other vote registers (such as RPM) do not keep permanent votes.
+	 */
+	VERBOSE("Clearing GCC_RPM_SMMU_CLOCK_BRANCH_ENA_VOTE (was: 0x%x)\n",
+		mmio_read_32(GCC_RPM_SMMU_CLOCK_BRANCH_ENA_VOTE));
+	mmio_write_32(GCC_RPM_SMMU_CLOCK_BRANCH_ENA_VOTE, 0);
+}
+
+void msm8916_configure(void)
+{
+	msm8916_gicv2_configure();
+	msm8916_configure_apcs();
+	msm8916_configure_smmu();
+}
diff --git a/plat/qti/msm8916/msm8916_config.h b/plat/qti/msm8916/msm8916_config.h
new file mode 100644
index 0000000..977d02c
--- /dev/null
+++ b/plat/qti/msm8916/msm8916_config.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MSM8916_CONFIG_H
+#define MSM8916_CONFIG_H
+
+void msm8916_configure(void);
+void msm8916_configure_early(void);
+
+#endif /* MSM8916_CONFIG_H */
diff --git a/plat/qti/msm8916/msm8916_cpu_boot.c b/plat/qti/msm8916/msm8916_cpu_boot.c
index b3f51f6..d6faa59 100644
--- a/plat/qti/msm8916/msm8916_cpu_boot.c
+++ b/plat/qti/msm8916/msm8916_cpu_boot.c
@@ -1,14 +1,14 @@
 /*
- * Copyright (c) 2021, Stephan Gerhold <stephan@gerhold.net>
+ * Copyright (c) 2021-2022, Stephan Gerhold <stephan@gerhold.net>
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <arch_helpers.h>
+#include <common/debug.h>
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
 
-#include <msm8916_mmap.h>
 #include "msm8916_pm.h"
 
 #define CPU_PWR_CTL			0x4
@@ -26,12 +26,39 @@
 #define APC_PWR_GATE_CTL_GHDS_EN	BIT_32(0)
 #define APC_PWR_GATE_CTL_GHDS_CNT(cnt)	((cnt) << 24)
 
+#define PWR_CTL_OVERRIDE		0xc
+#define L2_PWR_CTL			0x14
+#define L2_PWR_STATUS			0x18
+#define CORE_CBCR			0x58
+
+#define PWR_CTL_OVERRIDE_PRESETDBG	BIT_32(22)
+
+#define L2_PWR_CTL_L2_ARRAY_HS		BIT_32(0)
+#define L2_PWR_CTL_SCU_ARRAY_HS		BIT_32(1)
+#define L2_PWR_CTL_L2_RST_DIS		BIT_32(2)
+#define L2_PWR_CTL_L2_HS_CLAMP		BIT_32(8)
+#define L2_PWR_CTL_L2_HS_EN		BIT_32(9)
+#define L2_PWR_CTL_L2_HS_RST		BIT_32(10)
+#define L2_PWR_CTL_L2_SLEEP_STATE	BIT_32(11)
+#define L2_PWR_CTL_SYS_RESET		BIT_32(12)
+#define L2_PWR_CTL_L2_RET_SLP		BIT_32(13)
+#define L2_PWR_CTL_SCU_ARRAY_HS_CLAMP	BIT_32(14)
+#define L2_PWR_CTL_L2_ARRAY_HS_CLAMP	BIT_32(15)
+#define L2_PWR_CTL_L2_HS_CNT(cnt)	((cnt) << 16)
+#define L2_PWR_CTL_PMIC_APC_ON		BIT_32(28)
+
+#define L2_PWR_STATUS_L2_HS_STS		BIT_32(9)
+
+#define CORE_CBCR_CLK_ENABLE		BIT_32(0)
+#define CORE_CBCR_HW_CTL		BIT_32(1)
+
 /* Boot a secondary CPU core for the first time. */
-void msm8916_cpu_boot(unsigned int core)
+void msm8916_cpu_boot(uintptr_t acs)
 {
-	uintptr_t acs = APCS_ALIAS_ACS(core);
 	uint32_t pwr_ctl;
 
+	VERBOSE("PSCI: Powering on CPU @ 0x%08lx\n", acs);
+
 	pwr_ctl = CPU_PWR_CTL_CLAMP | CPU_PWR_CTL_CORE_MEM_CLAMP |
 		  CPU_PWR_CTL_CORE_RST | CPU_PWR_CTL_COREPOR_RST;
 	mmio_write_32(acs + CPU_PWR_CTL, pwr_ctl);
@@ -64,3 +91,60 @@
 	mmio_write_32(acs + CPU_PWR_CTL, pwr_ctl);
 	dsb();
 }
+
+/* Power on cluster L2 cache for the first time. */
+void msm8916_l2_boot(uintptr_t base)
+{
+	uint32_t pwr_ctl, cbcr, ovr;
+
+	/* Skip if cluster L2 is already powered on */
+	if (mmio_read_32(base + L2_PWR_STATUS) & L2_PWR_STATUS_L2_HS_STS) {
+		VERBOSE("PSCI: L2 cache @ 0x%08lx is already powered on\n", base);
+		return;
+	}
+
+	VERBOSE("PSCI: Powering on L2 cache @ 0x%08lx\n", base);
+
+	pwr_ctl = L2_PWR_CTL_L2_HS_CLAMP | L2_PWR_CTL_L2_HS_EN |
+		  L2_PWR_CTL_L2_HS_RST | L2_PWR_CTL_SYS_RESET |
+		  L2_PWR_CTL_SCU_ARRAY_HS_CLAMP | L2_PWR_CTL_L2_ARRAY_HS_CLAMP |
+		  L2_PWR_CTL_L2_HS_CNT(16);
+	mmio_write_32(base + L2_PWR_CTL, pwr_ctl);
+
+	ovr = PWR_CTL_OVERRIDE_PRESETDBG;
+	mmio_write_32(base + PWR_CTL_OVERRIDE, ovr);
+	dsb();
+	udelay(2);
+
+	pwr_ctl &= ~(L2_PWR_CTL_SCU_ARRAY_HS_CLAMP |
+		     L2_PWR_CTL_L2_ARRAY_HS_CLAMP);
+	mmio_write_32(base + L2_PWR_CTL, pwr_ctl);
+
+	pwr_ctl |= (L2_PWR_CTL_L2_ARRAY_HS | L2_PWR_CTL_SCU_ARRAY_HS);
+	mmio_write_32(base + L2_PWR_CTL, pwr_ctl);
+	dsb();
+	udelay(2);
+
+	cbcr = CORE_CBCR_CLK_ENABLE;
+	mmio_write_32(base + CORE_CBCR, cbcr);
+
+	pwr_ctl &= ~L2_PWR_CTL_L2_HS_CLAMP;
+	mmio_write_32(base + L2_PWR_CTL, pwr_ctl);
+	dsb();
+	udelay(2);
+
+	ovr &= ~PWR_CTL_OVERRIDE_PRESETDBG;
+	mmio_write_32(base + PWR_CTL_OVERRIDE, ovr);
+
+	pwr_ctl &= ~(L2_PWR_CTL_L2_HS_RST | L2_PWR_CTL_SYS_RESET);
+	mmio_write_32(base + L2_PWR_CTL, pwr_ctl);
+	dsb();
+	udelay(54);
+
+	pwr_ctl |= L2_PWR_CTL_PMIC_APC_ON;
+	mmio_write_32(base + L2_PWR_CTL, pwr_ctl);
+
+	cbcr |= CORE_CBCR_HW_CTL;
+	mmio_write_32(base + CORE_CBCR, cbcr);
+	dsb();
+}
diff --git a/plat/qti/msm8916/msm8916_gicv2.c b/plat/qti/msm8916/msm8916_gicv2.c
index 25a6628..82cecf2 100644
--- a/plat/qti/msm8916/msm8916_gicv2.c
+++ b/plat/qti/msm8916/msm8916_gicv2.c
@@ -10,6 +10,7 @@
 
 #include "msm8916_gicv2.h"
 #include <msm8916_mmap.h>
+#include <platform_def.h>
 
 #define IRQ_SEC_SGI_0		8
 #define IRQ_SEC_SGI_1		9
@@ -20,8 +21,6 @@
 #define IRQ_SEC_SGI_6		14
 #define IRQ_SEC_SGI_7		15
 
-#define IRQ_SEC_PHY_TIMER	(16 + 2)	/* PPI #2 */
-
 static const interrupt_prop_t msm8916_interrupt_props[] = {
 	INTR_PROP_DESC(IRQ_SEC_SGI_0, GIC_HIGHEST_SEC_PRIORITY,
 		       GICV2_INTR_GROUP0, GIC_INTR_CFG_LEVEL),
@@ -53,6 +52,10 @@
 void msm8916_gicv2_init(void)
 {
 	gicv2_driver_init(&msm8916_gic_data);
+}
+
+void msm8916_gicv2_configure(void)
+{
 	gicv2_distif_init();
 	gicv2_pcpu_distif_init();
 	gicv2_cpuif_enable();
diff --git a/plat/qti/msm8916/msm8916_gicv2.h b/plat/qti/msm8916/msm8916_gicv2.h
index 99db0d3..919e294 100644
--- a/plat/qti/msm8916/msm8916_gicv2.h
+++ b/plat/qti/msm8916/msm8916_gicv2.h
@@ -8,5 +8,6 @@
 #define MSM8916_GICV2_H
 
 void msm8916_gicv2_init(void);
+void msm8916_gicv2_configure(void);
 
 #endif /* MSM8916_GICV2_H */
diff --git a/plat/qti/msm8916/msm8916_pm.c b/plat/qti/msm8916/msm8916_pm.c
index 792a096..fd44f04 100644
--- a/plat/qti/msm8916/msm8916_pm.c
+++ b/plat/qti/msm8916/msm8916_pm.c
@@ -4,9 +4,12 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
+
 #include <arch.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/arm/cci.h>
 #include <drivers/arm/gicv2.h>
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
@@ -16,18 +19,51 @@
 #include <msm8916_mmap.h>
 #include "msm8916_pm.h"
 
+/*
+ * On platforms with two clusters the index of the APCS memory region is swapped
+ * compared to the MPIDR cluster affinity level: APCS cluster 0 manages CPUs
+ * with cluster affinity level 1, while APCS cluster 1 manages CPUs with level 0.
+ *
+ * On platforms with a single cluster there is only one APCS memory region.
+ */
+#if PLATFORM_CLUSTER_COUNT == 2
+#define MPIDR_APCS_CLUSTER(mpidr)	!MPIDR_AFFLVL1_VAL(mpidr)
+#else
+#define MPIDR_APCS_CLUSTER(mpidr)	0
+#endif
+
+#define CLUSTER_PWR_STATE(state) ((state)->pwr_domain_state[MPIDR_AFFLVL1])
+
 static int msm8916_pwr_domain_on(u_register_t mpidr)
 {
-	unsigned int core = MPIDR_AFFLVL0_VAL(mpidr);
+	/* Should be never called on single-core platforms */
+	if (PLATFORM_CORE_COUNT == 1) {
+		assert(false);
+		return PSCI_E_ALREADY_ON;
+	}
 
-	VERBOSE("PSCI: Booting CPU %d\n", core);
-	msm8916_cpu_boot(core);
-
+	/* Power on L2 cache and secondary CPU core for the first time */
+	if (PLATFORM_CLUSTER_COUNT > 1) {
+		msm8916_l2_boot(APCS_GLB(MPIDR_APCS_CLUSTER(mpidr)));
+	}
+	msm8916_cpu_boot(APCS_ALIAS_ACS(MPIDR_APCS_CLUSTER(mpidr),
+					MPIDR_AFFLVL0_VAL(mpidr)));
 	return PSCI_E_SUCCESS;
 }
 
 static void msm8916_pwr_domain_on_finish(const psci_power_state_t *target_state)
 {
+	/* Should be never called on single-core platforms */
+	if (PLATFORM_CORE_COUNT == 1) {
+		assert(false);
+		return;
+	}
+
+	if (PLATFORM_CLUSTER_COUNT > 1 &&
+	    CLUSTER_PWR_STATE(target_state) == PLAT_MAX_OFF_STATE) {
+		cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+	}
+
 	gicv2_pcpu_distif_init();
 	gicv2_cpuif_enable();
 }
diff --git a/plat/qti/msm8916/msm8916_pm.h b/plat/qti/msm8916/msm8916_pm.h
index 5473bfa..f301d3c 100644
--- a/plat/qti/msm8916/msm8916_pm.h
+++ b/plat/qti/msm8916/msm8916_pm.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Stephan Gerhold <stephan@gerhold.net>
+ * Copyright (c) 2021-2022, Stephan Gerhold <stephan@gerhold.net>
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,7 @@
 #ifndef MSM8916_PM_H
 #define MSM8916_PM_H
 
-void msm8916_cpu_boot(unsigned int core);
+void msm8916_cpu_boot(uintptr_t acs);
+void msm8916_l2_boot(uintptr_t base);
 
 #endif /* MSM8916_PM_H */
diff --git a/plat/qti/msm8916/msm8916_setup.c b/plat/qti/msm8916/msm8916_setup.c
new file mode 100644
index 0000000..69c0d78
--- /dev/null
+++ b/plat/qti/msm8916/msm8916_setup.c
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2021-2023, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/bl_common.h>
+#include <drivers/console.h>
+#include <drivers/generic_delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_mmu_helpers.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+
+#include "msm8916_gicv2.h"
+#include <msm8916_mmap.h>
+#include "msm8916_setup.h"
+#include <uartdm_console.h>
+
+static const mmap_region_t msm8916_mmap[] = {
+	MAP_REGION_FLAT(PCNOC_BASE, PCNOC_SIZE,
+			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER),
+	MAP_REGION_FLAT(APCS_BASE, APCS_SIZE,
+			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER),
+	{},
+};
+
+static console_t console;
+
+unsigned int plat_get_syscnt_freq2(void)
+{
+	return PLAT_SYSCNT_FREQ;
+}
+
+#define GPIO_CFG_FUNC(n)		((n) << 2)
+#define GPIO_CFG_DRV_STRENGTH_MA(ma)	(((ma) / 2 - 1) << 6)
+
+#define CLK_ENABLE			BIT_32(0)
+#define CLK_OFF				BIT_32(31)
+#define GCC_BLSP1_AHB_CBCR		(GCC_BASE + 0x01008)
+#define GCC_BLSP1_UART_APPS_CBCR(n)	(GCC_BASE + \
+	(((n) == 2) ? (0x0302c) : (0x0203c + (((n) - 1) * 0x1000))))
+#define GCC_APCS_CLOCK_BRANCH_ENA_VOTE	(GCC_BASE + 0x45004)
+#define BLSP1_AHB_CLK_ENA		BIT_32(10)
+
+struct uartdm_gpios {
+	unsigned int tx, rx, func;
+};
+
+static const struct uartdm_gpios uartdm_gpio_map[] = {
+#if defined(PLAT_msm8909)
+	{4, 5, 0x2}, {20, 21, 0x3},
+#elif defined(PLAT_msm8916) || defined(PLAT_msm8939)
+	{0, 1, 0x2}, {4, 5, 0x2},
+#elif defined(PLAT_mdm9607)
+	{12, 13, 0x2}, {4, 5, 0x2}, {0, 1, 0x1},
+	{16, 17, 0x2}, {8, 9, 0x2}, {20, 21, 0x2},
+#endif
+};
+
+/*
+ * The previous boot stage seems to disable most of the UART setup before exit
+ * so it must be enabled here again before the UART console can be used.
+ */
+static void msm8916_enable_blsp_uart(void)
+{
+	const struct uartdm_gpios *gpios = &uartdm_gpio_map[QTI_UART_NUM - 1];
+
+	CASSERT(QTI_UART_NUM > 0 && QTI_UART_NUM <= ARRAY_SIZE(uartdm_gpio_map),
+		assert_qti_blsp_uart_valid);
+
+	/* Route GPIOs to BLSP UART */
+	mmio_write_32(TLMM_GPIO_CFG(gpios->tx), GPIO_CFG_FUNC(gpios->func) |
+		      GPIO_CFG_DRV_STRENGTH_MA(8));
+	mmio_write_32(TLMM_GPIO_CFG(gpios->rx), GPIO_CFG_FUNC(gpios->func) |
+		      GPIO_CFG_DRV_STRENGTH_MA(8));
+
+	/* Enable AHB clock */
+	mmio_setbits_32(GCC_APCS_CLOCK_BRANCH_ENA_VOTE, BLSP1_AHB_CLK_ENA);
+	while (mmio_read_32(GCC_BLSP1_AHB_CBCR) & CLK_OFF) {
+	}
+
+	/* Enable BLSP UART clock */
+	mmio_setbits_32(GCC_BLSP1_UART_APPS_CBCR(QTI_UART_NUM), CLK_ENABLE);
+	while (mmio_read_32(GCC_BLSP1_UART_APPS_CBCR(QTI_UART_NUM)) & CLK_OFF) {
+	}
+}
+
+void msm8916_early_platform_setup(void)
+{
+	/* Initialize the debug console as early as possible */
+	msm8916_enable_blsp_uart();
+	console_uartdm_register(&console, BLSP_UART_BASE);
+
+	if (QTI_RUNTIME_UART) {
+		/* Mark UART as runtime usable */
+		console_set_scope(&console, CONSOLE_FLAG_BOOT |
+				  CONSOLE_FLAG_RUNTIME | CONSOLE_FLAG_CRASH);
+	}
+}
+
+void msm8916_plat_arch_setup(uintptr_t base, size_t size)
+{
+	mmap_add_region(base, base, size, MT_RW_DATA | MT_SECURE);
+	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE,
+			BL_CODE_END - BL_CODE_BASE,
+			MT_CODE | MT_SECURE);
+	mmap_add_region(BL_RO_DATA_BASE, BL_RO_DATA_BASE,
+			BL_RO_DATA_END - BL_RO_DATA_BASE,
+			MT_RO_DATA | MT_SECURE);
+	mmap_add_region(BL_COHERENT_RAM_BASE, BL_COHERENT_RAM_BASE,
+			BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE,
+			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER);
+
+	mmap_add(msm8916_mmap);
+	init_xlat_tables();
+}
+
+void msm8916_platform_setup(void)
+{
+	generic_delay_timer_init();
+	msm8916_gicv2_init();
+}
diff --git a/plat/qti/msm8916/msm8916_setup.h b/plat/qti/msm8916/msm8916_setup.h
new file mode 100644
index 0000000..d2de943
--- /dev/null
+++ b/plat/qti/msm8916/msm8916_setup.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MSM8916_SETUP_H
+#define MSM8916_SETUP_H
+
+void msm8916_early_platform_setup(void);
+void msm8916_plat_arch_setup(uintptr_t base, size_t size);
+void msm8916_platform_setup(void);
+
+#endif /* MSM8916_SETUP_H */
diff --git a/plat/qti/msm8916/msm8916_topology.c b/plat/qti/msm8916/msm8916_topology.c
index 4d0ed8f..d8cdc0e 100644
--- a/plat/qti/msm8916/msm8916_topology.c
+++ b/plat/qti/msm8916/msm8916_topology.c
@@ -1,4 +1,5 @@
 /*
+ * Copyright (c) 2022, Stephan Gerhold <stephan@gerhold.net>
  * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
@@ -9,24 +10,27 @@
 
 #include <platform_def.h>
 
-static const unsigned char plat_power_domain_tree_desc[PLAT_MAX_PWR_LVL + 1] = {
+static const unsigned char plat_power_domain_tree_desc[] = {
 	PLATFORM_SYSTEM_COUNT,
 	PLATFORM_CLUSTER_COUNT,
-	PLATFORM_MAX_CPUS_PER_CLUSTER,
+	PLATFORM_CPUS_PER_CLUSTER,
+#if PLATFORM_CLUSTER_COUNT > 1
+	PLATFORM_CPUS_PER_CLUSTER,
+#endif
 };
 
 int plat_core_pos_by_mpidr(u_register_t mpidr)
 {
+	unsigned int cluster = MPIDR_AFFLVL1_VAL(mpidr);
 	unsigned int core = MPIDR_AFFLVL0_VAL(mpidr);
 
 	if (MPIDR_AFFLVL3_VAL(mpidr) > 0 ||
 	    MPIDR_AFFLVL2_VAL(mpidr) > 0 ||
-	    MPIDR_AFFLVL1_VAL(mpidr) > 0 ||
-	    core >= PLATFORM_MAX_CPUS_PER_CLUSTER) {
+	    cluster >= PLATFORM_CLUSTER_COUNT ||
+	    core >= PLATFORM_CPUS_PER_CLUSTER) {
 		return -1;
 	}
-
-	return core;
+	return core | (cluster << PLATFORM_CPU_PER_CLUSTER_SHIFT);
 }
 
 const unsigned char *plat_get_power_domain_tree_desc(void)
diff --git a/plat/qti/msm8916/platform.mk b/plat/qti/msm8916/platform.mk
index 107296b..4f4dcb4 100644
--- a/plat/qti/msm8916/platform.mk
+++ b/plat/qti/msm8916/platform.mk
@@ -7,35 +7,39 @@
 include drivers/arm/gic/v2/gicv2.mk
 include lib/xlat_tables_v2/xlat_tables.mk
 
-PLAT_BL_COMMON_SOURCES	:= ${XLAT_TABLES_LIB_SRCS}
+PLAT_BL_COMMON_SOURCES	:=	${GICV2_SOURCES}				\
+				${XLAT_TABLES_LIB_SRCS}				\
+				drivers/delay_timer/delay_timer.c		\
+				drivers/delay_timer/generic_delay_timer.c	\
+				plat/common/plat_gicv2.c			\
+				plat/qti/msm8916/msm8916_gicv2.c		\
+				plat/qti/msm8916/msm8916_setup.c		\
+				plat/qti/msm8916/${ARCH}/msm8916_helpers.S	\
+				plat/qti/msm8916/${ARCH}/uartdm_console.S
 
-PLAT_INCLUDES	:=	-Iinclude/plat/arm/common/${ARCH}		\
-			-Iplat/qti/msm8916/include
+MSM8916_CPU		:=	$(if ${ARM_CORTEX_A7},cortex_a7,cortex_a53)
+MSM8916_PM_SOURCES	:=	drivers/arm/cci/cci.c				\
+				lib/cpus/${ARCH}/${MSM8916_CPU}.S		\
+				plat/common/plat_psci_common.c			\
+				plat/qti/msm8916/msm8916_config.c		\
+				plat/qti/msm8916/msm8916_cpu_boot.c		\
+				plat/qti/msm8916/msm8916_pm.c			\
+				plat/qti/msm8916/msm8916_topology.c
 
-BL31_SOURCES	+=	${GICV2_SOURCES}				\
-			drivers/delay_timer/delay_timer.c		\
-			drivers/delay_timer/generic_delay_timer.c	\
-			lib/cpus/${ARCH}/cortex_a53.S			\
-			plat/common/plat_gicv2.c			\
-			plat/common/plat_psci_common.c			\
-			plat/qti/msm8916/msm8916_bl31_setup.c		\
-			plat/qti/msm8916/msm8916_cpu_boot.c		\
-			plat/qti/msm8916/msm8916_gicv2.c		\
-			plat/qti/msm8916/msm8916_pm.c			\
-			plat/qti/msm8916/msm8916_topology.c		\
-			plat/qti/msm8916/${ARCH}/msm8916_helpers.S	\
-			plat/qti/msm8916/${ARCH}/uartdm_console.S
+BL31_SOURCES		+=	${MSM8916_PM_SOURCES}				\
+				plat/qti/msm8916/msm8916_bl31_setup.c
+
+PLAT_INCLUDES		:=	-Iplat/qti/msm8916/include
+
+ifeq (${ARCH},aarch64)
+# arm_macros.S exists only on aarch64 currently
+PLAT_INCLUDES		+=	-Iinclude/plat/arm/common/${ARCH}
+endif
 
 # Only BL31 is supported at the moment and is entered on a single CPU
 RESET_TO_BL31			:= 1
 COLD_BOOT_SINGLE_CPU		:= 1
 
-# Build config flags
-# ------------------
-BL31_BASE			?= 0x86500000
-BL32_BASE			?= 0x86000000
-PRELOADED_BL33_BASE		?= 0x8f600000
-
 # Have different sections for code and rodata
 SEPARATE_CODE_AND_RODATA	:= 1
 
@@ -46,11 +50,14 @@
 ENABLE_SPE_FOR_NS		:= 0
 ENABLE_SVE_FOR_NS		:= 0
 
-# Disable workarounds unnecessary for Cortex-A53
+# Disable workarounds unnecessary for Cortex-A7/A53
 WORKAROUND_CVE_2017_5715	:= 0
 WORKAROUND_CVE_2022_23960	:= 0
 
-# MSM8916 uses ARM Cortex-A53 r0p0 so likely all the errata apply
+ifeq (${MSM8916_CPU},cortex_a53)
+# The Cortex-A53 revision varies depending on the SoC revision.
+# msm8916 uses r0p0, msm8939 uses r0p1 or r0p4. Enable all errata
+# and rely on the runtime detection to apply them only if needed.
 ERRATA_A53_819472		:= 1
 ERRATA_A53_824069		:= 1
 ERRATA_A53_826319		:= 1
@@ -58,8 +65,31 @@
 ERRATA_A53_835769		:= 1
 ERRATA_A53_836870		:= 1
 ERRATA_A53_843419		:= 1
-ERRATA_A53_855873		:= 0	# Workaround works only for >= r0p3
+ERRATA_A53_855873		:= 1
 ERRATA_A53_1530924		:= 1
+endif
 
+# Build config flags
+# ------------------
+BL31_BASE			?= 0x86500000
+PRELOADED_BL33_BASE		?= 0x8f600000
+
+ifeq (${ARCH},aarch64)
+BL32_BASE			?= BL31_LIMIT
 $(eval $(call add_define,BL31_BASE))
+else
+# There is no BL31 on aarch32, so reuse its location for BL32
+BL32_BASE			?= $(BL31_BASE)
+endif
 $(eval $(call add_define,BL32_BASE))
+
+# UART number to use for TF-A output during early boot
+QTI_UART_NUM			?= 2
+$(eval $(call assert_numeric,QTI_UART_NUM))
+$(eval $(call add_define,QTI_UART_NUM))
+
+# Set to 1 on the command line to keep using UART after early boot.
+# Requires reserving the UART and related clocks inside the normal world.
+QTI_RUNTIME_UART		?= 0
+$(eval $(call assert_boolean,QTI_RUNTIME_UART))
+$(eval $(call add_define,QTI_RUNTIME_UART))
diff --git a/plat/qti/msm8916/sp_min/msm8916_sp_min_setup.c b/plat/qti/msm8916/sp_min/msm8916_sp_min_setup.c
new file mode 100644
index 0000000..3c93305
--- /dev/null
+++ b/plat/qti/msm8916/sp_min/msm8916_sp_min_setup.c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <common/debug.h>
+#include <lib/xlat_tables/xlat_mmu_helpers.h>
+#include <platform_sp_min.h>
+
+#include "../msm8916_config.h"
+#include "../msm8916_setup.h"
+
+static struct {
+	entry_point_info_t bl33;
+} image_ep_info = {
+	/* BL33 entry point */
+	SET_STATIC_PARAM_HEAD(bl33, PARAM_EP, VERSION_1,
+			      entry_point_info_t, NON_SECURE),
+	.bl33.pc = PRELOADED_BL33_BASE,
+	.bl33.spsr = SPSR_MODE32(MODE32_hyp, SPSR_T_ARM, SPSR_E_LITTLE,
+				 DISABLE_ALL_EXCEPTIONS),
+};
+
+void sp_min_early_platform_setup2(u_register_t arg0, u_register_t arg1,
+				  u_register_t arg2, u_register_t arg3)
+{
+	msm8916_early_platform_setup();
+	msm8916_configure_early();
+}
+
+void sp_min_plat_arch_setup(void)
+{
+	msm8916_plat_arch_setup(BL32_BASE, BL32_END - BL32_BASE);
+	enable_mmu_svc_mon(0);
+}
+
+void sp_min_platform_setup(void)
+{
+	INFO("SP_MIN: Platform setup start\n");
+	msm8916_platform_setup();
+	msm8916_configure();
+	INFO("SP_MIN: Platform setup done\n");
+}
+
+entry_point_info_t *sp_min_plat_get_bl33_ep_info(void)
+{
+	return &image_ep_info.bl33;
+}
diff --git a/plat/qti/msm8916/sp_min/sp_min-msm8916.mk b/plat/qti/msm8916/sp_min/sp_min-msm8916.mk
new file mode 100644
index 0000000..be07f94
--- /dev/null
+++ b/plat/qti/msm8916/sp_min/sp_min-msm8916.mk
@@ -0,0 +1,11 @@
+#
+# Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+BL32_SOURCES	+=	${MSM8916_PM_SOURCES}				\
+			plat/common/${ARCH}/platform_mp_stack.S		\
+			plat/qti/msm8916/sp_min/msm8916_sp_min_setup.c
+
+override RESET_TO_SP_MIN := 1
diff --git a/plat/qti/msm8916/tsp/msm8916_tsp_setup.c b/plat/qti/msm8916/tsp/msm8916_tsp_setup.c
new file mode 100644
index 0000000..218af57
--- /dev/null
+++ b/plat/qti/msm8916/tsp/msm8916_tsp_setup.c
@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2023, Stephan Gerhold <stephan@gerhold.net>
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <platform_tsp.h>
+
+#include "../msm8916_setup.h"
+#include <platform_def.h>
+
+void tsp_early_platform_setup(void)
+{
+	msm8916_early_platform_setup();
+}
+
+void tsp_plat_arch_setup(void)
+{
+	msm8916_plat_arch_setup(BL32_BASE, BL32_END - BL32_BASE);
+	enable_mmu_el1(0);
+}
+
+void tsp_platform_setup(void)
+{
+	INFO("TSP: Platform setup start\n");
+	msm8916_platform_setup();
+	INFO("TSP: Platform setup done\n");
+
+	console_switch_state(CONSOLE_FLAG_RUNTIME);
+}
diff --git a/plat/qti/msm8916/tsp/tsp-msm8916.mk b/plat/qti/msm8916/tsp/tsp-msm8916.mk
new file mode 100644
index 0000000..0d50058
--- /dev/null
+++ b/plat/qti/msm8916/tsp/tsp-msm8916.mk
@@ -0,0 +1,8 @@
+#
+# Copyright (c) 2023, Stephan Gerhold <stephan@gerhold.net>
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+BL32_SOURCES	+=	plat/common/${ARCH}/platform_mp_stack.S		\
+			plat/qti/msm8916/tsp/msm8916_tsp_setup.c
diff --git a/plat/qti/msm8939/platform.mk b/plat/qti/msm8939/platform.mk
new file mode 100644
index 0000000..9bf6d4d
--- /dev/null
+++ b/plat/qti/msm8939/platform.mk
@@ -0,0 +1,11 @@
+#
+# Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Cache cannot be enabled early on MSM8939 because the CCI-400 must be
+# enabled before the CPUs in both clusters become cache-coherent.
+override WARMBOOT_ENABLE_DCACHE_EARLY := 0
+
+include plat/qti/msm8916/platform.mk
diff --git a/plat/qti/msm8939/sp_min/sp_min-msm8939.mk b/plat/qti/msm8939/sp_min/sp_min-msm8939.mk
new file mode 100644
index 0000000..28a6f01
--- /dev/null
+++ b/plat/qti/msm8939/sp_min/sp_min-msm8939.mk
@@ -0,0 +1,7 @@
+#
+# Copyright (c) 2022-2023, Stephan Gerhold <stephan@gerhold.net>
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include plat/qti/msm8916/sp_min/sp_min-msm8916.mk
diff --git a/plat/qti/msm8939/tsp/tsp-msm8939.mk b/plat/qti/msm8939/tsp/tsp-msm8939.mk
new file mode 100644
index 0000000..4eefc64
--- /dev/null
+++ b/plat/qti/msm8939/tsp/tsp-msm8939.mk
@@ -0,0 +1,7 @@
+#
+# Copyright (c) 2023, Stephan Gerhold <stephan@gerhold.net>
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include plat/qti/msm8916/tsp/tsp-msm8916.mk
diff --git a/plat/renesas/rcar/bl2_plat_setup.c b/plat/renesas/rcar/bl2_plat_setup.c
index 9ec4bcd..81ee93e 100644
--- a/plat/renesas/rcar/bl2_plat_setup.c
+++ b/plat/renesas/rcar/bl2_plat_setup.c
@@ -236,6 +236,56 @@
 	       mmio_read_32(AXI_DCMPAREACRA0 + 0x8 * no),
 	       mmio_read_32(AXI_DCMPAREACRB0 + 0x8 * no));
 }
+
+static int bl2_create_reserved_memory(void)
+{
+	int ret;
+
+	int fcnlnode = fdt_add_subnode(fdt, 0, "reserved-memory");
+	if (fcnlnode < 0) {
+		NOTICE("BL2: Cannot create reserved mem node (ret=%i)\n",
+			fcnlnode);
+		panic();
+	}
+
+	ret = fdt_setprop(fdt, fcnlnode, "ranges", NULL, 0);
+	if (ret < 0) {
+		NOTICE("BL2: Cannot add FCNL ranges prop (ret=%i)\n", ret);
+		panic();
+	}
+
+	ret = fdt_setprop_u32(fdt, fcnlnode, "#address-cells", 2);
+	if (ret < 0) {
+		NOTICE("BL2: Cannot add FCNL #address-cells prop (ret=%i)\n", ret);
+		panic();
+	}
+
+	ret = fdt_setprop_u32(fdt, fcnlnode, "#size-cells", 2);
+	if (ret < 0) {
+		NOTICE("BL2: Cannot add FCNL #size-cells prop (ret=%i)\n", ret);
+		panic();
+	}
+
+	return fcnlnode;
+}
+
+static void bl2_create_fcnl_reserved_memory(void)
+{
+	int fcnlnode;
+
+	NOTICE("BL2: Lossy Decomp areas\n");
+
+	fcnlnode = bl2_create_reserved_memory();
+
+	bl2_lossy_setting(0, LOSSY_ST_ADDR0, LOSSY_END_ADDR0,
+			  LOSSY_FMT0, LOSSY_ENA_DIS0, fcnlnode);
+	bl2_lossy_setting(1, LOSSY_ST_ADDR1, LOSSY_END_ADDR1,
+			  LOSSY_FMT1, LOSSY_ENA_DIS1, fcnlnode);
+	bl2_lossy_setting(2, LOSSY_ST_ADDR2, LOSSY_END_ADDR2,
+			  LOSSY_FMT2, LOSSY_ENA_DIS2, fcnlnode);
+}
+#else
+static void bl2_create_fcnl_reserved_memory(void) {}
 #endif
 
 void bl2_plat_flush_bl31_params(void)
@@ -820,9 +870,6 @@
 #else
 	const char *boot_hyper160 = "HyperFlash(160MHz)";
 #endif
-#if (RCAR_LOSSY_ENABLE == 1)
-	int fcnlnode;
-#endif
 
 	bl2_init_generic_timer();
 
@@ -1099,23 +1146,8 @@
 		reg &= ~((uint32_t) 1 << 12);
 		mmio_write_32(CPG_PLL0CR, reg);
 	}
-#if (RCAR_LOSSY_ENABLE == 1)
-	NOTICE("BL2: Lossy Decomp areas\n");
 
-	fcnlnode = fdt_add_subnode(fdt, 0, "reserved-memory");
-	if (fcnlnode < 0) {
-		NOTICE("BL2: Cannot create reserved mem node (ret=%i)\n",
-			fcnlnode);
-		panic();
-	}
-
-	bl2_lossy_setting(0, LOSSY_ST_ADDR0, LOSSY_END_ADDR0,
-			  LOSSY_FMT0, LOSSY_ENA_DIS0, fcnlnode);
-	bl2_lossy_setting(1, LOSSY_ST_ADDR1, LOSSY_END_ADDR1,
-			  LOSSY_FMT1, LOSSY_ENA_DIS1, fcnlnode);
-	bl2_lossy_setting(2, LOSSY_ST_ADDR2, LOSSY_END_ADDR2,
-			  LOSSY_FMT2, LOSSY_ENA_DIS2, fcnlnode);
-#endif
+	bl2_create_fcnl_reserved_memory();
 
 	fdt_pack(fdt);
 	NOTICE("BL2: FDT at %p\n", fdt);
diff --git a/plat/xilinx/common/include/plat_fdt.h b/plat/xilinx/common/include/plat_fdt.h
new file mode 100644
index 0000000..a1ee1e1
--- /dev/null
+++ b/plat/xilinx/common/include/plat_fdt.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2023, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+#ifndef PLAT_FDT_H
+#define PLAT_FDT_H
+
+void prepare_dtb(void);
+
+#endif /* PLAT_FDT_H */
diff --git a/plat/xilinx/common/include/plat_startup.h b/plat/xilinx/common/include/plat_startup.h
index d1c5303..5270e13 100644
--- a/plat/xilinx/common/include/plat_startup.h
+++ b/plat/xilinx/common/include/plat_startup.h
@@ -10,34 +10,34 @@
 
 #include <common/bl_common.h>
 
-/* For FSBL handover */
-enum fsbl_handoff {
-	FSBL_HANDOFF_SUCCESS = 0,
-	FSBL_HANDOFF_NO_STRUCT,
-	FSBL_HANDOFF_INVAL_STRUCT,
-	FSBL_HANDOFF_TOO_MANY_PARTS
+/* For Xilinx bootloader XBL handover */
+enum xbl_handoff {
+	XBL_HANDOFF_SUCCESS = 0,
+	XBL_HANDOFF_NO_STRUCT,
+	XBL_HANDOFF_INVAL_STRUCT,
+	XBL_HANDOFF_TOO_MANY_PARTS
 };
 
-#define FSBL_MAX_PARTITIONS		8U
+#define XBL_MAX_PARTITIONS		8U
 
 /* Structure corresponding to each partition entry */
-struct xfsbl_partition {
+struct xbl_partition {
 	uint64_t entry_point;
 	uint64_t flags;
 };
 
 /* Structure for handoff parameters to TrustedFirmware-A (TF-A) */
-struct xfsbl_tfa_handoff_params {
+struct xbl_handoff_params {
 	uint8_t magic[4];
 	uint32_t num_entries;
-	struct xfsbl_partition partition[FSBL_MAX_PARTITIONS];
+	struct xbl_partition partition[XBL_MAX_PARTITIONS];
 };
 
-#define TFA_HANDOFF_PARAMS_MAX_SIZE	sizeof(struct xfsbl_tfa_handoff_params)
+#define HANDOFF_PARAMS_MAX_SIZE	 sizeof(struct xbl_handoff_params)
 
-enum fsbl_handoff fsbl_tfa_handover(entry_point_info_t *bl32,
+enum xbl_handoff xbl_handover(entry_point_info_t *bl32,
 					entry_point_info_t *bl33,
-					uint64_t tfa_handoff_addr);
+					uint64_t handoff_addr);
 
 /* JEDEC Standard Manufacturer's Identification Code and Bank ID JEP106 */
 #define JEDEC_XILINX_MFID	U(0x49)
diff --git a/plat/xilinx/common/include/pm_api_sys.h b/plat/xilinx/common/include/pm_api_sys.h
index e8a9627..3fcb62f 100644
--- a/plat/xilinx/common/include/pm_api_sys.h
+++ b/plat/xilinx/common/include/pm_api_sys.h
@@ -68,7 +68,7 @@
 					uint32_t flag);
 enum pm_ret_status pm_get_chipid(uint32_t *value);
 
-/**
+/*
  * Assigning of argument values into array elements.
  */
 #define PM_PACK_PAYLOAD1(pl, mid, flag, arg0) {	\
diff --git a/plat/xilinx/common/include/pm_common.h b/plat/xilinx/common/include/pm_common.h
index af7ca87..c0308ab 100644
--- a/plat/xilinx/common/include/pm_common.h
+++ b/plat/xilinx/common/include/pm_common.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2013-2018, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -33,10 +34,11 @@
 				 TZ_VERSION_MINOR)
 
 /**
- * pm_ipi - struct for capturing IPI-channel specific info
- * @local_ipi_id	Local IPI agent ID
- * @remote_ipi_id	Remote IPI Agent ID
- * @buffer_base	base address for payload buffer
+ * struct pm_ipi - struct for capturing IPI-channel specific info.
+ * @local_ipi_id: Local IPI agent ID.
+ * @remote_ipi_id: Remote IPI Agent ID.
+ * @buffer_base: base address for payload buffer.
+ *
  */
 struct pm_ipi {
 	const uint32_t local_ipi_id;
@@ -45,11 +47,12 @@
 };
 
 /**
- * pm_proc - struct for capturing processor related info
- * @node_id	node-ID of the processor
- * @pwrdn_mask	cpu-specific mask to be used for power control register
- * @ipi		pointer to IPI channel structure
- *		(in APU all processors share one IPI channel)
+ * struct pm_proc - struct for capturing processor related info.
+ * @node_id: node-ID of the processor.
+ * @pwrdn_mask: cpu-specific mask to be used for power control register.
+ * @ipi: pointer to IPI channel structure.
+ *       (in APU all processors share one IPI channel)
+ *
  */
 struct pm_proc {
 	const uint32_t node_id;
diff --git a/plat/xilinx/common/include/pm_defs.h b/plat/xilinx/common/include/pm_defs.h
index c5587fd..b80cf45 100644
--- a/plat/xilinx/common/include/pm_defs.h
+++ b/plat/xilinx/common/include/pm_defs.h
@@ -59,7 +59,9 @@
  * Enum definitions
  ********************************************************************/
 
-//ioctl id
+/*
+ * ioctl id
+ */
 enum {
 	IOCTL_GET_RPU_OPER_MODE = 0,
 	IOCTL_SET_RPU_OPER_MODE = 1,
@@ -98,16 +100,18 @@
 };
 
 /**
- * @PM_PLL_PARAM_DIV2:		Enable for divide by 2 function inside the PLL
- * @PM_PLL_PARAM_FBDIV:		Feedback divisor integer portion for the PLL
- * @PM_PLL_PARAM_DATA:		Feedback divisor fractional portion for the PLL
- * @PM_PLL_PARAM_PRE_SRC:	Clock source for PLL input
- * @PM_PLL_PARAM_POST_SRC:	Clock source for PLL Bypass mode
- * @PM_PLL_PARAM_LOCK_DLY:	Lock circuit config settings for lock windowsize
- * @PM_PLL_PARAM_LOCK_CNT:	Lock circuit counter setting
- * @PM_PLL_PARAM_LFHF:		PLL loop filter high frequency capacitor control
- * @PM_PLL_PARAM_CP:		PLL charge pump control
- * @PM_PLL_PARAM_RES:		PLL loop filter resistor control
+ * enum pm_pll_param - enum represents the parameters for a phase-locked loop.
+ * @PM_PLL_PARAM_DIV2: Enable for divide by 2 function inside the PLL.
+ * @PM_PLL_PARAM_FBDIV: Feedback divisor integer portion for the PLL.
+ * @PM_PLL_PARAM_DATA: Feedback divisor fractional portion for the PLL.
+ * @PM_PLL_PARAM_PRE_SRC: Clock source for PLL input.
+ * @PM_PLL_PARAM_POST_SRC: Clock source for PLL Bypass mode.
+ * @PM_PLL_PARAM_LOCK_DLY: Lock circuit config settings for lock windowsize.
+ * @PM_PLL_PARAM_LOCK_CNT: Lock circuit counter setting.
+ * @PM_PLL_PARAM_LFHF: PLL loop filter high frequency capacitor control.
+ * @PM_PLL_PARAM_CP: PLL charge pump control.
+ * @PM_PLL_PARAM_RES: PLL loop filter resistor control.
+ * @PM_PLL_PARAM_MAX: Represents the maximum parameter value for the PLL
  */
 enum pm_pll_param {
 	PM_PLL_PARAM_DIV2,
@@ -205,7 +209,7 @@
 	PM_OPCHAR_TYPE_LATENCY,
 };
 
-/**
+/*
  * Subsystem IDs
  */
 typedef enum {
@@ -223,20 +227,24 @@
 
 /* TODO: move pm_ret_status from device specific location to common location */
 /**
- * @PM_RET_SUCCESS:		success
- * @PM_RET_ERROR_ARGS:		illegal arguments provided (deprecated)
- * @PM_RET_ERROR_NOTSUPPORTED:	feature not supported  (deprecated)
- * @PM_RET_ERROR_NOFEATURE:	feature is not available
- * @PM_RET_ERROR_INVALID_CRC:	invalid crc in IPI communication
- * @PM_RET_ERROR_NOT_ENABLED:   feature is not enabled
- * @PM_RET_ERROR_INTERNAL:	internal error
- * @PM_RET_ERROR_CONFLICT:	conflict
- * @PM_RET_ERROR_ACCESS:	access rights violation
- * @PM_RET_ERROR_INVALID_NODE:	invalid node
- * @PM_RET_ERROR_DOUBLE_REQ:	duplicate request for same node
- * @PM_RET_ERROR_ABORT_SUSPEND:	suspend procedure has been aborted
- * @PM_RET_ERROR_TIMEOUT:	timeout in communication with PMU
- * @PM_RET_ERROR_NODE_USED:	node is already in use
+ * enum pm_ret_status - enum represents the return status codes for a PM
+ *                      operation.
+ * @PM_RET_SUCCESS: success.
+ * @PM_RET_ERROR_ARGS: illegal arguments provided (deprecated).
+ * @PM_RET_ERROR_NOTSUPPORTED: feature not supported  (deprecated).
+ * @PM_RET_ERROR_NOFEATURE: feature is not available.
+ * @PM_RET_ERROR_INVALID_CRC: invalid crc in IPI communication.
+ * @PM_RET_ERROR_NOT_ENABLED: feature is not enabled.
+ * @PM_RET_ERROR_INTERNAL: internal error.
+ * @PM_RET_ERROR_CONFLICT: conflict.
+ * @PM_RET_ERROR_ACCESS: access rights violation.
+ * @PM_RET_ERROR_INVALID_NODE: invalid node.
+ * @PM_RET_ERROR_DOUBLE_REQ: duplicate request for same node.
+ * @PM_RET_ERROR_ABORT_SUSPEND: suspend procedure has been aborted.
+ * @PM_RET_ERROR_TIMEOUT: timeout in communication with PMU.
+ * @PM_RET_ERROR_NODE_USED: node is already in use.
+ * @PM_RET_ERROR_NO_FEATURE: indicates that the requested feature is not
+ *                           supported.
  */
 enum pm_ret_status {
 	PM_RET_SUCCESS,
@@ -256,7 +264,7 @@
 	PM_RET_ERROR_NO_FEATURE = 2008
 };
 
-/**
+/*
  * Qids
  */
 enum pm_query_id {
diff --git a/plat/xilinx/common/include/pm_ipi.h b/plat/xilinx/common/include/pm_ipi.h
index 27fab7f..976abd6 100644
--- a/plat/xilinx/common/include/pm_ipi.h
+++ b/plat/xilinx/common/include/pm_ipi.h
@@ -9,8 +9,9 @@
 #ifndef PM_IPI_H
 #define PM_IPI_H
 
-#include <plat_ipi.h>
 #include <stddef.h>
+
+#include <plat_ipi.h>
 #include "pm_common.h"
 
 #define IPI_BLOCKING		1
diff --git a/plat/xilinx/common/include/pm_node.h b/plat/xilinx/common/include/pm_node.h
index b6c2d81..46f6bcf 100644
--- a/plat/xilinx/common/include/pm_node.h
+++ b/plat/xilinx/common/include/pm_node.h
@@ -188,54 +188,54 @@
 	XPM_NODEIDX_DEV_GT_10 = 0x53,
 
 #if defined(PLAT_versal_net)
-	XPM_NODEIDX_DEV_ACPU_0_0 = 0x54,
-	XPM_NODEIDX_DEV_ACPU_0_1 = 0x55,
-	XPM_NODEIDX_DEV_ACPU_0_2 = 0x56,
-	XPM_NODEIDX_DEV_ACPU_0_3 = 0x57,
-	XPM_NODEIDX_DEV_ACPU_1_0 = 0x58,
-	XPM_NODEIDX_DEV_ACPU_1_1 = 0x59,
-	XPM_NODEIDX_DEV_ACPU_1_2 = 0x5A,
-	XPM_NODEIDX_DEV_ACPU_1_3 = 0x5B,
-	XPM_NODEIDX_DEV_ACPU_2_0 = 0x5C,
-	XPM_NODEIDX_DEV_ACPU_2_1 = 0x5D,
-	XPM_NODEIDX_DEV_ACPU_2_2 = 0x5E,
-	XPM_NODEIDX_DEV_ACPU_2_3 = 0x5F,
-	XPM_NODEIDX_DEV_ACPU_3_0 = 0x60,
-	XPM_NODEIDX_DEV_ACPU_3_1 = 0x61,
-	XPM_NODEIDX_DEV_ACPU_3_2 = 0x62,
-	XPM_NODEIDX_DEV_ACPU_3_3 = 0x63,
-	XPM_NODEIDX_DEV_RPU_A_0 = 0x64,
-	XPM_NODEIDX_DEV_RPU_A_1 = 0x65,
-	XPM_NODEIDX_DEV_RPU_B_0 = 0x66,
-	XPM_NODEIDX_DEV_RPU_B_1 = 0x67,
-	XPM_NODEIDX_DEV_OCM_0_0 = 0x68,
-	XPM_NODEIDX_DEV_OCM_0_1 = 0x69,
-	XPM_NODEIDX_DEV_OCM_0_2 = 0x6A,
-	XPM_NODEIDX_DEV_OCM_0_3 = 0x6B,
-	XPM_NODEIDX_DEV_OCM_1_0 = 0x6C,
-	XPM_NODEIDX_DEV_OCM_1_1 = 0x6D,
-	XPM_NODEIDX_DEV_OCM_1_2 = 0x6E,
-	XPM_NODEIDX_DEV_OCM_1_3 = 0x6F,
-	XPM_NODEIDX_DEV_TCM_A_0A = 0x70,
-	XPM_NODEIDX_DEV_TCM_A_0B = 0x71,
-	XPM_NODEIDX_DEV_TCM_A_0C = 0x72,
-	XPM_NODEIDX_DEV_TCM_A_1A = 0x73,
-	XPM_NODEIDX_DEV_TCM_A_1B = 0x74,
-	XPM_NODEIDX_DEV_TCM_A_1C = 0x75,
-	XPM_NODEIDX_DEV_TCM_B_0A = 0x76,
-	XPM_NODEIDX_DEV_TCM_B_0B = 0x77,
-	XPM_NODEIDX_DEV_TCM_B_0C = 0x78,
-	XPM_NODEIDX_DEV_TCM_B_1A = 0x79,
-	XPM_NODEIDX_DEV_TCM_B_1B = 0x7A,
-	XPM_NODEIDX_DEV_TCM_B_1C = 0x7B,
-	XPM_NODEIDX_DEV_USB_1 = 0x7C,
-	XPM_NODEIDX_DEV_PMC_WWDT = 0x7D,
-	XPM_NODEIDX_DEV_LPD_SWDT_0 = 0x7E,
-	XPM_NODEIDX_DEV_LPD_SWDT_1 = 0x7F,
-	XPM_NODEIDX_DEV_FPD_SWDT_0 = 0x80,
-	XPM_NODEIDX_DEV_FPD_SWDT_1 = 0x81,
-	XPM_NODEIDX_DEV_FPD_SWDT_2 = 0x82,
-	XPM_NODEIDX_DEV_FPD_SWDT_3 = 0x83,
+	XPM_NODEIDX_DEV_ACPU_0_0 = 0xAF,
+	XPM_NODEIDX_DEV_ACPU_0_1 = 0xB0,
+	XPM_NODEIDX_DEV_ACPU_0_2 = 0xB1,
+	XPM_NODEIDX_DEV_ACPU_0_3 = 0xB2,
+	XPM_NODEIDX_DEV_ACPU_1_0 = 0xB3,
+	XPM_NODEIDX_DEV_ACPU_1_1 = 0xB4,
+	XPM_NODEIDX_DEV_ACPU_1_2 = 0xB5,
+	XPM_NODEIDX_DEV_ACPU_1_3 = 0xB6,
+	XPM_NODEIDX_DEV_ACPU_2_0 = 0xB7,
+	XPM_NODEIDX_DEV_ACPU_2_1 = 0xB8,
+	XPM_NODEIDX_DEV_ACPU_2_2 = 0xB9,
+	XPM_NODEIDX_DEV_ACPU_2_3 = 0xBA,
+	XPM_NODEIDX_DEV_ACPU_3_0 = 0xBB,
+	XPM_NODEIDX_DEV_ACPU_3_1 = 0xBC,
+	XPM_NODEIDX_DEV_ACPU_3_2 = 0xBD,
+	XPM_NODEIDX_DEV_ACPU_3_3 = 0xBE,
+	XPM_NODEIDX_DEV_RPU_A_0 = 0xBF,
+	XPM_NODEIDX_DEV_RPU_A_1 = 0xC0,
+	XPM_NODEIDX_DEV_RPU_B_0 = 0xC1,
+	XPM_NODEIDX_DEV_RPU_B_1 = 0xC2,
+	XPM_NODEIDX_DEV_OCM_0_0 = 0xC3,
+	XPM_NODEIDX_DEV_OCM_0_1 = 0xC4,
+	XPM_NODEIDX_DEV_OCM_0_2 = 0xC5,
+	XPM_NODEIDX_DEV_OCM_0_3 = 0xC6,
+	XPM_NODEIDX_DEV_OCM_1_0 = 0xC7,
+	XPM_NODEIDX_DEV_OCM_1_1 = 0xC8,
+	XPM_NODEIDX_DEV_OCM_1_2 = 0xC9,
+	XPM_NODEIDX_DEV_OCM_1_3 = 0xCA,
+	XPM_NODEIDX_DEV_TCM_A_0A = 0xCB,
+	XPM_NODEIDX_DEV_TCM_A_0B = 0xCC,
+	XPM_NODEIDX_DEV_TCM_A_0C = 0xCD,
+	XPM_NODEIDX_DEV_TCM_A_1A = 0xCE,
+	XPM_NODEIDX_DEV_TCM_A_1B = 0xCF,
+	XPM_NODEIDX_DEV_TCM_A_1C = 0xD0,
+	XPM_NODEIDX_DEV_TCM_B_0A = 0xD1,
+	XPM_NODEIDX_DEV_TCM_B_0B = 0xD2,
+	XPM_NODEIDX_DEV_TCM_B_0C = 0xD3,
+	XPM_NODEIDX_DEV_TCM_B_1A = 0xD4,
+	XPM_NODEIDX_DEV_TCM_B_1B = 0xD5,
+	XPM_NODEIDX_DEV_TCM_B_1C = 0xD6,
+	XPM_NODEIDX_DEV_USB_1 = 0xD7,
+	XPM_NODEIDX_DEV_PMC_WWDT = 0xD8,
+	XPM_NODEIDX_DEV_LPD_SWDT_0 = 0xD9,
+	XPM_NODEIDX_DEV_LPD_SWDT_1 = 0xDA,
+	XPM_NODEIDX_DEV_FPD_SWDT_0 = 0xDB,
+	XPM_NODEIDX_DEV_FPD_SWDT_1 = 0xDC,
+	XPM_NODEIDX_DEV_FPD_SWDT_2 = 0xDD,
+	XPM_NODEIDX_DEV_FPD_SWDT_3 = 0xDE,
 #endif
 	XPM_NODEIDX_DEV_MAX,
 };
diff --git a/plat/xilinx/common/ipi.c b/plat/xilinx/common/ipi.c
index 9c169ab..399d283 100644
--- a/plat/xilinx/common/ipi.c
+++ b/plat/xilinx/common/ipi.c
@@ -1,6 +1,7 @@
 /*
  * Copyright (c) 2017-2020, Arm Limited and Contributors. All rights reserved.
  * Copyright (c) 2020-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -45,10 +46,9 @@
 static uint32_t ipi_total;
 
 /**
- * ipi_config_init() - Initialize IPI configuration data
- *
- * @ipi_config_table  - IPI configuration table
- * @ipi_total - Total number of IPI available
+ * ipi_config_table_init() - Initialize IPI configuration data.
+ * @ipi_config_table: IPI configuration table.
+ * @total_ipi: Total number of IPI available.
  *
  */
 void ipi_config_table_init(const struct ipi_config *ipi_config_table,
@@ -58,12 +58,13 @@
 	ipi_total = total_ipi;
 }
 
-/* is_ipi_mb_within_range() - verify if IPI mailbox is within range
+/**
+ * is_ipi_mb_within_range() - verify if IPI mailbox is within range.
+ * @local: local IPI ID.
+ * @remote: remote IPI ID.
  *
- * @local  - local IPI ID
- * @remote - remote IPI ID
+ * Return: - 1 if within range, 0 if not.
  *
- * return - 1 if within range, 0 if not
  */
 static inline int is_ipi_mb_within_range(uint32_t local, uint32_t remote)
 {
@@ -77,13 +78,13 @@
 }
 
 /**
- * ipi_mb_validate() - validate IPI mailbox access
+ * ipi_mb_validate() - validate IPI mailbox access.
+ * @local: local IPI ID.
+ * @remote: remote IPI ID.
+ * @is_secure: indicate if the requester is from secure software.
  *
- * @local  - local IPI ID
- * @remote - remote IPI ID
- * @is_secure - indicate if the requester is from secure software
+ * Return: 0 success, negative value for errors.
  *
- * return - 0 success, negative value for errors
  */
 int ipi_mb_validate(uint32_t local, uint32_t remote, unsigned int is_secure)
 {
@@ -104,9 +105,8 @@
 
 /**
  * ipi_mb_open() - Open IPI mailbox.
- *
- * @local  - local IPI ID
- * @remote - remote IPI ID
+ * @local: local IPI ID.
+ * @remote: remote IPI ID.
  *
  */
 void ipi_mb_open(uint32_t local, uint32_t remote)
@@ -119,9 +119,8 @@
 
 /**
  * ipi_mb_release() - Open IPI mailbox.
- *
- * @local  - local IPI ID
- * @remote - remote IPI ID
+ * @local: local IPI ID.
+ * @remote: remote IPI ID.
  *
  */
 void ipi_mb_release(uint32_t local, uint32_t remote)
@@ -131,13 +130,13 @@
 }
 
 /**
- * ipi_mb_enquire_status() - Enquire IPI mailbox status
+ * ipi_mb_enquire_status() - Enquire IPI mailbox status.
+ * @local: local IPI ID.
+ * @remote: remote IPI ID.
  *
- * @local  - local IPI ID
- * @remote - remote IPI ID
+ * Return: 0 idle, positive value for pending sending or receiving,
+ *         negative value for errors.
  *
- * return - 0 idle, positive value for pending sending or receiving,
- *          negative value for errors
  */
 int ipi_mb_enquire_status(uint32_t local, uint32_t remote)
 {
@@ -156,11 +155,11 @@
 	return ret;
 }
 
-/* ipi_mb_notify() - Trigger IPI mailbox notification
- *
- * @local - local IPI ID
- * @remote - remote IPI ID
- * @is_blocking - if to trigger the notification in blocking mode or not.
+/**
+ * ipi_mb_notify() - Trigger IPI mailbox notification.
+ * @local: local IPI ID.
+ * @remote: remote IPI ID.
+ * @is_blocking: if to trigger the notification in blocking mode or not.
  *
  * It sets the remote bit in the IPI agent trigger register.
  *
@@ -179,10 +178,10 @@
 	}
 }
 
-/* ipi_mb_ack() - Ack IPI mailbox notification from the other end
- *
- * @local - local IPI ID
- * @remote - remote IPI ID
+/**
+ * ipi_mb_ack() - Ack IPI mailbox notification from the other end.
+ * @local: local IPI ID.
+ * @remote: remote IPI ID.
  *
  * It will clear the remote bit in the isr register.
  *
@@ -193,10 +192,10 @@
 		      IPI_BIT_MASK(remote));
 }
 
-/* ipi_mb_disable_irq() - Disable IPI mailbox notification interrupt
- *
- * @local - local IPI ID
- * @remote - remote IPI ID
+/**
+ * ipi_mb_disable_irq() - Disable IPI mailbox notification interrupt.
+ * @local: local IPI ID.
+ * @remote: remote IPI ID.
  *
  * It will mask the remote bit in the idr register.
  *
@@ -207,10 +206,10 @@
 		      IPI_BIT_MASK(remote));
 }
 
-/* ipi_mb_enable_irq() - Enable IPI mailbox notification interrupt
- *
- * @local - local IPI ID
- * @remote - remote IPI ID
+/**
+ * ipi_mb_enable_irq() - Enable IPI mailbox notification interrupt.
+ * @local: local IPI ID.
+ * @remote: remote IPI ID.
  *
  * It will mask the remote bit in the idr register.
  *
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 330288c..434cd88 100644
--- a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
+++ b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2017-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -17,11 +18,10 @@
 #include <lib/mmio.h>
 
 #include <ipi.h>
+#include "ipi_mailbox_svc.h"
 #include <plat_ipi.h>
 #include <plat_private.h>
 
-#include "ipi_mailbox_svc.h"
-
 /*********************************************************************
  * Macros definitions
  ********************************************************************/
@@ -46,21 +46,25 @@
 #define UNSIGNED32_MASK			0xFFFFFFFFU /* 32bit mask */
 
 /**
- * ipi_smc_handler() - SMC handler for IPI SMC calls
+ * ipi_smc_handler() - SMC handler for IPI SMC calls.
+ * @smc_fid: Function identifier.
+ * @x1: Arguments.
+ * @x2: Arguments.
+ * @x3: Arguments.
+ * @x4: Arguments.
+ * @cookie: Unused.
+ * @handle: Pointer to caller's context structure.
+ * @flags: SECURE_FLAG or NON_SECURE_FLAG.
  *
- * @smc_fid - Function identifier
- * @x1 - x4 - Arguments
- * @cookie  - Unused
- * @handler - Pointer to caller's context structure
- *
- * @return  - Unused
+ * 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 unknown SMC Function ID.
  *
  * The SMC calls for PM service are forwarded from SIP Service SMC handler
- * function with rt_svc_handle signature
+ * 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, const void *cookie,
diff --git a/plat/xilinx/common/plat_fdt.c b/plat/xilinx/common/plat_fdt.c
new file mode 100644
index 0000000..3d12d51
--- /dev/null
+++ b/plat/xilinx/common/plat_fdt.c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2023, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+#include <common/fdt_fixup.h>
+#include <common/fdt_wrappers.h>
+
+#include <plat_fdt.h>
+#include <platform_def.h>
+
+#if (defined(XILINX_OF_BOARD_DTB_ADDR) && !IS_TFA_IN_OCM(BL31_BASE))
+void prepare_dtb(void)
+{
+	void *dtb = (void *)XILINX_OF_BOARD_DTB_ADDR;
+	int ret;
+
+	/* Return if no device tree is detected */
+	if (fdt_check_header(dtb) != 0) {
+		NOTICE("Can't read DT at %p\n", dtb);
+		return;
+	}
+
+	ret = fdt_open_into(dtb, dtb, XILINX_OF_BOARD_DTB_MAX_SIZE);
+	if (ret < 0) {
+		ERROR("Invalid Device Tree at %p: error %d\n", dtb, ret);
+		return;
+	}
+
+	/* Reserve memory used by Trusted Firmware. */
+	if (fdt_add_reserved_memory(dtb, "tf-a", BL31_BASE, BL31_LIMIT - BL31_BASE + 1)) {
+		WARN("Failed to add reserved memory nodes for BL31 to DT.\n");
+		return;
+	}
+
+	ret = fdt_pack(dtb);
+	if (ret < 0) {
+		ERROR("Failed to pack Device Tree at %p: error %d\n", dtb, ret);
+		return;
+	}
+
+	clean_dcache_range((uintptr_t)dtb, fdt_blob_size(dtb));
+	INFO("Changed device tree to advertise PSCI and reserved memories.\n");
+}
+#else
+void prepare_dtb(void)
+{
+}
+#endif
diff --git a/plat/xilinx/common/plat_startup.c b/plat/xilinx/common/plat_startup.c
index 007c045..f45c9f0 100644
--- a/plat/xilinx/common/plat_startup.c
+++ b/plat/xilinx/common/plat_startup.c
@@ -15,7 +15,7 @@
 
 
 /*
- * TFAHandoffParams
+ * HandoffParams
  * Parameter		bitfield	encoding
  * -----------------------------------------------------------------------------
  * Exec State		0		0 -> Aarch64, 1-> Aarch32
@@ -23,93 +23,104 @@
  * secure (TZ)		2		0 -> Non secure, 1 -> secure
  * EL			3:4		00 -> EL0, 01 -> EL1, 10 -> EL2, 11 -> EL3
  * CPU#			5:6		00 -> A53_0, 01 -> A53_1, 10 -> A53_2, 11 -> A53_3
+ * Reserved		7:10		Reserved
+ * Cluster#		11:12		00 -> Cluster 0, 01 -> Cluster 1, 10 -> Cluster 2,
+ *					11 -> Cluster (Applicable for Versal NET only).
+ * Reserved		13:16		Reserved
  */
 
-#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 XBL_FLAGS_ESTATE_SHIFT		0U
+#define XBL_FLAGS_ESTATE_MASK		(1U << XBL_FLAGS_ESTATE_SHIFT)
+#define XBL_FLAGS_ESTATE_A64		0U
+#define XBL_FLAGS_ESTATE_A32		1U
 
-#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 XBL_FLAGS_ENDIAN_SHIFT		1U
+#define XBL_FLAGS_ENDIAN_MASK		(1U << XBL_FLAGS_ENDIAN_SHIFT)
+#define XBL_FLAGS_ENDIAN_LE		0U
+#define XBL_FLAGS_ENDIAN_BE		1U
 
-#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 XBL_FLAGS_TZ_SHIFT		2U
+#define XBL_FLAGS_TZ_MASK		(1U << XBL_FLAGS_TZ_SHIFT)
+#define XBL_FLAGS_NON_SECURE		0U
+#define XBL_FLAGS_SECURE		1U
 
-#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 XBL_FLAGS_EL_SHIFT		3U
+#define XBL_FLAGS_EL_MASK		(3U << XBL_FLAGS_EL_SHIFT)
+#define XBL_FLAGS_EL0			0U
+#define XBL_FLAGS_EL1			1U
+#define XBL_FLAGS_EL2			2U
+#define XBL_FLAGS_EL3			3U
 
-#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 XBL_FLAGS_CPU_SHIFT		5U
+#define XBL_FLAGS_CPU_MASK		(3U << XBL_FLAGS_CPU_SHIFT)
+#define XBL_FLAGS_A53_0		0U
+#define XBL_FLAGS_A53_1		1U
+#define XBL_FLAGS_A53_2		2U
+#define XBL_FLAGS_A53_3		3U
+
+#if defined(PLAT_versal_net)
+#define XBL_FLAGS_CLUSTER_SHIFT		11U
+#define XBL_FLAGS_CLUSTER_MASK		GENMASK(11, 12)
+
+#define XBL_FLAGS_CLUSTER_0		0U
+#endif /* PLAT_versal_net */
 
 /**
- * @partition: Pointer to partition struct
+ * get_xbl_cpu() - Get the target CPU for partition.
+ * @partition: Pointer to partition struct.
  *
- * Get the target CPU for @partition.
+ * Return: XBL_FLAGS_A53_0, XBL_FLAGS_A53_1, XBL_FLAGS_A53_2 or XBL_FLAGS_A53_3.
  *
- * Return: FSBL_FLAGS_A53_0, FSBL_FLAGS_A53_1, FSBL_FLAGS_A53_2 or FSBL_FLAGS_A53_3
  */
-static int32_t get_fsbl_cpu(const struct xfsbl_partition *partition)
+static int32_t get_xbl_cpu(const struct xbl_partition *partition)
 {
-	uint64_t flags = partition->flags & FSBL_FLAGS_CPU_MASK;
+	uint64_t flags = partition->flags & XBL_FLAGS_CPU_MASK;
 
-	return flags >> FSBL_FLAGS_CPU_SHIFT;
+	return flags >> XBL_FLAGS_CPU_SHIFT;
 }
 
 /**
- * @partition: Pointer to partition struct
+ * get_xbl_el() - Get the target exception level for partition.
+ * @partition: Pointer to partition struct.
  *
- * Get the target exception level for @partition.
+ * Return: XBL_FLAGS_EL0, XBL_FLAGS_EL1, XBL_FLAGS_EL2 or XBL_FLAGS_EL3.
  *
- * Return: FSBL_FLAGS_EL0, FSBL_FLAGS_EL1, FSBL_FLAGS_EL2 or FSBL_FLAGS_EL3
  */
-static int32_t get_fsbl_el(const struct xfsbl_partition *partition)
+static int32_t get_xbl_el(const struct xbl_partition *partition)
 {
-	uint64_t flags = partition->flags & FSBL_FLAGS_EL_MASK;
+	uint64_t flags = partition->flags & XBL_FLAGS_EL_MASK;
 
-	return flags >> FSBL_FLAGS_EL_SHIFT;
+	return flags >> XBL_FLAGS_EL_SHIFT;
 }
 
 /**
- * @partition: Pointer to partition struct
+ * get_xbl_ss() - Get the target security state for partition.
+ * @partition: Pointer to partition struct.
  *
- * Get the target security state for @partition.
+ * Return: XBL_FLAGS_NON_SECURE or XBL_FLAGS_SECURE.
  *
- * Return: FSBL_FLAGS_NON_SECURE or FSBL_FLAGS_SECURE
  */
-static int32_t get_fsbl_ss(const struct xfsbl_partition *partition)
+static int32_t get_xbl_ss(const struct xbl_partition *partition)
 {
-	uint64_t flags = partition->flags & FSBL_FLAGS_TZ_MASK;
+	uint64_t flags = partition->flags & XBL_FLAGS_TZ_MASK;
 
-	return flags >> FSBL_FLAGS_TZ_SHIFT;
+	return flags >> XBL_FLAGS_TZ_SHIFT;
 }
 
 /**
- * @partition: Pointer to partition struct
+ * get_xbl_endian() - Get the target endianness for partition.
+ * @partition: Pointer to partition struct.
  *
- * Get the target endianness for @partition.
+ * Return: SPSR_E_LITTLE or SPSR_E_BIG.
  *
- * Return: SPSR_E_LITTLE or SPSR_E_BIG
  */
-static int32_t get_fsbl_endian(const struct xfsbl_partition *partition)
+static int32_t get_xbl_endian(const struct xbl_partition *partition)
 {
-	uint64_t flags = partition->flags & FSBL_FLAGS_ENDIAN_MASK;
+	uint64_t flags = partition->flags & XBL_FLAGS_ENDIAN_MASK;
 
-	flags >>= FSBL_FLAGS_ENDIAN_SHIFT;
+	flags >>= XBL_FLAGS_ENDIAN_SHIFT;
 
-	if (flags == FSBL_FLAGS_ENDIAN_BE) {
+	if (flags == XBL_FLAGS_ENDIAN_BE) {
 		return SPSR_E_BIG;
 	} else {
 		return SPSR_E_LITTLE;
@@ -117,57 +128,73 @@
 }
 
 /**
- * @partition: Pointer to partition struct
+ * get_xbl_estate() - Get the target execution state for partition.
+ * @partition: Pointer to partition struct.
  *
- * Get the target execution state for @partition.
+ * Return: XBL_FLAGS_ESTATE_A32 or XBL_FLAGS_ESTATE_A64.
  *
- * Return: FSBL_FLAGS_ESTATE_A32 or FSBL_FLAGS_ESTATE_A64
  */
-static int32_t get_fsbl_estate(const struct xfsbl_partition *partition)
+static int32_t get_xbl_estate(const struct xbl_partition *partition)
 {
-	uint64_t flags = partition->flags & FSBL_FLAGS_ESTATE_MASK;
+	uint64_t flags = partition->flags & XBL_FLAGS_ESTATE_MASK;
 
-	return flags >> FSBL_FLAGS_ESTATE_SHIFT;
+	return flags >> XBL_FLAGS_ESTATE_SHIFT;
 }
 
+#if defined(PLAT_versal_net)
 /**
- * Populates the bl32 and bl33 image info structures
- * @bl32:	BL32 image info structure
- * @bl33:	BL33 image info structure
- * tfa_handoff_addr:  TF-A handoff address
+ * get_xbl_cluster - Get the cluster number
+ * @partition: pointer to the partition structure.
  *
- * Process the handoff parameters from the FSBL and populate the BL32 and BL33
+ * Return: cluster number for the partition.
+ */
+static int32_t get_xbl_cluster(const struct xbl_partition *partition)
+{
+	uint64_t flags = partition->flags & XBL_FLAGS_CLUSTER_MASK;
+
+	return (int32_t)(flags >> XBL_FLAGS_CLUSTER_SHIFT);
+}
+#endif /* PLAT_versal_net */
+
+/**
+ * xbl_tfa_handover() - Populates the bl32 and bl33 image info structures.
+ * @bl32: BL32 image info structure.
+ * @bl33: BL33 image info structure.
+ * @tfa_handoff_addr: TF-A handoff address.
+ *
+ * Process the handoff parameters from the XBL and populate the BL32 and BL33
  * image info structures accordingly.
  *
  * Return: Return the status of the handoff. The value will be from the
- *         fsbl_handoff enum.
+ *         xbl_handoff enum.
+ *
  */
-enum fsbl_handoff fsbl_tfa_handover(entry_point_info_t *bl32,
+enum xbl_handoff xbl_handover(entry_point_info_t *bl32,
 					entry_point_info_t *bl33,
-					uint64_t tfa_handoff_addr)
+					uint64_t handoff_addr)
 {
-	const struct xfsbl_tfa_handoff_params *TFAHandoffParams;
-	if (!tfa_handoff_addr) {
-		WARN("BL31: No TFA handoff structure passed\n");
-		return FSBL_HANDOFF_NO_STRUCT;
+	const struct xbl_handoff_params *HandoffParams;
+
+	if (!handoff_addr) {
+		WARN("BL31: No handoff structure passed\n");
+		return XBL_HANDOFF_NO_STRUCT;
 	}
 
-	TFAHandoffParams = (struct xfsbl_tfa_handoff_params *)tfa_handoff_addr;
-	if ((TFAHandoffParams->magic[0] != 'X') ||
-	    (TFAHandoffParams->magic[1] != 'L') ||
-	    (TFAHandoffParams->magic[2] != 'N') ||
-	    (TFAHandoffParams->magic[3] != 'X')) {
-		ERROR("BL31: invalid TF-A handoff structure at %" PRIx64 "\n",
-		      tfa_handoff_addr);
-		return FSBL_HANDOFF_INVAL_STRUCT;
+	HandoffParams = (struct xbl_handoff_params *)handoff_addr;
+	if ((HandoffParams->magic[0] != 'X') ||
+	    (HandoffParams->magic[1] != 'L') ||
+	    (HandoffParams->magic[2] != 'N') ||
+	    (HandoffParams->magic[3] != 'X')) {
+		ERROR("BL31: invalid handoff structure at %" PRIx64 "\n", handoff_addr);
+		return XBL_HANDOFF_INVAL_STRUCT;
 	}
 
 	VERBOSE("BL31: TF-A handoff params at:0x%" PRIx64 ", entries:%u\n",
-		tfa_handoff_addr, TFAHandoffParams->num_entries);
-	if (TFAHandoffParams->num_entries > FSBL_MAX_PARTITIONS) {
+		handoff_addr, HandoffParams->num_entries);
+	if (HandoffParams->num_entries > XBL_MAX_PARTITIONS) {
 		ERROR("BL31: TF-A handoff params: too many partitions (%u/%u)\n",
-		      TFAHandoffParams->num_entries, FSBL_MAX_PARTITIONS);
-		return FSBL_HANDOFF_TOO_MANY_PARTS;
+		      HandoffParams->num_entries, XBL_MAX_PARTITIONS);
+		return XBL_HANDOFF_TOO_MANY_PARTS;
 	}
 
 	/*
@@ -175,43 +202,55 @@
 	 * (bl32, bl33). I.e. the last applicable images in the handoff
 	 * structure will be used for the hand off
 	 */
-	for (size_t i = 0; i < TFAHandoffParams->num_entries; i++) {
+	for (size_t i = 0; i < HandoffParams->num_entries; i++) {
 		entry_point_info_t *image;
 		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,
-			TFAHandoffParams->partition[i].entry_point,
-			TFAHandoffParams->partition[i].flags);
+			HandoffParams->partition[i].entry_point,
+			HandoffParams->partition[i].flags);
+
+#if defined(PLAT_versal_net)
+		uint32_t target_cluster;
 
-		target_cpu = get_fsbl_cpu(&TFAHandoffParams->partition[i]);
-		if (target_cpu != FSBL_FLAGS_A53_0) {
+		target_cluster = get_xbl_cluster(&HandoffParams->partition[i]);
+		if (target_cluster != XBL_FLAGS_CLUSTER_0) {
+			WARN("BL31: invalid target Cluster (%i)\n",
+			     target_cluster);
+			continue;
+		}
+#endif /* PLAT_versal_net */
+
+		target_cpu = get_xbl_cpu(&HandoffParams->partition[i]);
+		if (target_cpu != XBL_FLAGS_A53_0) {
 			WARN("BL31: invalid target CPU (%i)\n", target_cpu);
 			continue;
 		}
 
-		target_el = get_fsbl_el(&TFAHandoffParams->partition[i]);
-		if ((target_el == FSBL_FLAGS_EL3) ||
-		    (target_el == FSBL_FLAGS_EL0)) {
-			WARN("BL31: invalid exception level (%i)\n", target_el);
+		target_el = get_xbl_el(&HandoffParams->partition[i]);
+		if ((target_el == XBL_FLAGS_EL3) ||
+		    (target_el == XBL_FLAGS_EL0)) {
+			WARN("BL31: invalid target exception level(%i)\n",
+			     target_el);
 			continue;
 		}
 
-		target_secure = get_fsbl_ss(&TFAHandoffParams->partition[i]);
-		if (target_secure == FSBL_FLAGS_SECURE &&
-		    target_el == FSBL_FLAGS_EL2) {
+		target_secure = get_xbl_ss(&HandoffParams->partition[i]);
+		if (target_secure == XBL_FLAGS_SECURE &&
+		    target_el == XBL_FLAGS_EL2) {
 			WARN("BL31: invalid security state (%i) for exception level (%i)\n",
 			     target_secure, target_el);
 			continue;
 		}
 
-		target_estate = get_fsbl_estate(&TFAHandoffParams->partition[i]);
-		target_endianness = get_fsbl_endian(&TFAHandoffParams->partition[i]);
+		target_estate = get_xbl_estate(&HandoffParams->partition[i]);
+		target_endianness = get_xbl_endian(&HandoffParams->partition[i]);
 
-		if (target_secure == FSBL_FLAGS_SECURE) {
+		if (target_secure == XBL_FLAGS_SECURE) {
 			image = bl32;
 
-			if (target_estate == FSBL_FLAGS_ESTATE_A32) {
+			if (target_estate == XBL_FLAGS_ESTATE_A32) {
 				bl32->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
 							 target_endianness,
 							 DISABLE_ALL_EXCEPTIONS);
@@ -222,8 +261,8 @@
 		} else {
 			image = bl33;
 
-			if (target_estate == FSBL_FLAGS_ESTATE_A32) {
-				if (target_el == FSBL_FLAGS_EL2) {
+			if (target_estate == XBL_FLAGS_ESTATE_A32) {
+				if (target_el == XBL_FLAGS_EL2) {
 					target_el = MODE32_hyp;
 				} else {
 					target_el = MODE32_sys;
@@ -233,7 +272,7 @@
 							 target_endianness,
 							 DISABLE_ALL_EXCEPTIONS);
 			} else {
-				if (target_el == FSBL_FLAGS_EL2) {
+				if (target_el == XBL_FLAGS_EL2) {
 					target_el = MODE_EL2;
 				} else {
 					target_el = MODE_EL1;
@@ -245,10 +284,10 @@
 		}
 
 		VERBOSE("Setting up %s entry point to:%" PRIx64 ", el:%x\n",
-			target_secure == FSBL_FLAGS_SECURE ? "BL32" : "BL33",
-			TFAHandoffParams->partition[i].entry_point,
+			target_secure == XBL_FLAGS_SECURE ? "BL32" : "BL33",
+			HandoffParams->partition[i].entry_point,
 			target_el);
-		image->pc = TFAHandoffParams->partition[i].entry_point;
+		image->pc = HandoffParams->partition[i].entry_point;
 
 		if (target_endianness == SPSR_E_BIG) {
 			EP_SET_EE(image->h.attr, EP_EE_BIG);
@@ -257,5 +296,5 @@
 		}
 	}
 
-	return FSBL_HANDOFF_SUCCESS;
+	return XBL_HANDOFF_SUCCESS;
 }
diff --git a/plat/xilinx/common/pm_service/pm_api_sys.c b/plat/xilinx/common/pm_service/pm_api_sys.c
index dcdd2dc..ffc39bb 100644
--- a/plat/xilinx/common/pm_service/pm_api_sys.c
+++ b/plat/xilinx/common/pm_service/pm_api_sys.c
@@ -28,9 +28,10 @@
 static uint32_t pm_shutdown_scope = XPM_SHUTDOWN_SUBTYPE_RST_SYSTEM;
 
 /**
- * pm_get_shutdown_scope() - Get the currently set shutdown scope
+ * pm_get_shutdown_scope() - Get the currently set shutdown scope.
  *
- * @return	Shutdown scope value
+ * Return: Shutdown scope value.
+ *
  */
 uint32_t pm_get_shutdown_scope(void)
 {
@@ -42,7 +43,8 @@
 /**
  * pm_client_set_wakeup_sources - Set all devices with enabled interrupts as
  *                                wake sources in the XilPM.
- * @node_id:    Node id of processor
+ * @node_id: Node id of processor.
+ *
  */
 void pm_client_set_wakeup_sources(uint32_t node_id)
 {
@@ -91,13 +93,19 @@
 }
 
 /**
- * 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
+ * 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: Arguments received per SMC64 standard.
+ * @x1: Arguments received per SMC64 standard.
+ * @x2: Arguments received per SMC64 standard.
+ * @x3: Arguments received per SMC64 standard.
+ * @x4: Arguments received per SMC64 standard.
+ * @x5: Arguments received per SMC64 standard.
+ * @result: Payload received from firmware.
  *
- * @return	 PM_RET_SUCCESS on success or error code
+ * Return: PM_RET_SUCCESS on success or error code.
+ *
  */
 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,
@@ -119,17 +127,18 @@
 
 /**
  * pm_self_suspend() - PM call for processor to suspend itself
- * @nid		Node id of the processor or subsystem
- * @latency	Requested maximum wakeup latency (not supported)
- * @state	Requested state
- * @address	Resume address
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * @nid: Node id of the processor or subsystem.
+ * @latency: Requested maximum wakeup latency (not supported).
+ * @state: Requested state.
+ * @address: Resume address.
+ * @flag: 0 - Call from secure source.
+ *        1 - Call from non-secure source.
  *
  * This is a blocking call, it will return only once PMU has responded.
  * On a wakeup, resume address will be automatically set by PMU.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_self_suspend(uint32_t nid,
 				   uint32_t latency,
@@ -160,15 +169,16 @@
 
 /**
  * pm_abort_suspend() - PM call to announce that a prior suspend request
- *			is to be aborted.
- * @reason	Reason for the abort
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ *                      is to be aborted.
+ * @reason: Reason for the abort.
+ * @flag: 0 - Call from secure source.
+ *        1 - Call from non-secure source.
  *
  * Calling PU expects the PMU to abort the initiated suspend procedure.
  * This is a non-blocking call without any acknowledge.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_abort_suspend(enum pm_abort_reason reason, uint32_t flag)
 {
@@ -188,15 +198,16 @@
 
 /**
  * pm_req_suspend() - PM call to request for another PU or subsystem to
- *		      be suspended gracefully.
- * @target	Node id of the targeted PU or subsystem
- * @ack		Flag to specify whether acknowledge is requested
- * @latency	Requested wakeup latency (not supported)
- * @state	Requested state (not supported)
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ *                    be suspended gracefully.
+ * @target: Node id of the targeted PU or subsystem.
+ * @ack: Flag to specify whether acknowledge is requested.
+ * @latency: Requested wakeup latency (not supported)
+ * @state: Requested state (not supported).
+ * @flag: 0 - Call from secure source.
+ *        1 - Call from non-secure source.
+ *
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_req_suspend(uint32_t target, uint8_t ack,
 				  uint32_t latency, uint32_t state,
@@ -216,21 +227,22 @@
 
 /**
  * pm_req_wakeup() - PM call for processor to wake up selected processor
- *		     or subsystem
- * @target	Device ID of the processor or subsystem to wake up
- * @set_address	Resume address presence indicator
- *		1 - resume address specified, 0 - otherwise
- * @address	Resume address
- * @ack		Flag to specify whether acknowledge requested
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ *                   or subsystem.
+ * @target: Device ID of the processor or subsystem to wake up.
+ * @set_address: Resume address presence indicator.
+ *               1 - resume address specified, 0 - otherwise.
+ * @address: Resume address.
+ * @ack: Flag to specify whether acknowledge requested.
+ * @flag: 0 - Call from secure source.
+ *        1 - Call from non-secure source.
  *
  * This API function is either used to power up another APU core for SMP
  * (by PSCI) or to power up an entirely different PU or subsystem, such
  * as RPU0, RPU, or PL_CORE_xx. Resume address for the target PU will be
  * automatically set by PMC.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_req_wakeup(uint32_t target, uint32_t set_address,
 				 uintptr_t address, uint8_t ack, uint32_t flag)
@@ -245,15 +257,17 @@
 }
 
 /**
- * pm_get_callbackdata() - Read from IPI response buffer
- * @data - array of PAYLOAD_ARG_CNT elements
- * @flag - 0 - Call from secure source
- *	   1 - Call from non-secure source
- * @ack - 0 - Do not ack IPI after reading payload
- *        1 - Ack IPI after reading payload
+ * pm_get_callbackdata() - Read from IPI response buffer.
+ * @data: array of PAYLOAD_ARG_CNT elements.
+ * @count: Number of values to return.
+ * @flag: 0 - Call from secure source.
+ *        1 - Call from non-secure source.
+ * @ack: 0 - Do not ack IPI after reading payload.
+ *       1 - Ack IPI after reading payload.
  *
  * Read value from ipi buffer response buffer.
- * @return	Returns status, either success or error
+ * Return: Returns status, either success or error.
+ *
  */
 enum pm_ret_status pm_get_callbackdata(uint32_t *data, size_t count, uint32_t flag, uint32_t ack)
 {
@@ -273,19 +287,19 @@
 }
 
 /**
- * pm_pll_set_param() - Set PLL parameter
+ * pm_pll_set_param() - Set PLL parameter.
+ * @clk_id: PLL clock ID.
+ * @param: PLL parameter ID.
+ * @value: Value to set for PLL parameter.
+ * @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.
  *
- * @clk_id	PLL clock ID
- * @param	PLL parameter ID
- * @value	Value to set for PLL parameter
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_pll_set_param(uint32_t clk_id, uint32_t param,
 				    uint32_t value, uint32_t flag)
@@ -300,19 +314,19 @@
 }
 
 /**
- * pm_pll_get_param() - Get PLL parameter value
+ * pm_pll_get_param() - Get PLL parameter value.
+ * @clk_id: PLL clock ID.
+ * @param: PLL parameter ID.
+ * @value: Buffer to store PLL parameter 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.
  *
- * @clk_id	PLL clock ID
- * @param	PLL parameter ID
- * @value:	Buffer to store PLL parameter value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_pll_get_param(uint32_t clk_id, uint32_t param,
 				    uint32_t *value, uint32_t flag)
@@ -327,18 +341,18 @@
 }
 
 /**
- * pm_pll_set_mode() - Set PLL mode
+ * pm_pll_set_mode() - Set PLL mode.
+ * @clk_id: PLL clock ID.
+ * @mode: PLL mode.
+ * @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.
  *
- * @clk_id	PLL clock ID
- * @mode	PLL mode
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_pll_set_mode(uint32_t clk_id, uint32_t mode,
 				   uint32_t flag)
@@ -353,18 +367,18 @@
 }
 
 /**
- * pm_pll_get_mode() - Get PLL mode
+ * pm_pll_get_mode() - Get PLL mode.
+ * @clk_id: PLL clock ID.
+ * @mode: Buffer to store PLL mode.
+ * @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.
  *
- * @clk_id	PLL clock ID
- * @mode:	Buffer to store PLL mode
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_pll_get_mode(uint32_t clk_id, uint32_t *mode,
 				   uint32_t flag)
@@ -380,13 +394,14 @@
 
 /**
  * pm_force_powerdown() - PM call to request for another PU or subsystem to
- *			  be powered down forcefully
- * @target	Device ID of the PU node to be forced powered down.
- * @ack		Flag to specify whether acknowledge is requested
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ *                        be powered down forcefully.
+ * @target: Device ID of the PU node to be forced powered down.
+ * @ack: Flag to specify whether acknowledge is requested
+ * @flag: 0 - Call from secure source
+ *        1 - Call from non-secure source
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_force_powerdown(uint32_t target, uint8_t ack,
 				      uint32_t flag)
@@ -405,13 +420,14 @@
 }
 
 /**
- * pm_system_shutdown() - PM call to request a system shutdown or restart
- * @type	Shutdown or restart? 0=shutdown, 1=restart, 2=setscope
- * @subtype	Scope: 0=APU-subsystem, 1=PS, 2=system
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * pm_system_shutdown() - PM call to request a system shutdown or restart.
+ * @type: Shutdown or restart? 0=shutdown, 1=restart, 2=setscope.
+ * @subtype: Scope: 0=APU-subsystem, 1=PS, 2=system.
+ * @flag: 0 - Call from secure source.
+ *        1 - Call from non-secure source.
+ *
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_system_shutdown(uint32_t type, uint32_t subtype,
 				      uint32_t flag)
@@ -432,22 +448,22 @@
 }
 
 /**
- * pm_query_data() -  PM API for querying firmware data
+ * 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 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
+ * Return: 0 if success else non-zero error code of type
+ *         enum pm_ret_status.
  *
- * @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)
@@ -481,25 +497,25 @@
 	return ret;
 }
 /**
- * pm_api_ioctl() -  PM IOCTL API for device control and configs
+ * pm_api_ioctl() -  PM IOCTL API for device control and configs.
+ * @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 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 0 on success or non-zero error code
- * of type enum pm_ret_status
+ * 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 arg3,
@@ -536,14 +552,16 @@
 }
 
 /**
- * pm_set_wakeup_source() - PM call to specify the wakeup source while suspended
- * @target	Device id of the targeted PU or subsystem
- * @wkup_node	Device id of the wakeup peripheral
- * @enable	Enable or disable the specified peripheral as wake source
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * pm_set_wakeup_source() - PM call to specify the wakeup source while
+ *                          suspended.
+ * @target: Device id of the targeted PU or subsystem
+ * @wkup_device: Device id of the wakeup peripheral
+ * @enable: Enable or disable the specified peripheral as wake source
+ * @flag: 0 - Call from secure source
+ *        1 - Call from non-secure source
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_set_wakeup_source(uint32_t target, uint32_t wkup_device,
 					uint8_t enable, uint32_t flag)
@@ -556,15 +574,16 @@
 }
 
 /**
- * pm_feature_check() - Returns the supported API version if supported
- * @api_id	API ID to check
- * @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
+ * pm_feature_check() - Returns the supported API version if supported.
+ * @api_id: API ID to check.
+ * @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
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_feature_check(uint32_t api_id, uint32_t *ret_payload,
 				    uint32_t flag)
@@ -601,17 +620,17 @@
 }
 
 /**
- * pm_load_pdi() - Load the PDI
+ * pm_load_pdi() - Load the PDI. This function provides support to load
+ *                 PDI from linux.
  *
- * This function provides support to load PDI from linux
+ * @src: Source device of pdi(DDR, OCM, SD etc).
+ * @address_low: lower 32-bit Linear memory space address.
+ * @address_high: higher 32-bit Linear memory space address.
+ * @flag: 0 - Call from secure source.
+ *        1 - Call from non-secure source.
  *
- * src:        Source device of pdi(DDR, OCM, SD etc)
- * address_low: lower 32-bit Linear memory space address
- * address_high: higher 32-bit Linear memory space address
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * Return: Returns status, either success or error+reason.
  *
- * @return      Returns status, either success or error+reason
  */
 enum pm_ret_status pm_load_pdi(uint32_t src, uint32_t address_low,
 			       uint32_t address_high, uint32_t flag)
@@ -626,15 +645,16 @@
 
 /**
  * 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
- * @event	Event in question
- * @wake	Wake subsystem upon capturing the event if value 1
- * @enable	Enable the registration for value 1, disable for value 0
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ *                          about the device event.
+ * @device_id: Device ID for the Node to which the event is related.
+ * @event: Event in question.
+ * @wake: Wake subsystem upon capturing the event if value 1.
+ * @enable: Enable the registration for value 1, disable for value 0.
+ * @flag: 0 - Call from secure source.
+ *        1 - Call from non-secure source.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_register_notifier(uint32_t device_id, uint32_t event,
 					uint32_t wake, uint32_t enable,
@@ -650,11 +670,11 @@
 }
 
 /**
- * pm_get_chipid() - Read silicon ID registers
- * @value:	Buffer for two 32bit words.
+ * pm_get_chipid() - Read silicon ID registers.
+ * @value: Buffer for two 32bit words.
  *
- * @return:	Returns status, either success or error+reason and,
- *		optionally, @value.
+ * Return: Returns status, either success or error+reason and,
+ *         optionally, @value.
  */
 enum pm_ret_status pm_get_chipid(uint32_t *value)
 {
diff --git a/plat/xilinx/common/pm_service/pm_ipi.c b/plat/xilinx/common/pm_service/pm_ipi.c
index 2c3cb1b..56567dd 100644
--- a/plat/xilinx/common/pm_service/pm_ipi.c
+++ b/plat/xilinx/common/pm_service/pm_ipi.c
@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2013-2020, Arm Limited and Contributors. All rights reserved.
  * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
- * Copyright (c) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -11,11 +11,11 @@
 #include <lib/bakery_lock.h>
 #include <lib/mmio.h>
 #include <lib/spinlock.h>
+#include <plat/common/platform.h>
+
 #include <ipi.h>
 #include <plat_ipi.h>
 #include <plat_private.h>
-#include <plat/common/platform.h>
-
 #include "pm_defs.h"
 #include "pm_ipi.h"
 
@@ -52,14 +52,14 @@
 
 /**
  * pm_ipi_init() - Initialize IPI peripheral for communication with
- *		   remote processor
+ *                 remote processor.
+ * @proc: Pointer to the processor who is initiating request.
  *
- * @proc	Pointer to the processor who is initiating request
- * @return	On success, the initialization function must return 0.
- *		Any other return value will cause the framework to ignore
- *		the service
+ * Return: On success, the initialization function must return 0.
+ *         Any other return value will cause the framework to ignore
+ *         the service.
  *
- * Called from pm_setup initialization function
+ * Called from pm_setup initialization function.
  */
 void pm_ipi_init(const struct pm_proc *proc)
 {
@@ -67,14 +67,16 @@
 }
 
 /**
- * pm_ipi_send_common() - Sends IPI request to the remote processor
- * @proc	Pointer to the processor who is initiating request
- * @payload	API id and call arguments to be written in IPI buffer
+ * pm_ipi_send_common() - Sends IPI request to the remote processor.
+ * @proc: Pointer to the processor who is initiating request.
+ * @payload: API id and call arguments to be written in IPI buffer.
+ * @is_blocking: if to trigger the notification in blocking mode or not.
  *
  * Send an IPI request to the power controller. Caller needs to hold
  * the 'pm_secure_lock' lock.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ipi_send_common(const struct pm_proc *proc,
 					     uint32_t payload[PAYLOAD_ARG_CNT],
@@ -103,13 +105,14 @@
 
 /**
  * pm_ipi_send_non_blocking() - Sends IPI request to the remote processor
- *			        without blocking notification
- * @proc	Pointer to the processor who is initiating request
- * @payload	API id and call arguments to be written in IPI buffer
+ *                              without blocking notification.
+ * @proc: Pointer to the processor who is initiating request.
+ * @payload: API id and call arguments to be written in IPI buffer.
  *
  * Send an IPI request to the power controller.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_ipi_send_non_blocking(const struct pm_proc *proc,
 					    uint32_t payload[PAYLOAD_ARG_CNT])
@@ -126,13 +129,14 @@
 }
 
 /**
- * pm_ipi_send() - Sends IPI request to the remote processor
- * @proc	Pointer to the processor who is initiating request
- * @payload	API id and call arguments to be written in IPI buffer
+ * pm_ipi_send() - Sends IPI request to the remote processor.
+ * @proc: Pointer to the processor who is initiating request.
+ * @payload: API id and call arguments to be written in IPI buffer.
  *
  * Send an IPI request to the power controller.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_ipi_send(const struct pm_proc *proc,
 			       uint32_t payload[PAYLOAD_ARG_CNT])
@@ -151,12 +155,13 @@
 
 /**
  * pm_ipi_buff_read() - Reads IPI response after remote processor has handled
- *			interrupt
- * @proc	Pointer to the processor who is waiting and reading response
- * @value	Used to return value from IPI buffer element (optional)
- * @count	Number of values to return in @value
+ *                      interrupt.
+ * @proc: Pointer to the processor who is waiting and reading response.
+ * @value: Used to return value from IPI buffer element (optional).
+ * @count: Number of values to return in @value.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ipi_buff_read(const struct pm_proc *proc,
 					   uint32_t *value, size_t count)
@@ -208,13 +213,15 @@
 
 /**
  * pm_ipi_buff_read_callb() - Callback function that reads value from
- *			      ipi response buffer
- * @value	Used to return value from IPI buffer element
- * @count	Number of values to return in @value
+ *                            ipi response buffer.
+ * @value: Used to return value from IPI buffer element.
+ * @count: Number of values to return in @value.
  *
  * This callback function fills requested data in @value from ipi response
  * buffer.
- * @return 	Returns status, either success or error
+ *
+ * Return: Returns status, either success or error.
+ *
  */
 enum pm_ret_status pm_ipi_buff_read_callb(uint32_t *value, size_t count)
 {
@@ -258,16 +265,17 @@
 }
 
 /**
- * pm_ipi_send_sync() - Sends IPI request to the remote processor
- * @proc	Pointer to the processor who is initiating request
- * @payload	API id and call arguments to be written in IPI buffer
- * @value	Used to return value from IPI buffer element (optional)
- * @count	Number of values to return in @value
+ * pm_ipi_send_sync() - Sends IPI request to the remote processor.
+ * @proc: Pointer to the processor who is initiating request.
+ * @payload: API id and call arguments to be written in IPI buffer.
+ * @value: Used to return value from IPI buffer element (optional).
+ * @count: Number of values to return in @value.
  *
  * Send an IPI request to the power controller and wait for it to be handled.
  *
- * @return	Returns status, either success or error+reason and, optionally,
- *		@value
+ * Return: Returns status, either success or error+reason and, optionally,
+ *         @value.
+ *
  */
 enum pm_ret_status pm_ipi_send_sync(const struct pm_proc *proc,
 				    uint32_t payload[PAYLOAD_ARG_CNT],
diff --git a/plat/xilinx/common/pm_service/pm_svc_main.c b/plat/xilinx/common/pm_service/pm_svc_main.c
index 1bd2192..fb32f2a 100644
--- a/plat/xilinx/common/pm_service/pm_svc_main.c
+++ b/plat/xilinx/common/pm_service/pm_svc_main.c
@@ -11,15 +11,18 @@
  */
 
 #include <errno.h>
-#include <plat_private.h>
 #include <stdbool.h>
+
+#include "../drivers/arm/gic/v3/gicv3_private.h"
+
 #include <common/runtime_svc.h>
+#include <drivers/arm/gicv3.h>
 #include <plat/common/platform.h>
+
+#include <plat_private.h>
 #include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_ipi.h"
-#include <drivers/arm/gicv3.h>
-#include "../drivers/arm/gic/v3/gicv3_private.h"
 
 #define MODE				0x80000000U
 
@@ -84,13 +87,13 @@
 }
 
 /**
- * pm_register_sgi() - PM register the IPI interrupt
+ * pm_register_sgi() - PM register the IPI interrupt.
+ * @sgi_num: SGI number to be used for communication.
+ * @reset: Reset to invalid SGI when reset=1.
  *
- * @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
+ * Return: On success, the initialization function must return 0.
+ *         Any other return value will cause the framework to ignore
+ *         the service.
  *
  * Update the SGI number to be used.
  *
@@ -115,17 +118,18 @@
 }
 
 /**
- * pm_setup() - PM service setup
+ * pm_setup() - PM service setup.
  *
- * @return	On success, the initialization function must return 0.
- *		Any other return value will cause the framework to ignore
- *		the service
+ * Return: On success, the initialization function must return 0.
+ *         Any other return value will cause the framework to ignore
+ *         the service.
  *
  * Initialization functions for Versal power management for
  * communicaton with PMC.
  *
  * Called from sip_svc_setup initialization function with the
  * rt_svc_init signature.
+ *
  */
 int32_t pm_setup(void)
 {
@@ -152,14 +156,19 @@
 }
 
 /**
- * eemi_for_compatibility() - EEMI calls handler for deprecated calls
+ * eemi_for_compatibility() - EEMI calls handler for deprecated calls.
+ * @api_id: identifier for the API being called.
+ * @pm_arg: pointer to the argument data for the API call.
+ * @handle: Pointer to caller's context structure.
+ * @security_flag: SECURE_FLAG or NON_SECURE_FLAG.
  *
- * @return - If EEMI API found then, uintptr_t type address, else 0
+ * Return: If EEMI API found then, uintptr_t type address, else 0.
  *
  * 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.
+ *
  */
 static uintptr_t eemi_for_compatibility(uint32_t api_id, uint32_t *pm_arg,
 					void *handle, uint32_t security_flag)
@@ -214,14 +223,21 @@
 }
 
 /**
- * eemi_psci_debugfs_handler() - EEMI API invoked from PSCI
+ * eemi_psci_debugfs_handler() - EEMI API invoked from PSCI.
+ * @api_id: identifier for the API being called.
+ * @pm_arg: pointer to the argument data for the API call.
+ * @handle: Pointer to caller's context structure.
+ * @security_flag: SECURE_FLAG or NON_SECURE_FLAG.
  *
  * 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
+ * handled using common eemi handler.
+ *
+ * Return: If EEMI API found then, uintptr_t type address, else 0.
+ *
  */
 static uintptr_t eemi_psci_debugfs_handler(uint32_t api_id, uint32_t *pm_arg,
 					   void *handle, uint32_t security_flag)
@@ -258,11 +274,18 @@
 }
 
 /**
- * TF_A_specific_handler() - SMC handler for TF-A specific functionality
+ * TF_A_specific_handler() - SMC handler for TF-A specific functionality.
+ * @api_id: identifier for the API being called.
+ * @pm_arg: pointer to the argument data for the API call.
+ * @handle: Pointer to caller's context structure.
+ * @security_flag: SECURE_FLAG or NON_SECURE_FLAG.
  *
  * 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.
+ *
+ * Return: If TF-A specific API found then, uintptr_t type address, else 0
+ *
  */
 static uintptr_t TF_A_specific_handler(uint32_t api_id, uint32_t *pm_arg,
 				       void *handle, uint32_t security_flag)
@@ -306,7 +329,11 @@
 }
 
 /**
- * eemi_handler() - Prepare EEMI payload and perform IPI transaction
+ * eemi_handler() - Prepare EEMI payload and perform IPI transaction.
+ * @api_id: identifier for the API being called.
+ * @pm_arg: pointer to the argument data for the API call.
+ * @handle: Pointer to caller's context structure.
+ * @security_flag: SECURE_FLAG or NON_SECURE_FLAG.
  *
  * EEMI - Embedded Energy Management Interface is Xilinx proprietary protocol
  * to allow communication between power management controller and different
@@ -314,6 +341,9 @@
  *
  * This handler prepares EEMI protocol payload received from kernel and performs
  * IPI transaction.
+ *
+ * Return: If EEMI API found then, uintptr_t type address, else 0
+ *
  */
 static uintptr_t eemi_handler(uint32_t api_id, uint32_t *pm_arg,
 			      void *handle, uint32_t security_flag)
@@ -345,20 +375,24 @@
 
 /**
  * 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
+ * @smc_fid: Function Identifier.
+ * @x1: SMC64 Arguments from kernel.
+ * @x2: SMC64 Arguments from kernel.
+ * @x3: SMC64 Arguments from kernel (upper 32-bits).
+ * @x4: Unused.
+ * @cookie: Unused.
+ * @handle: Pointer to caller's context structure.
+ * @flags: SECURE_FLAG or NON_SECURE_FLAG.
  *
- * @return  - Unused
+ * 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 unknown SMC Function ID.
  *
  * The SMC calls for PM service are forwarded from SIP Service SMC handler
- * function with rt_svc_handle signature
+ * 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)
diff --git a/plat/xilinx/common/versal.c b/plat/xilinx/common/versal.c
index 019c862..3ea022c 100644
--- a/plat/xilinx/common/versal.c
+++ b/plat/xilinx/common/versal.c
@@ -15,12 +15,12 @@
 
 /**
  * plat_is_smccc_feature_available() - This function checks whether SMCCC
- *					feature is availabile for platform.
- * @fid: SMCCC function id
+ *                                     feature is availabile for platform.
+ * @fid: SMCCC function id.
  *
- * Return:
- * * SMC_ARCH_CALL_SUCCESS		- if SMCCC feature is available
- * * SMC_ARCH_CALL_NOT_SUPPORTED	- Otherwise
+ * Return: SMC_ARCH_CALL_SUCCESS - if SMCCC feature is available.
+ *         SMC_ARCH_CALL_NOT_SUPPORTED - Otherwise.
+ *
  */
 int32_t plat_is_smccc_feature_available(u_register_t fid)
 {
@@ -33,12 +33,12 @@
 }
 
 /**
- * plat_get_soc_version() - Get the SOC version of the platform
+ * plat_get_soc_version() - Get the SOC version of the platform.
  *
- * This function is called when the SoC_ID_type == 0.
- * For further details please refer to section 7.4 of SMC Calling Convention
+ * Return: SiP defined SoC version in JEP-106.
  *
- * Return:  SiP defined SoC version in JEP-106
+ * This function is called when the SoC_ID_type == 0.
+ * For further details please refer to section 7.4 of SMC Calling Convention.
  */
 int32_t plat_get_soc_version(void)
 {
@@ -50,12 +50,12 @@
 }
 
 /**
- * plat_get_soc_revision() - Get the SOC revision for the platform
+ * plat_get_soc_revision() - Get the SOC revision for the platform.
+ *
+ * Return: SiP defined SoC revision.
  *
  * This function is called when the  SoC_ID_type == 1
  * For further details please refer to section 7.4 of SMC Calling Convention
- *
- * Return:  SiP defined SoC revision
  */
 int32_t plat_get_soc_revision(void)
 {
diff --git a/plat/xilinx/versal/aarch64/versal_helpers.S b/plat/xilinx/versal/aarch64/versal_helpers.S
index 30bbfb4..f868b18 100644
--- a/plat/xilinx/versal/aarch64/versal_helpers.S
+++ b/plat/xilinx/versal/aarch64/versal_helpers.S
@@ -4,9 +4,11 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <arch.h>
 #include <asm_macros.S>
+
+#include <arch.h>
 #include <drivers/arm/gicv3.h>
+
 #include <platform_def.h>
 
 	.globl	plat_secondary_cold_boot_setup
diff --git a/plat/xilinx/versal/bl31_versal_setup.c b/plat/xilinx/versal/bl31_versal_setup.c
index e5e095a..6cb529b 100644
--- a/plat/xilinx/versal/bl31_versal_setup.c
+++ b/plat/xilinx/versal/bl31_versal_setup.c
@@ -8,8 +8,7 @@
 
 #include <assert.h>
 #include <errno.h>
-#include <plat_arm.h>
-#include <plat_private.h>
+
 #include <bl31/bl31.h>
 #include <common/bl_common.h>
 #include <common/debug.h>
@@ -19,12 +18,15 @@
 #include <lib/mmio.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
-#include <versal_def.h>
+#include <plat_arm.h>
+
+#include <plat_fdt.h>
 #include <plat_private.h>
 #include <plat_startup.h>
-#include <pm_ipi.h>
-#include "pm_client.h"
 #include "pm_api_sys.h"
+#include "pm_client.h"
+#include <pm_ipi.h>
+#include <versal_def.h>
 
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
@@ -68,9 +70,9 @@
 				u_register_t arg2, u_register_t arg3)
 {
 	uint64_t tfa_handoff_addr;
-	uint32_t payload[PAYLOAD_ARG_CNT], max_size = TFA_HANDOFF_PARAMS_MAX_SIZE;
+	uint32_t payload[PAYLOAD_ARG_CNT], max_size = HANDOFF_PARAMS_MAX_SIZE;
 	enum pm_ret_status ret_status;
-	uint64_t addr[TFA_HANDOFF_PARAMS_MAX_SIZE];
+	uint64_t addr[HANDOFF_PARAMS_MAX_SIZE];
 
 	if (VERSAL_CONSOLE_IS(pl011) || (VERSAL_CONSOLE_IS(pl011_1))) {
 		static console_t versal_runtime_console;
@@ -125,14 +127,14 @@
 		tfa_handoff_addr = mmio_read_32(PMC_GLOBAL_GLOB_GEN_STORAGE4);
 	}
 
-	enum fsbl_handoff ret = fsbl_tfa_handover(&bl32_image_ep_info,
+	enum xbl_handoff ret = xbl_handover(&bl32_image_ep_info,
 						  &bl33_image_ep_info,
 						  tfa_handoff_addr);
-	if (ret == FSBL_HANDOFF_NO_STRUCT || ret == FSBL_HANDOFF_INVAL_STRUCT) {
+	if (ret == XBL_HANDOFF_NO_STRUCT || ret == XBL_HANDOFF_INVAL_STRUCT) {
 		bl31_set_default_config();
-	} else if (ret == FSBL_HANDOFF_TOO_MANY_PARTS) {
+	} else if (ret == XBL_HANDOFF_TOO_MANY_PARTS) {
 		ERROR("BL31: Error too many partitions %u\n", ret);
-	} else if (ret != FSBL_HANDOFF_SUCCESS) {
+	} else if (ret != XBL_HANDOFF_SUCCESS) {
 		panic();
 	} else {
 		INFO("BL31: PLM to TF-A handover success %u\n", ret);
@@ -190,8 +192,11 @@
 
 	return 0;
 }
+
 void bl31_platform_setup(void)
 {
+	prepare_dtb();
+
 	/* Initialize the gic cpu and distributor interfaces */
 	plat_versal_gic_driver_init();
 	plat_versal_gic_init();
@@ -219,6 +224,10 @@
 	plat_arm_interconnect_enter_coherency();
 
 	const mmap_region_t bl_regions[] = {
+#if (defined(XILINX_OF_BOARD_DTB_ADDR) && !IS_TFA_IN_OCM(BL31_BASE))
+		MAP_REGION_FLAT(XILINX_OF_BOARD_DTB_ADDR, XILINX_OF_BOARD_DTB_MAX_SIZE,
+				MT_MEMORY | MT_RW | MT_NS),
+#endif
 		MAP_REGION_FLAT(BL31_BASE, BL31_END - BL31_BASE,
 			MT_MEMORY | MT_RW | MT_SECURE),
 		MAP_REGION_FLAT(BL_CODE_BASE, BL_CODE_END - BL_CODE_BASE,
diff --git a/plat/xilinx/versal/include/plat_ipi.h b/plat/xilinx/versal/include/plat_ipi.h
index e4922e4..4f7cb5f 100644
--- a/plat/xilinx/versal/include/plat_ipi.h
+++ b/plat/xilinx/versal/include/plat_ipi.h
@@ -10,9 +10,10 @@
 #ifndef PLAT_IPI_H
 #define PLAT_IPI_H
 
-#include <ipi.h>
 #include <stdint.h>
 
+#include <ipi.h>
+
 /*********************************************************************
  * IPI agent IDs macros
  ********************************************************************/
diff --git a/plat/xilinx/versal/include/plat_pm_common.h b/plat/xilinx/versal/include/plat_pm_common.h
index 4c057b8..0b4a115 100644
--- a/plat/xilinx/versal/include/plat_pm_common.h
+++ b/plat/xilinx/versal/include/plat_pm_common.h
@@ -12,8 +12,10 @@
 #ifndef PLAT_PM_COMMON_H
 #define PLAT_PM_COMMON_H
 
-#include <common/debug.h>
 #include <stdint.h>
+
+#include <common/debug.h>
+
 #include "pm_defs.h"
 
 #define NON_SECURE_FLAG		1U
diff --git a/plat/xilinx/versal/include/plat_private.h b/plat/xilinx/versal/include/plat_private.h
index b3f6aca..48f64ea 100644
--- a/plat/xilinx/versal/include/plat_private.h
+++ b/plat/xilinx/versal/include/plat_private.h
@@ -9,8 +9,8 @@
 #ifndef PLAT_PRIVATE_H
 #define PLAT_PRIVATE_H
 
-#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <bl31/interrupt_mgmt.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 
 typedef struct versal_intr_info_type_el3 {
 	uint32_t id;
diff --git a/plat/xilinx/versal/include/platform_def.h b/plat/xilinx/versal/include/platform_def.h
index 6c1d8b6..4c0df4f 100644
--- a/plat/xilinx/versal/include/platform_def.h
+++ b/plat/xilinx/versal/include/platform_def.h
@@ -76,8 +76,29 @@
  ******************************************************************************/
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ull << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ull << 32)
+
+#define XILINX_OF_BOARD_DTB_MAX_SIZE	U(0x200000)
+
+#define PLAT_OCM_BSE			U(0xFFFE0000)
+#define PLAT_OCM_LIMIT			U(0xFFFFFFFF)
+
+#define IS_TFA_IN_OCM(x)	((x >= PLAT_OCM_BASE) && (x < PLAT_OCM_LIMIT))
+
+#ifndef MAX_MMAP_REGIONS
+#if (defined(XILINX_OF_BOARD_DTB_ADDR) && !IS_TFA_IN_OCM(BL31_BASE))
+#define MAX_MMAP_REGIONS		9
+#else
 #define MAX_MMAP_REGIONS		8
-#define MAX_XLAT_TABLES			5
+#endif
+#endif
+
+#ifndef MAX_XLAT_TABLES
+#if !IS_TFA_IN_OCM(BL31_BASE)
+#define MAX_XLAT_TABLES		9
+#else
+#define MAX_XLAT_TABLES		5
+#endif
+#endif
 
 #define CACHE_WRITEBACK_SHIFT	6
 #define CACHE_WRITEBACK_GRANULE	(1 << CACHE_WRITEBACK_SHIFT)
diff --git a/plat/xilinx/versal/plat_psci.c b/plat/xilinx/versal/plat_psci.c
index b901e3d..56d98f7 100644
--- a/plat/xilinx/versal/plat_psci.c
+++ b/plat/xilinx/versal/plat_psci.c
@@ -1,21 +1,23 @@
 /*
  * Copyright (c) 2018-2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <assert.h>
-#include <plat_arm.h>
-#include <plat_private.h>
-#include <pm_common.h>
+
 #include <common/debug.h>
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
-#include <plat/common/platform.h>
 #include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+#include <plat_arm.h>
 
+#include <plat_private.h>
 #include "pm_api_sys.h"
 #include "pm_client.h"
+#include <pm_common.h>
 
 static uintptr_t versal_sec_entry;
 
@@ -44,9 +46,9 @@
 
 /**
  * versal_pwr_domain_suspend() - This function sends request to PMC to suspend
- * core.
+ *                               core.
+ * @target_state: Targated state.
  *
- * @target_state	Targated state
  */
 static void versal_pwr_domain_suspend(const psci_power_state_t *target_state)
 {
@@ -81,9 +83,9 @@
 
 /**
  * versal_pwr_domain_suspend_finish() - This function performs actions to finish
- * suspend procedure.
+ *                                      suspend procedure.
+ * @target_state: Targated state.
  *
- * @target_state	Targated state
  */
 static void versal_pwr_domain_suspend_finish(
 					const psci_power_state_t *target_state)
@@ -120,8 +122,9 @@
 }
 
 /**
- * versal_system_off() - This function sends the system off request
- * to firmware.  This function does not return.
+ * versal_system_off() - This function sends the system off request to firmware.
+ *                       This function does not return.
+ *
  */
 static void __dead2 versal_system_off(void)
 {
@@ -135,8 +138,10 @@
 }
 
 /**
- * versal_system_reset() - This function sends the reset request
- * to firmware for the system to reset.  This function does not return.
+ * versal_system_reset() - This function sends the reset request to firmware
+ *                         for the system to reset.  This function does not
+ *			   return.
+ *
  */
 static void __dead2 versal_system_reset(void)
 {
@@ -150,9 +155,9 @@
 }
 
 /**
- * versal_pwr_domain_off() - This function performs actions to turn off core
+ * versal_pwr_domain_off() - This function performs actions to turn off core.
+ * @target_state: Targated state.
  *
- * @target_state	Targated state
  */
 static void versal_pwr_domain_off(const psci_power_state_t *target_state)
 {
@@ -181,12 +186,12 @@
 
 /**
  * versal_validate_power_state() - This function ensures that the power state
- * parameter in request is valid.
+ *                                 parameter in request is valid.
+ * @power_state: Power state of core.
+ * @req_state: Requested state.
  *
- * @power_state		Power state of core
- * @req_state		Requested state
+ * Return: Returns status, either success or reason.
  *
- * @return	Returns status, either success or reason
  */
 static int32_t versal_validate_power_state(uint32_t power_state,
 				       psci_power_state_t *req_state)
@@ -213,9 +218,9 @@
 }
 
 /**
- * versal_get_sys_suspend_power_state() -  Get power state for system suspend
+ * versal_get_sys_suspend_power_state() - Get power state for system suspend.
+ * @req_state: Requested state.
  *
- * @req_state	Requested state
  */
 static void versal_get_sys_suspend_power_state(psci_power_state_t *req_state)
 {
diff --git a/plat/xilinx/versal/plat_versal.c b/plat/xilinx/versal/plat_versal.c
index e561048..ba17b1d 100644
--- a/plat/xilinx/versal/plat_versal.c
+++ b/plat/xilinx/versal/plat_versal.c
@@ -4,9 +4,10 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <plat_private.h>
 #include <plat/common/platform.h>
 
+#include <plat_private.h>
+
 int32_t plat_core_pos_by_mpidr(u_register_t mpidr)
 {
 	if ((mpidr & MPIDR_CLUSTER_MASK) != 0U) {
diff --git a/plat/xilinx/versal/platform.mk b/plat/xilinx/versal/platform.mk
index 0b6aea0..c936220 100644
--- a/plat/xilinx/versal/platform.mk
+++ b/plat/xilinx/versal/platform.mk
@@ -45,12 +45,17 @@
 VERSAL_PLATFORM ?= silicon
 $(eval $(call add_define_val,VERSAL_PLATFORM,VERSAL_PLATFORM_ID_${VERSAL_PLATFORM}))
 
+ifdef XILINX_OF_BOARD_DTB_ADDR
+$(eval $(call add_define,XILINX_OF_BOARD_DTB_ADDR))
+endif
+
 PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/			\
 				-Iplat/xilinx/common/include/			\
 				-Iplat/xilinx/common/ipi_mailbox_service/	\
 				-Iplat/xilinx/versal/include/			\
 				-Iplat/xilinx/versal/pm_service/
 
+include lib/libfdt/libfdt.mk
 # Include GICv3 driver files
 include drivers/arm/gic/v3/gicv3.mk
 include lib/xlat_tables_v2/xlat_tables.mk
@@ -80,6 +85,7 @@
 				lib/cpus/aarch64/cortex_a72.S			\
 				plat/common/plat_psci_common.c			\
 				plat/xilinx/common/ipi.c			\
+				plat/xilinx/common/plat_fdt.c			\
 				plat/xilinx/common/plat_startup.c		\
 				plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c \
 				plat/xilinx/common/pm_service/pm_ipi.c		\
@@ -93,7 +99,9 @@
 				plat/xilinx/versal/sip_svc_setup.c		\
 				plat/xilinx/versal/versal_gicv3.c		\
 				plat/xilinx/versal/versal_ipi.c			\
-				plat/xilinx/versal/pm_service/pm_client.c
+				plat/xilinx/versal/pm_service/pm_client.c	\
+				common/fdt_fixup.c				\
+				${LIBFDT_SRCS}
 
 ifeq ($(HARDEN_SLS_ALL), 1)
 TF_CFLAGS_aarch64      +=      -mharden-sls=all
diff --git a/plat/xilinx/versal/pm_service/pm_client.c b/plat/xilinx/versal/pm_service/pm_client.c
index af5263d..ccbfe77 100644
--- a/plat/xilinx/versal/pm_service/pm_client.c
+++ b/plat/xilinx/versal/pm_service/pm_client.c
@@ -11,18 +11,20 @@
  */
 
 #include <assert.h>
-#include <plat_ipi.h>
-#include <platform_def.h>
-#include <versal_def.h>
+
+#include <drivers/arm/gic_common.h>
+#include <drivers/arm/gicv3.h>
 #include <lib/bakery_lock.h>
 #include <lib/mmio.h>
 #include <lib/utils.h>
-#include <drivers/arm/gicv3.h>
-#include <drivers/arm/gic_common.h>
 #include <plat/common/platform.h>
+
+#include <plat_ipi.h>
+#include <platform_def.h>
 #include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_defs.h"
+#include <versal_def.h>
 
 #define UNDEFINED_CPUID		(~0)
 
@@ -51,10 +53,11 @@
 const struct pm_proc *primary_proc = &pm_procs_all[0];
 
 /**
- * irq_to_pm_node_idx - Get PM node index corresponding to the interrupt number
- * @irq:	Interrupt number
+ * irq_to_pm_node_idx - Get PM node index corresponding to the interrupt number.
+ * @irq: Interrupt number
  *
- * Return:	PM node index corresponding to the specified interrupt
+ * Return: PM node index corresponding to the specified interrupt.
+ *
  */
 enum pm_device_node_idx irq_to_pm_node_idx(uint32_t irq)
 {
@@ -172,11 +175,14 @@
 }
 
 /**
- * pm_client_suspend() - Client-specific suspend actions
+ * pm_client_suspend() - Client-specific suspend actions.
+ * @proc: processor which need to suspend.
+ * @state: desired suspend state.
  *
  * This function should contain any PU-specific actions
  * 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, uint32_t state)
 {
@@ -194,10 +200,11 @@
 }
 
 /**
- * pm_client_abort_suspend() - Client-specific abort-suspend actions
+ * pm_client_abort_suspend() - Client-specific abort-suspend actions.
  *
  * This function should contain any PU-specific actions
- * required for aborting a prior suspend request
+ * required for aborting a prior suspend request.
+ *
  */
 void pm_client_abort_suspend(void)
 {
@@ -214,10 +221,11 @@
 }
 
 /**
- * pm_get_cpuid() - get the local cpu ID for a global node ID
- * @nid:	node id of the processor
+ * pm_get_cpuid() - get the local cpu ID for a global node ID.
+ * @nid: node id of the processor.
+ *
+ * Return: the cpu ID (starting from 0) for the subsystem.
  *
- * Return: the cpu ID (starting from 0) for the subsystem
  */
 static uint32_t pm_get_cpuid(uint32_t nid)
 {
@@ -230,10 +238,12 @@
 }
 
 /**
- * pm_client_wakeup() - Client-specific wakeup actions
+ * pm_client_wakeup() - Client-specific wakeup actions.
+ * @proc: Processor which need to wakeup.
  *
  * This function should contain any PU-specific actions
- * required for waking up another APU core
+ * required for waking up another APU core.
+ *
  */
 void pm_client_wakeup(const struct pm_proc *proc)
 {
@@ -254,10 +264,11 @@
 }
 
 /**
- * pm_get_proc() - returns pointer to the proc structure
- * @cpuid:	id of the cpu whose proc struct pointer should be returned
+ * pm_get_proc() - returns pointer to the proc structure.
+ * @cpuid: id of the cpu whose proc struct pointer should be returned.
+ *
+ * Return: pointer to a proc structure if proc is found, otherwise NULL.
  *
- * Return: pointer to a proc structure if proc is found, otherwise NULL
  */
 const struct pm_proc *pm_get_proc(uint32_t cpuid)
 {
diff --git a/plat/xilinx/versal/sip_svc_setup.c b/plat/xilinx/versal/sip_svc_setup.c
index 35118a7..b30254d 100644
--- a/plat/xilinx/versal/sip_svc_setup.c
+++ b/plat/xilinx/versal/sip_svc_setup.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018-2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,7 +41,9 @@
 /**
  * sip_svc_setup() - Setup SiP Service
  *
- * Invokes PM setup
+ * Return: 0 on success,negative error code on failure.
+ *
+ * Invokes PM setup.
  */
 static int32_t sip_svc_setup(void)
 {
@@ -51,10 +54,20 @@
 }
 
 /**
- * sip_svc_smc_handler() - Top-level SiP Service SMC handler
+ * sip_svc_smc_handler() - Top-level SiP Service SMC handler.
+ * @smc_fid: Function Identifier.
+ * @x1: SMC64 Arguments 1 from kernel.
+ * @x2: SMC64 Arguments 2 from kernel.
+ * @x3: SMC64 Arguments 3 from kernel(upper 32-bits).
+ * @x4: SMC64 Arguments 4 from kernel.
+ * @cookie: Unused
+ * @handle: Pointer to caller's context structure.
+ * @flags: SECURE_FLAG or NON_SECURE_FLAG.
  *
  * Handler for all SiP SMC calls. Handles standard SIP requests
  * and calls PM SMC handler if the call is for a PM-API function.
+ *
+ * Return: Unused.
  */
 uintptr_t sip_svc_smc_handler(uint32_t smc_fid,
 			     u_register_t x1,
diff --git a/plat/xilinx/versal/versal_gicv3.c b/plat/xilinx/versal/versal_gicv3.c
index 33d3d35..197d047 100644
--- a/plat/xilinx/versal/versal_gicv3.c
+++ b/plat/xilinx/versal/versal_gicv3.c
@@ -5,14 +5,15 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <plat_private.h>
-#include <platform_def.h>
-
 #include <common/interrupt_props.h>
 #include <drivers/arm/gicv3.h>
 #include <lib/utils.h>
 #include <plat/common/platform.h>
 
+#include <plat_private.h>
+#include <platform_def.h>
+
+
 /******************************************************************************
  * The following functions are defined as weak to allow a platform to override
  * the way the GICv3 driver is initialised and used.
diff --git a/plat/xilinx/versal/versal_ipi.c b/plat/xilinx/versal/versal_ipi.c
index 51eb759..cdee6b5 100644
--- a/plat/xilinx/versal/versal_ipi.c
+++ b/plat/xilinx/versal/versal_ipi.c
@@ -10,6 +10,7 @@
  */
 
 #include <lib/utils_def.h>
+
 #include <ipi.h>
 #include <plat_ipi.h>
 
@@ -65,10 +66,9 @@
 	},
 };
 
-/* versal_ipi_config_table_init() - Initialize versal IPI configuration data
- *
- * @ipi_config_table  - IPI configuration table
- * @ipi_total - Total number of IPI available
+/* versal_ipi_config_table_init() - Initialize versal IPI configuration data.
+ * @ipi_config_table: IPI configuration table.
+ * @ipi_total: Total number of IPI available.
  *
  */
 void versal_ipi_config_table_init(void)
diff --git a/plat/xilinx/versal_net/aarch64/versal_net_helpers.S b/plat/xilinx/versal_net/aarch64/versal_net_helpers.S
index e1e2317..bc62efc 100644
--- a/plat/xilinx/versal_net/aarch64/versal_net_helpers.S
+++ b/plat/xilinx/versal_net/aarch64/versal_net_helpers.S
@@ -6,9 +6,11 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <arch.h>
 #include <asm_macros.S>
+
+#include <arch.h>
 #include <drivers/arm/gicv3.h>
+
 #include <platform_def.h>
 
 	.globl	plat_secondary_cold_boot_setup
diff --git a/plat/xilinx/versal_net/bl31_versal_net_setup.c b/plat/xilinx/versal_net/bl31_versal_net_setup.c
index ae9dfe8..d2b6c7b 100644
--- a/plat/xilinx/versal_net/bl31_versal_net_setup.c
+++ b/plat/xilinx/versal_net/bl31_versal_net_setup.c
@@ -12,19 +12,20 @@
 #include <bl31/bl31.h>
 #include <common/bl_common.h>
 #include <common/debug.h>
-#include <common/fdt_fixup.h>
-#include <common/fdt_wrappers.h>
 #include <drivers/arm/dcc.h>
 #include <drivers/arm/pl011.h>
 #include <drivers/console.h>
 #include <lib/mmio.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
-#include <libfdt.h>
 #include <plat/common/platform.h>
 #include <plat_arm.h>
 
+#include <plat_fdt.h>
 #include <plat_private.h>
 #include <plat_startup.h>
+#include <pm_api_sys.h>
+#include <pm_client.h>
+#include <pm_ipi.h>
 #include <versal_net_def.h>
 
 static entry_point_info_t bl32_image_ep_info;
@@ -70,6 +71,11 @@
 {
 	uint32_t uart_clock;
 	int32_t rc;
+#if !(TFA_NO_PM)
+	uint64_t tfa_handoff_addr, buff[HANDOFF_PARAMS_MAX_SIZE] = {0};
+	uint32_t payload[PAYLOAD_ARG_CNT], max_size = HANDOFF_PARAMS_MAX_SIZE;
+	enum pm_ret_status ret_status;
+#endif /* !(TFA_NO_PM) */
 
 	board_detection();
 
@@ -136,8 +142,32 @@
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
 	SET_PARAM_HEAD(&bl33_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
+#if !(TFA_NO_PM)
+	PM_PACK_PAYLOAD4(payload, LOADER_MODULE_ID, 1, PM_LOAD_GET_HANDOFF_PARAMS,
+			 (uintptr_t)buff >> 32U, (uintptr_t)buff, max_size);
 
+	ret_status = pm_ipi_send_sync(primary_proc, payload, NULL, 0);
+	if (ret_status == PM_RET_SUCCESS) {
+		enum xbl_handoff xbl_ret;
+
+		tfa_handoff_addr = (uintptr_t)&buff;
+
+		xbl_ret = xbl_handover(&bl32_image_ep_info, &bl33_image_ep_info,
+				       tfa_handoff_addr);
+		if (xbl_ret != XBL_HANDOFF_SUCCESS) {
+			ERROR("BL31: PLM to TF-A handover failed %u\n", xbl_ret);
+			panic();
+		}
+
+		INFO("BL31: PLM to TF-A handover success\n");
+	} else {
+		INFO("BL31: setting up default configs\n");
+
+		bl31_set_default_config();
+	}
+#else
 	bl31_set_default_config();
+#endif /* !(TFA_NO_PM) */
 
 	NOTICE("BL31: Secure code at 0x%lx\n", bl32_image_ep_info.pc);
 	NOTICE("BL31: Non secure code at 0x%lx\n", bl33_image_ep_info.pc);
@@ -194,6 +224,8 @@
 
 void bl31_platform_setup(void)
 {
+	prepare_dtb();
+
 	/* Initialize the gic cpu and distributor interfaces */
 	plat_versal_net_gic_driver_init();
 	plat_versal_net_gic_init();
@@ -218,6 +250,10 @@
 void bl31_plat_arch_setup(void)
 {
 	const mmap_region_t bl_regions[] = {
+#if (defined(XILINX_OF_BOARD_DTB_ADDR) && !IS_TFA_IN_OCM(BL31_BASE))
+		MAP_REGION_FLAT(XILINX_OF_BOARD_DTB_ADDR, XILINX_OF_BOARD_DTB_MAX_SIZE,
+				MT_MEMORY | MT_RW | MT_NS),
+#endif
 		MAP_REGION_FLAT(BL31_BASE, BL31_END - BL31_BASE,
 			MT_MEMORY | MT_RW | MT_SECURE),
 		MAP_REGION_FLAT(BL_CODE_BASE, BL_CODE_END - BL_CODE_BASE,
diff --git a/plat/xilinx/versal_net/include/platform_def.h b/plat/xilinx/versal_net/include/platform_def.h
index b256b05..872b6ee 100644
--- a/plat/xilinx/versal_net/include/platform_def.h
+++ b/plat/xilinx/versal_net/include/platform_def.h
@@ -84,13 +84,25 @@
 
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32U)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32U)
-#if (BL31_LIMIT < PLAT_DDR_LOWMEM_MAX)
-#define MAX_MMAP_REGIONS		U(10)
+
+#define XILINX_OF_BOARD_DTB_MAX_SIZE	U(0x200000)
+
+#define PLAT_OCM_BASE			U(0xBBF00000)
+#define PLAT_OCM_LIMIT			U(0xBC000000)
+
+#define IS_TFA_IN_OCM(x)	((x >= PLAT_OCM_BASE) && (x < PLAT_OCM_LIMIT))
+
+#ifndef MAX_MMAP_REGIONS
+#if (defined(XILINX_OF_BOARD_DTB_ADDR) && !IS_TFA_IN_OCM(BL31_BASE))
+#define MAX_MMAP_REGIONS		9
 #else
-#define MAX_MMAP_REGIONS		U(9)
+#define MAX_MMAP_REGIONS		8
+#endif
 #endif
 
-#define MAX_XLAT_TABLES			U(8)
+#ifndef MAX_XLAT_TABLES
+#define MAX_XLAT_TABLES			U(9)
+#endif
 
 #define CACHE_WRITEBACK_SHIFT	U(6)
 #define CACHE_WRITEBACK_GRANULE	(1 << CACHE_WRITEBACK_SHIFT)
diff --git a/plat/xilinx/versal_net/plat_psci_pm.c b/plat/xilinx/versal_net/plat_psci_pm.c
index d39fc2e..87e25bc 100644
--- a/plat/xilinx/versal_net/plat_psci_pm.c
+++ b/plat/xilinx/versal_net/plat_psci_pm.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2022, Xilinx, Inc. All rights reserved.
- * Copyright (c) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -50,9 +50,10 @@
 }
 
 /**
- * versal_net_pwr_domain_off() - This function performs actions to turn off core
+ * versal_net_pwr_domain_off() - This function performs actions to turn off
+ *                               core.
+ * @target_state: Targeted state.
  *
- * @param target_state	Targeted state
  */
 static void versal_net_pwr_domain_off(const psci_power_state_t *target_state)
 {
@@ -80,8 +81,10 @@
 }
 
 /**
- * versal_net_system_reset() - This function sends the reset request
- * to firmware for the system to reset.  This function does not return.
+ * versal_net_system_reset() - This function sends the reset request to firmware
+ *                             for the system to reset. This function does not
+ *                             return.
+ *
  */
 static void __dead2 versal_net_system_reset(void)
 {
@@ -96,9 +99,9 @@
 
 /**
  * versal_net_pwr_domain_suspend() - This function sends request to PMC to suspend
- * core.
+ *                                   core.
+ * @target_state: Targeted state.
  *
- * @param target_state	Targeted state
  */
 static void versal_net_pwr_domain_suspend(const psci_power_state_t *target_state)
 {
@@ -140,9 +143,9 @@
 
 /**
  * versal_net_pwr_domain_suspend_finish() - This function performs actions to finish
- * suspend procedure.
+ *                                          suspend procedure.
+ * @target_state: Targeted state.
  *
- * @param target_state	Targeted state
  */
 static void versal_net_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
 {
@@ -168,7 +171,8 @@
 
 /**
  * versal_net_system_off() - This function sends the system off request
- * to firmware.  This function does not return.
+ *                           to firmware. This function does not return.
+ *
  */
 static void __dead2 versal_net_system_off(void)
 {
@@ -183,12 +187,12 @@
 
 /**
  * versal_net_validate_power_state() - This function ensures that the power state
- * parameter in request is valid.
+ *                                     parameter in request is valid.
+ * @power_state: Power state of core.
+ * @req_state: Requested state.
  *
- * @param power_state		Power state of core
- * @param req_state		Requested state
+ * Return: Returns status, either PSCI_E_SUCCESS or reason.
  *
- * @return Returns status, either PSCI_E_SUCCESS or reason
  */
 static int32_t versal_net_validate_power_state(unsigned int power_state,
 					       psci_power_state_t *req_state)
@@ -215,9 +219,10 @@
 }
 
 /**
- * versal_net_get_sys_suspend_power_state() - Get power state for system suspend
+ * versal_net_get_sys_suspend_power_state() - Get power state for system
+ *                                            suspend.
+ * @req_state: Requested state.
  *
- * @param req_state	Requested state
  */
 static void versal_net_get_sys_suspend_power_state(psci_power_state_t *req_state)
 {
diff --git a/plat/xilinx/versal_net/platform.mk b/plat/xilinx/versal_net/platform.mk
index 398ef85..9c4cfa0 100644
--- a/plat/xilinx/versal_net/platform.mk
+++ b/plat/xilinx/versal_net/platform.mk
@@ -65,6 +65,10 @@
 
 $(eval $(call add_define_val,VERSAL_NET_CONSOLE,VERSAL_NET_CONSOLE_ID_${VERSAL_NET_CONSOLE}))
 
+ifdef XILINX_OF_BOARD_DTB_ADDR
+$(eval $(call add_define,XILINX_OF_BOARD_DTB_ADDR))
+endif
+
 PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/			\
 				-Iplat/xilinx/common/include/			\
 				-Iplat/xilinx/common/ipi_mailbox_service/	\
@@ -101,7 +105,8 @@
 else
 BL31_SOURCES		+=	${PLAT_PATH}/plat_psci.c
 endif
-BL31_SOURCES		+=	plat/xilinx/common/plat_startup.c		\
+BL31_SOURCES		+=	plat/xilinx/common/plat_fdt.c			\
+				plat/xilinx/common/plat_startup.c		\
 				plat/xilinx/common/ipi.c			\
 				plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c \
 				plat/xilinx/common/versal.c			\
diff --git a/plat/xilinx/versal_net/pm_service/pm_client.c b/plat/xilinx/versal_net/pm_service/pm_client.c
index 3387891..626611c 100644
--- a/plat/xilinx/versal_net/pm_service/pm_client.c
+++ b/plat/xilinx/versal_net/pm_service/pm_client.c
@@ -16,9 +16,8 @@
 #include <drivers/arm/gicv3.h>
 #include <lib/bakery_lock.h>
 #include <lib/mmio.h>
-#include <lib/mmio.h>
-#include <lib/utils.h>
 #include <lib/spinlock.h>
+#include <lib/utils.h>
 #include <plat/common/platform.h>
 
 #include <plat_ipi.h>
@@ -152,10 +151,11 @@
 const struct pm_proc *primary_proc = &pm_procs_all[0];
 
 /**
- * pm_get_proc() - returns pointer to the proc structure
- * @param cpuid	id of the cpu whose proc struct pointer should be returned
+ * pm_get_proc() - returns pointer to the proc structure.
+ * @cpuid: id of the cpu whose proc struct pointer should be returned.
  *
- * @return pointer to a proc structure if proc is found, otherwise NULL
+ * Return: Pointer to a proc structure if proc is found, otherwise NULL.
+ *
  */
 const struct pm_proc *pm_get_proc(uint32_t cpuid)
 {
@@ -168,10 +168,11 @@
 }
 
 /**
- * irq_to_pm_node_idx - Get PM node index corresponding to the interrupt number
- * @irq:        Interrupt number
+ * irq_to_pm_node_idx - Get PM node index corresponding to the interrupt number.
+ * @irq: Interrupt number.
+ *
+ * Return: PM node index corresponding to the specified interrupt.
  *
- * Return:      PM node index corresponding to the specified interrupt
  */
 enum pm_device_node_idx irq_to_pm_node_idx(uint32_t irq)
 {
@@ -295,14 +296,13 @@
 }
 
 /**
- * pm_client_suspend() - Client-specific suspend actions
+ * pm_client_suspend() - Client-specific suspend actions. This function
+ *                       should contain any PU-specific actions required
+ *                       prior to sending suspend request to PMU. Actions
+ *                       taken depend on the state system is suspending to.
+ * @proc: processor which need to suspend.
+ * @state: desired suspend state.
  *
- * This function should contain any PU-specific actions
- * required prior to sending suspend request to PMU
- * Actions taken depend on the state system is suspending to.
- *
- * @param proc	processor which need to suspend
- * @param state	desired suspend state
  */
 void pm_client_suspend(const struct pm_proc *proc, uint32_t state)
 {
@@ -338,10 +338,11 @@
 }
 
 /**
- * pm_get_cpuid() - get the local cpu ID for a global node ID
- * @param nid	node id of the processor
+ * pm_get_cpuid() - get the local cpu ID for a global node ID.
+ * @nid: node id of the processor.
  *
- * @return the cpu ID (starting from 0) for the subsystem
+ * Return: the cpu ID (starting from 0) for the subsystem.
+ *
  */
 static uint32_t pm_get_cpuid(uint32_t nid)
 {
@@ -354,12 +355,12 @@
 }
 
 /**
- * pm_client_wakeup() - Client-specific wakeup actions
+ * pm_client_wakeup() - Client-specific wakeup actions.
+ * @proc: Processor which need to wakeup.
  *
  * This function should contain any PU-specific actions
- * required for waking up another APU core
+ * required for waking up another APU core.
  *
- * @param proc	Processor which need to wakeup
  */
 void pm_client_wakeup(const struct pm_proc *proc)
 {
@@ -393,10 +394,11 @@
 }
 
 /**
- * pm_client_abort_suspend() - Client-specific abort-suspend actions
+ * pm_client_abort_suspend() - Client-specific abort-suspend actions.
  *
  * This function should contain any PU-specific actions
- * required for aborting a prior suspend request
+ * required for aborting a prior suspend request.
+ *
  */
 void pm_client_abort_suspend(void)
 {
diff --git a/plat/xilinx/versal_net/sip_svc_setup.c b/plat/xilinx/versal_net/sip_svc_setup.c
index f6240f3..0c27dec 100644
--- a/plat/xilinx/versal_net/sip_svc_setup.c
+++ b/plat/xilinx/versal_net/sip_svc_setup.c
@@ -43,6 +43,9 @@
 
 /**
  * sip_svc_setup() - Setup SiP Service
+ *
+ * Return: 0 on success, negative error code on failure.
+ *
  */
 static int32_t sip_svc_setup(void)
 {
diff --git a/plat/xilinx/versal_net/versal_net_ipi.c b/plat/xilinx/versal_net/versal_net_ipi.c
index ed3f2bb..0943c62 100644
--- a/plat/xilinx/versal_net/versal_net_ipi.c
+++ b/plat/xilinx/versal_net/versal_net_ipi.c
@@ -65,10 +65,10 @@
 	},
 };
 
-/* versal_net_ipi_config_table_init() - Initialize versal_net IPI configuration data
- *
- * @ipi_config_table  - IPI configuration table
- * @ipi_total - Total number of IPI available
+/* versal_net_ipi_config_table_init() - Initialize versal_net IPI configuration
+ *                                      data.
+ * @ipi_config_table: IPI configuration table.
+ * @ipi_total: Total number of IPI available.
  *
  */
 void versal_net_ipi_config_table_init(void)
diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index 18ccafd..a96c378 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -13,11 +13,12 @@
 #include <lib/mmio.h>
 #include <lib/smccc.h>
 #include <lib/xlat_tables/xlat_tables.h>
+#include <plat/common/platform.h>
+#include <services/arm_arch_svc.h>
+
 #include <plat_ipi.h>
 #include <plat_private.h>
 #include <plat_startup.h>
-#include <plat/common/platform.h>
-#include <services/arm_arch_svc.h>
 
 #include "zynqmp_pm_api_sys.h"
 
diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index b00513e..56d402f 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -11,20 +11,20 @@
 #include <bl31/bl31.h>
 #include <common/bl_common.h>
 #include <common/debug.h>
+#include <common/fdt_fixup.h>
+#include <common/fdt_wrappers.h>
 #include <drivers/arm/dcc.h>
 #include <drivers/console.h>
+#include <lib/mmio.h>
+#include <libfdt.h>
 #include <plat/arm/common/plat_arm.h>
 #include <plat/common/platform.h>
-#include <lib/mmio.h>
 
 #include <custom_svc.h>
-#include <plat_startup.h>
 #include <plat_private.h>
+#include <plat_startup.h>
 #include <zynqmp_def.h>
 
-#include <common/fdt_fixup.h>
-#include <common/fdt_wrappers.h>
-#include <libfdt.h>
 
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
@@ -112,11 +112,11 @@
 	if (zynqmp_get_bootmode() == ZYNQMP_BOOTMODE_JTAG) {
 		bl31_set_default_config();
 	} else {
-		/* use parameters from FSBL */
-		enum fsbl_handoff ret = fsbl_tfa_handover(&bl32_image_ep_info,
+		/* use parameters from XBL */
+		enum xbl_handoff ret = xbl_handover(&bl32_image_ep_info,
 							  &bl33_image_ep_info,
 							  tfa_handoff_addr);
-		if (ret != FSBL_HANDOFF_SUCCESS) {
+		if (ret != XBL_HANDOFF_SUCCESS) {
 			panic();
 		}
 	}
diff --git a/plat/xilinx/zynqmp/include/platform_def.h b/plat/xilinx/zynqmp/include/platform_def.h
index f6d9ce1..0c83a56 100644
--- a/plat/xilinx/zynqmp/include/platform_def.h
+++ b/plat/xilinx/zynqmp/include/platform_def.h
@@ -47,10 +47,10 @@
 # define BL31_LIMIT			U(0x80000)
 #endif
 #else
-# define BL31_BASE			(ZYNQMP_ATF_MEM_BASE)
-# define BL31_LIMIT			(ZYNQMP_ATF_MEM_BASE + ZYNQMP_ATF_MEM_SIZE)
+# define BL31_BASE			U(ZYNQMP_ATF_MEM_BASE)
+# define BL31_LIMIT			(UL(ZYNQMP_ATF_MEM_BASE) + U(ZYNQMP_ATF_MEM_SIZE))
 # ifdef ZYNQMP_ATF_MEM_PROGBITS_SIZE
-#  define BL31_PROGBITS_LIMIT		(ZYNQMP_ATF_MEM_BASE + ZYNQMP_ATF_MEM_PROGBITS_SIZE)
+#  define BL31_PROGBITS_LIMIT		(UL(ZYNQMP_ATF_MEM_BASE) + U(ZYNQMP_ATF_MEM_PROGBITS_SIZE))
 # endif
 #endif
 
@@ -61,8 +61,8 @@
 # define BL32_BASE			U(0x60000000)
 # define BL32_LIMIT			U(0x80000000)
 #else
-# define BL32_BASE			(ZYNQMP_BL32_MEM_BASE)
-# define BL32_LIMIT			(ZYNQMP_BL32_MEM_BASE + ZYNQMP_BL32_MEM_SIZE)
+# define BL32_BASE			U(ZYNQMP_BL32_MEM_BASE)
+# define BL32_LIMIT			(UL(ZYNQMP_BL32_MEM_BASE) + U(ZYNQMP_BL32_MEM_SIZE))
 #endif
 
 /*******************************************************************************
@@ -71,7 +71,7 @@
 #ifndef PRELOADED_BL33_BASE
 # define PLAT_ARM_NS_IMAGE_BASE	U(0x8000000)
 #else
-# define PLAT_ARM_NS_IMAGE_BASE	PRELOADED_BL33_BASE
+# define PLAT_ARM_NS_IMAGE_BASE	U(PRELOADED_BL33_BASE)
 #endif
 
 /*******************************************************************************
diff --git a/plat/xilinx/zynqmp/plat_zynqmp.c b/plat/xilinx/zynqmp/plat_zynqmp.c
index 7b9f41d..e3a979e 100644
--- a/plat/xilinx/zynqmp/plat_zynqmp.c
+++ b/plat/xilinx/zynqmp/plat_zynqmp.c
@@ -4,9 +4,10 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <plat_private.h>
 #include <plat/common/platform.h>
 
+#include <plat_private.h>
+
 int32_t plat_core_pos_by_mpidr(u_register_t mpidr)
 {
 	if (mpidr & MPIDR_CLUSTER_MASK) {
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
index 2f8f4c1..2041541 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
@@ -198,14 +198,15 @@
 	}
 
 /**
- * struct pm_clock_node - Clock topology node information
- * @type:	Topology type (mux/div1/div2/gate/pll/fixed factor)
- * @offset:	Offset in control register
- * @width:	Width of the specific type in control register
- * @clkflags:	Clk specific flags
- * @typeflags:	Type specific flags
- * @mult:	Multiplier for fixed factor
- * @div:	Divisor for fixed factor
+ * struct pm_clock_node - Clock topology node information.
+ * @type: Topology type (mux/div1/div2/gate/pll/fixed factor).
+ * @offset: Offset in control register.
+ * @width: Width of the specific type in control register.
+ * @clkflags: Clk specific flags.
+ * @typeflags: Type specific flags.
+ * @mult: Multiplier for fixed factor.
+ * @div: Divisor for fixed factor.
+ *
  */
 struct pm_clock_node {
 	uint16_t clkflags;
@@ -218,13 +219,15 @@
 };
 
 /**
- * struct pm_clock - Clock structure
- * @name:	Clock name
- * @control_reg:	Control register address
- * @status_reg:	Status register address
- * @parents:	Parents for first clock node. Lower byte indicates parent
- *		clock id and upper byte indicate flags for that id.
- * pm_clock_node:	Clock nodes
+ * struct pm_clock - Clock structure.
+ * @name: Clock name.
+ * @num_nodes: number of nodes.
+ * @control_reg: Control register address.
+ * @status_reg: Status register address.
+ * @parents: Parents for first clock node. Lower byte indicates parent
+ *           clock id and upper byte indicate flags for that id.
+ * @nodes: Clock nodes.
+ *
  */
 struct pm_clock {
 	char name[CLK_NAME_LEN];
@@ -236,8 +239,9 @@
 };
 
 /**
- * struct pm_clock - Clock structure
- * @name:		Clock name
+ * struct pm_ext_clock - Clock structure.
+ * @name: Clock name.
+ *
  */
 struct pm_ext_clock {
 	char name[CLK_NAME_LEN];
@@ -2386,8 +2390,8 @@
 };
 
 /**
- * pm_clock_valid - Check if clock is valid or not
- * @clock_id	Id of the clock to be queried
+ * pm_clock_valid - Check if clock is valid or not.
+ * @clock_id: Id of the clock to be queried.
  *
  * This function is used to check if given clock is valid
  * or not for the chip variant.
@@ -2396,6 +2400,7 @@
  * different variants.
  *
  * Return: Returns 1 if clock is valid else 0.
+ *
  */
 static bool pm_clock_valid(uint32_t clock_id)
 {
@@ -2409,12 +2414,13 @@
 }
 
 /**
- * pm_clock_type - Get clock's type
- * @clock_id	Id of the clock to be queried
+ * pm_clock_type - Get clock's type.
+ * @clock_id: Id of the clock to be queried.
  *
  * This function is used to check type of clock (OUTPUT/EXTERNAL).
  *
  * Return: Returns type of clock (OUTPUT/EXTERNAL).
+ *
  */
 static uint32_t pm_clock_type(uint32_t clock_id)
 {
@@ -2423,12 +2429,13 @@
 }
 
 /**
- * pm_api_clock_get_num_clocks() - PM call to request number of clocks
- * @nclocks	Number of clocks
+ * pm_api_clock_get_num_clocks() - PM call to request number of clocks.
+ * @nclocks: Number of clocks.
  *
  * This function is used by master to get number of clocks.
  *
- * @return	Returns success.
+ * Return: Returns success.
+ *
  */
 enum pm_ret_status pm_api_clock_get_num_clocks(uint32_t *nclocks)
 {
@@ -2438,12 +2445,13 @@
 }
 
 /**
- * pm_api_clock_get_name() - PM call to request a clock's name
- * @clock_id	Clock ID
- * @name	Name of clock (max 16 bytes)
+ * pm_api_clock_get_name() - PM call to request a clock's name.
+ * @clock_id: Clock ID.
+ * @name: Name of clock (max 16 bytes).
  *
  * This function is used by master to get nmae of clock specified
  * by given clock ID.
+ *
  */
 void pm_api_clock_get_name(uint32_t clock_id, char *name)
 {
@@ -2461,17 +2469,18 @@
 }
 
 /**
- * pm_api_clock_get_topology() - PM call to request a clock's topology
- * @clock_id	Clock ID
- * @index	Topology index for next toplogy node
- * @topology	Buffer to store nodes in topology and flags
+ * pm_api_clock_get_topology() - PM call to request a clock's topology.
+ * @clock_id: Clock ID.
+ * @index: Topology index for next toplogy node.
+ * @topology: Buffer to store nodes in topology and flags.
  *
  * This function is used by master to get topology information for the
  * clock specified by given clock ID. Each response would return 3
  * topology nodes. To get next nodes, caller needs to call this API with
  * index of next node. Index starts from 0.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_api_clock_get_topology(uint32_t clock_id,
 					     uint32_t index,
@@ -2519,15 +2528,16 @@
 
 /**
  * pm_api_clock_get_fixedfactor_params() - PM call to request a clock's fixed
- *					   factor parameters for fixed clock
- * @clock_id	Clock ID
- * @mul		Multiplication value
- * @div		Divisor value
+ *					   factor parameters for fixed clock.
+ * @clock_id: Clock ID.
+ * @mul: Multiplication value.
+ * @div: Divisor value.
  *
  * This function is used by master to get fixed factor parameers for the
  * fixed clock. This API is application only for the fixed clock.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_api_clock_get_fixedfactor_params(uint32_t clock_id,
 						       uint32_t *mul,
@@ -2566,10 +2576,10 @@
 }
 
 /**
- * pm_api_clock_get_parents() - PM call to request a clock's first 3 parents
- * @clock_id	Clock ID
- * @index	Index of next parent
- * @parents	Parents of the given clock
+ * pm_api_clock_get_parents() - PM call to request a clock's first 3 parents.
+ * @clock_id: Clock ID.
+ * @index: Index of next parent.
+ * @parents: Parents of the given clock.
  *
  * This function is used by master to get clock's parents information.
  * This API will return 3 parents with a single response. To get other
@@ -2580,7 +2590,8 @@
  * 2. Next call, index should be 3 which will return parent 3,4 and 5 and
  * so on.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_api_clock_get_parents(uint32_t clock_id,
 					    uint32_t index,
@@ -2622,14 +2633,15 @@
 }
 
 /**
- * pm_api_clock_get_attributes() - PM call to request a clock's attributes
- * @clock_id	Clock ID
- * @attr	Clock attributes
+ * pm_api_clock_get_attributes() - PM call to request a clock's attributes.
+ * @clock_id: Clock ID.
+ * @attr: Clock attributes.
  *
  * This function is used by master to get clock's attributes
  * (e.g. valid, clock type, etc).
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_api_clock_get_attributes(uint32_t clock_id,
 					       uint32_t *attr)
@@ -2648,14 +2660,15 @@
 }
 
 /**
- * pm_api_clock_get_max_divisor - PM call to get max divisor
- * @clock_id	Clock ID
- * @div_type	Divisor Type (TYPE_DIV1 or TYPE_DIV2)
- * @max_div	Maximum supported divisor
+ * pm_api_clock_get_max_divisor - PM call to get max divisor.
+ * @clock_id: Clock ID.
+ * @div_type: Divisor Type (TYPE_DIV1 or TYPE_DIV2).
+ * @max_div: Maximum supported divisor.
  *
  * This function is used by master to get maximum supported value.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_api_clock_get_max_divisor(enum clock_id clock_id,
 						uint8_t div_type,
@@ -2685,15 +2698,16 @@
 }
 
 /**
- * struct pm_pll - PLL related data required to map IOCTL-based PLL control
- * implemented by linux to system-level EEMI APIs
- * @nid:	PLL node ID
- * @cid:	PLL clock ID
- * @pre_src:	Pre-source PLL clock ID
- * @post_src:	Post-source PLL clock ID
- * @div2:	DIV2 PLL clock ID
- * @bypass:	PLL output clock ID that maps to bypass select output
- * @mode:	PLL mode currently set via IOCTL (PLL_FRAC_MODE/PLL_INT_MODE)
+ * struct pm_pll - PLL related data required to map IOCTL-based PLL control.
+ * implemented by linux to system-level EEMI APIs.
+ * @nid: PLL node ID.
+ * @cid: PLL clock ID.
+ * @pre_src: Pre-source PLL clock ID.
+ * @post_src: Post-source PLL clock ID.
+ * @div2: DIV2 PLL clock ID.
+ * @bypass: PLL output clock ID that maps to bypass select output.
+ * @mode: PLL mode currently set via IOCTL (PLL_FRAC_MODE/PLL_INT_MODE).
+ *
  */
 struct pm_pll {
 	const enum pm_node_id nid;
@@ -2745,10 +2759,11 @@
 };
 
 /**
- * pm_clock_get_pll() - Get PLL structure by PLL clock ID
- * @clock_id	Clock ID of the target PLL
+ * pm_clock_get_pll() - Get PLL structure by PLL clock ID.
+ * @clock_id: Clock ID of the target PLL.
  *
- * @return	Pointer to PLL structure if found, NULL otherwise
+ * Return: Pointer to PLL structure if found, NULL otherwise.
+ *
  */
 struct pm_pll *pm_clock_get_pll(enum clock_id clock_id)
 {
@@ -2764,11 +2779,12 @@
 }
 
 /**
- * pm_clock_get_pll_node_id() - Get PLL node ID by PLL clock ID
- * @clock_id	Clock ID of the target PLL
- * @node_id	Location to store node ID of the target PLL
+ * pm_clock_get_pll_node_id() - Get PLL node ID by PLL clock ID.
+ * @clock_id: Clock ID of the target PLL.
+ * @node_id: Location to store node ID of the target PLL.
  *
- * @return	PM_RET_SUCCESS if node ID is found, PM_RET_ERROR_ARGS otherwise
+ * Return: PM_RET_SUCCESS if node ID is found, PM_RET_ERROR_ARGS otherwise.
+ *
  */
 enum pm_ret_status pm_clock_get_pll_node_id(enum clock_id clock_id,
 					    enum pm_node_id *node_id)
@@ -2784,10 +2800,12 @@
 }
 
 /**
- * pm_clock_get_pll_by_related_clk() - Get PLL structure by PLL-related clock ID
- * @clock_id	Clock ID
+ * pm_clock_get_pll_by_related_clk() - Get PLL structure by PLL-related clock
+ *                                     ID.
+ * @clock_id: Clock ID.
  *
- * @return	Pointer to PLL structure if found, NULL otherwise
+ * Return: Pointer to PLL structure if found, NULL otherwise.
+ *
  */
 struct pm_pll *pm_clock_get_pll_by_related_clk(enum clock_id clock_id)
 {
@@ -2806,13 +2824,14 @@
 }
 
 /**
- * pm_clock_pll_enable() - "Enable" the PLL clock (lock the PLL)
- * @pll: PLL to be locked
+ * pm_clock_pll_enable() - "Enable" the PLL clock (lock the PLL).
+ * @pll: PLL to be locked.
  *
  * This function is used to map IOCTL/linux-based PLL handling to system-level
- * EEMI APIs
+ * EEMI APIs.
+ *
+ * Return: Error if the argument is not valid or status as returned by PMU.
  *
- * Return: Error if the argument is not valid or status as returned by PMU
  */
 enum pm_ret_status pm_clock_pll_enable(struct pm_pll *pll)
 {
@@ -2829,13 +2848,14 @@
 }
 
 /**
- * pm_clock_pll_disable - "Disable" the PLL clock (bypass/reset the PLL)
- * @pll		PLL to be bypassed/reset
+ * pm_clock_pll_disable - "Disable" the PLL clock (bypass/reset the PLL).
+ * @pll: PLL to be bypassed/reset.
  *
  * This function is used to map IOCTL/linux-based PLL handling to system-level
- * EEMI APIs
+ * EEMI APIs.
  *
- * Return: Error if the argument is not valid or status as returned by PMU
+ * Return: Error if the argument is not valid or status as returned by PMU.
+ *
  */
 enum pm_ret_status pm_clock_pll_disable(struct pm_pll *pll)
 {
@@ -2847,15 +2867,15 @@
 }
 
 /**
- * pm_clock_pll_get_state - Get state of the PLL
- * @pll		Pointer to the target PLL structure
- * @state	Location to store the state: 1/0 ("Enabled"/"Disabled")
+ * pm_clock_pll_get_state - Get state of the PLL.
+ * @pll: Pointer to the target PLL structure.
+ * @state: Location to store the state: 1/0 ("Enabled"/"Disabled").
  *
  * "Enable" actually means that the PLL is locked and its bypass is deasserted,
  * "Disable" means that it is bypassed.
  *
  * Return: PM_RET_ERROR_ARGS error if the argument is not valid, success if
- * returned state value is valid or an error if returned by PMU
+ *         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,
 					  uint32_t *state)
@@ -2882,16 +2902,17 @@
 }
 
 /**
- * pm_clock_pll_set_parent - Set the clock parent for PLL-related clock id
- * @pll			Target PLL structure
- * @clock_id		Id of the clock
- * @parent_index	parent index (=mux select value)
+ * pm_clock_pll_set_parent - Set the clock parent for PLL-related clock id.
+ * @pll: Target PLL structure.
+ * @clock_id: Id of the clock.
+ * @parent_index: parent index (=mux select value).
  *
  * The whole clock-tree implementation relies on the fact that parent indexes
  * match to the multiplexer select values. This function has to rely on that
  * assumption as well => parent_index is actually the mux select value.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_clock_pll_set_parent(struct pm_pll *pll,
 					   enum clock_id clock_id,
@@ -2917,14 +2938,15 @@
 }
 
 /**
- * pm_clock_pll_get_parent - Get mux select value of PLL-related clock parent
- * @pll			Target PLL structure
- * @clock_id		Id of the clock
- * @parent_index	parent index (=mux select value)
+ * pm_clock_pll_get_parent - Get mux select value of PLL-related clock parent.
+ * @pll: Target PLL structure.
+ * @clock_id: Id of the clock.
+ * @parent_index: parent index (=mux select value).
  *
  * This function is used by master to get parent index for PLL-related clock.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_clock_pll_get_parent(struct pm_pll *pll,
 					   enum clock_id clock_id,
@@ -2954,13 +2976,14 @@
 }
 
 /**
- * pm_clock_set_pll_mode() -  Set PLL mode
- * @clock_id	PLL clock id
- * @mode	Mode fractional/integer
+ * pm_clock_set_pll_mode() -  Set PLL mode.
+ * @clock_id: PLL clock id.
+ * @mode: Mode fractional/integer.
  *
  * This function buffers/saves the PLL mode that is set.
  *
- * @return      Success if mode is buffered or error if an argument is invalid
+ * 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,
 					 uint32_t mode)
@@ -2976,13 +2999,14 @@
 }
 
 /**
- * pm_clock_get_pll_mode() -  Get PLL mode
- * @clock_id	PLL clock id
- * @mode	Location to store the mode (fractional/integer)
+ * pm_clock_get_pll_mode() -  Get PLL mode.
+ * @clock_id: PLL clock id.
+ * @mode: Location to store the mode (fractional/integer).
  *
  * This function returns buffered PLL mode.
  *
- * @return      Success if mode is stored or error if an argument is invalid
+ * 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,
 					 uint32_t *mode)
@@ -2998,10 +3022,11 @@
 }
 
 /**
- * pm_clock_id_is_valid() -  Check if given clock ID is valid
- * @clock_id   ID of the clock to be checked
+ * pm_clock_id_is_valid() -  Check if given clock ID is valid.
+ * @clock_id: ID of the clock to be checked.
  *
- * @return     Returns success if clock_id is valid, otherwise an error
+ * Return: Returns success if clock_id is valid, otherwise an error.
+ *
  */
 enum pm_ret_status pm_clock_id_is_valid(uint32_t clock_id)
 {
@@ -3017,11 +3042,12 @@
 }
 
 /**
- * pm_clock_has_div() - Check if the clock has divider with given ID
- * @clock_id	Clock ID
- * @div_id	Divider ID
+ * pm_clock_has_div() - Check if the clock has divider with given ID.
+ * @clock_id: Clock ID.
+ * @div_id: Divider ID.
+ *
+ * Return: True(1)=clock has the divider, false(0)=otherwise.
  *
- * @return	True(1)=clock has the divider, false(0)=otherwise
  */
 uint8_t pm_clock_has_div(uint32_t clock_id, enum pm_clock_div_id div_id)
 {
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
index afd664e..e812ad6 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
@@ -13,22 +13,23 @@
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
 #include <plat/common/platform.h>
-#include <zynqmp_def.h>
 
 #include "pm_api_clock.h"
 #include "pm_api_ioctl.h"
 #include "pm_client.h"
 #include "pm_common.h"
 #include "pm_ipi.h"
+#include <zynqmp_def.h>
 #include "zynqmp_pm_api_sys.h"
 
 /**
- * pm_ioctl_get_rpu_oper_mode () - Get current RPU operation mode
- * @mode	Buffer to store value of oper mode(Split/Lock-step)
+ * pm_ioctl_get_rpu_oper_mode () - Get current RPU operation mode.
+ * @mode: Buffer to store value of oper mode(Split/Lock-step)
  *
  * This function provides current configured RPU operational mode.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_get_rpu_oper_mode(uint32_t *mode)
 {
@@ -46,15 +47,16 @@
 }
 
 /**
- * pm_ioctl_set_rpu_oper_mode () - Configure RPU operation mode
- * @mode	Value to set for oper mode(Split/Lock-step)
+ * pm_ioctl_set_rpu_oper_mode () - Configure RPU operation mode.
+ * @mode: Value to set for oper mode(Split/Lock-step).
  *
  * This function configures RPU operational mode(Split/Lock-step).
  * It also sets TCM combined mode in RPU lock-step and TCM non-combined
  * mode for RPU split mode. In case of Lock step mode, RPU1's output is
  * clamped.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_set_rpu_oper_mode(uint32_t mode)
 {
@@ -84,13 +86,14 @@
 }
 
 /**
- * pm_ioctl_config_boot_addr() - Configure RPU boot address
- * @nid		Node ID of RPU
- * @value	Value to set for boot address (TCM/OCM)
+ * pm_ioctl_config_boot_addr() - Configure RPU boot address.
+ * @nid: Node ID of RPU.
+ * @value: Value to set for boot address (TCM/OCM).
  *
  * This function configures RPU boot address(memory).
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_config_boot_addr(enum pm_node_id nid,
 						    uint32_t value)
@@ -121,13 +124,14 @@
 }
 
 /**
- * pm_ioctl_config_tcm_comb() - Configure TCM combined mode
- * @value	Value to set (Split/Combined)
+ * pm_ioctl_config_tcm_comb() - Configure TCM combined mode.
+ * @value: Value to set (Split/Combined).
  *
  * This function configures TCM to be in split mode or combined
  * mode.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_config_tcm_comb(uint32_t value)
 {
@@ -149,13 +153,14 @@
 }
 
 /**
- * pm_ioctl_set_tapdelay_bypass() -  Enable/Disable tap delay bypass
- * @type	Type of tap delay to enable/disable (e.g. QSPI)
- * @value	Enable/Disable
+ * pm_ioctl_set_tapdelay_bypass() -  Enable/Disable tap delay bypass.
+ * @type: Type of tap delay to enable/disable (e.g. QSPI).
+ * @value: Enable/Disable.
  *
  * This function enable/disable tap delay bypass.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_set_tapdelay_bypass(uint32_t type,
 						       uint32_t value)
@@ -169,15 +174,16 @@
 }
 
 /**
- * pm_ioctl_set_sgmii_mode() -  Set SGMII mode for the GEM device
- * @nid		Node ID of the device
- * @value	Enable/Disable
+ * pm_ioctl_set_sgmii_mode() -  Set SGMII mode for the GEM device.
+ * @nid: Node ID of the device.
+ * @value: Enable/Disable.
  *
  * This function enable/disable SGMII mode for the GEM device.
  * While enabling SGMII mode, it also ties the GEM PCS Signal
  * Detect to 1 and selects EMIO for RX clock generation.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_set_sgmii_mode(enum pm_node_id nid,
 						  uint32_t value)
@@ -229,13 +235,14 @@
 }
 
 /**
- * pm_ioctl_sd_dll_reset() -  Reset DLL logic
- * @nid		Node ID of the device
- * @type	Reset type
+ * pm_ioctl_sd_dll_reset() -  Reset DLL logic.
+ * @nid: Node ID of the device.
+ * @type: Reset type.
  *
  * This function resets DLL logic for the SD device.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_sd_dll_reset(enum pm_node_id nid,
 						uint32_t type)
@@ -278,14 +285,15 @@
 }
 
 /**
- * pm_ioctl_sd_set_tapdelay() -  Set tap delay for the SD device
- * @nid		Node ID of the device
- * @type	Type of tap delay to set (input/output)
- * @value	Value to set fot the tap delay
+ * pm_ioctl_sd_set_tapdelay() -  Set tap delay for the SD device.
+ * @nid: Node ID of the device.
+ * @type: Type of tap delay to set (input/output).
+ * @value: Value to set fot the tap delay.
  *
  * This function sets input/output tap delay for the SD device.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_sd_set_tapdelay(enum pm_node_id nid,
 						   enum tap_delay_type type,
@@ -375,14 +383,14 @@
 }
 
 /**
- * pm_ioctl_set_pll_frac_mode() -  Ioctl function for
- *				   setting pll mode
- * @pll     PLL clock id
- * @mode    Mode fraction/integar
+ * pm_ioctl_set_pll_frac_mode() -  Ioctl function for setting pll mode.
+ * @pll: PLL clock id.
+ * @mode: Mode fraction/integar.
  *
- * This function sets PLL mode
+ * This function sets PLL mode.
  *
- * @return      Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_set_pll_frac_mode
 			(uint32_t pll, uint32_t mode)
@@ -391,14 +399,14 @@
 }
 
 /**
- * pm_ioctl_get_pll_frac_mode() -  Ioctl function for
- *				   getting pll mode
- * @pll     PLL clock id
- * @mode    Mode fraction/integar
+ * pm_ioctl_get_pll_frac_mode() -  Ioctl function for getting pll mode.
+ * @pll: PLL clock id.
+ * @mode: Mode fraction/integar.
  *
- * This function return current PLL mode
+ * This function return current PLL mode.
  *
- * @return      Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_get_pll_frac_mode
 			(uint32_t pll, uint32_t *mode)
@@ -407,15 +415,15 @@
 }
 
 /**
- * pm_ioctl_set_pll_frac_data() -  Ioctl function for
- *				   setting pll fraction data
- * @pll     PLL clock id
- * @data    fraction data
+ * pm_ioctl_set_pll_frac_data() -  Ioctl function for setting pll fraction data.
+ * @pll: PLL clock id.
+ * @data: fraction data.
  *
  * This function sets fraction data.
  * It is valid for fraction mode only.
  *
- * @return      Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_set_pll_frac_data
 			(uint32_t pll, uint32_t data)
@@ -433,14 +441,14 @@
 }
 
 /**
- * pm_ioctl_get_pll_frac_data() -  Ioctl function for
- *				   getting pll fraction data
- * @pll     PLL clock id
- * @data    fraction data
+ * pm_ioctl_get_pll_frac_data() -  Ioctl function for getting pll fraction data.
+ * @pll: PLL clock id.
+ * @data: fraction data.
  *
  * This function returns fraction data value.
  *
- * @return      Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_get_pll_frac_data
 			(uint32_t pll, uint32_t *data)
@@ -458,14 +466,15 @@
 }
 
 /**
- * pm_ioctl_write_ggs() - Ioctl function for writing
- *			  global general storage (ggs)
- * @index	GGS register index
- * @value	Register value to be written
+ * pm_ioctl_write_ggs() - Ioctl function for writing global general storage
+ *                        (ggs).
+ * @index: GGS register index.
+ * @value: Register value to be written.
  *
  * This function writes value to GGS register.
  *
- * @return      Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_write_ggs(uint32_t index,
 					     uint32_t value)
@@ -479,14 +488,15 @@
 }
 
 /**
- * pm_ioctl_read_ggs() - Ioctl function for reading
- *			 global general storage (ggs)
- * @index	GGS register index
- * @value	Register value
+ * pm_ioctl_read_ggs() - Ioctl function for reading global general storage
+ *                       (ggs).
+ * @index: GGS register index.
+ * @value: Register value.
  *
  * This function returns GGS register value.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return      Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_read_ggs(uint32_t index,
 					    uint32_t *value)
@@ -499,14 +509,15 @@
 }
 
 /**
- * pm_ioctl_write_pggs() - Ioctl function for writing persistent
- *			   global general storage (pggs)
- * @index	PGGS register index
- * @value	Register value to be written
+ * pm_ioctl_write_pggs() - Ioctl function for writing persistent global general
+ *                         storage (pggs).
+ * @index: PGGS register index.
+ * @value: Register value to be written.
  *
  * This function writes value to PGGS register.
  *
- * @return      Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_ioctl_write_pggs(uint32_t index,
 					      uint32_t value)
@@ -520,13 +531,12 @@
 }
 
 /**
- * pm_ioctl_afi() - Ioctl function for writing afi values
+ * pm_ioctl_afi() - Ioctl function for writing afi values.
+ * @index: AFI register index.
+ * @value: Register value to be written.
  *
- * @index 	AFI register index
- * @value	Register value to be written
+ * Return: Returns status, either success or error+reason.
  *
- *
- * @return      Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_afi(uint32_t index,
 					      uint32_t value)
@@ -564,14 +574,15 @@
 }
 
 /**
- * pm_ioctl_read_pggs() - Ioctl function for reading persistent
- *			  global general storage (pggs)
- * @index	PGGS register index
- * @value	Register value
+ * pm_ioctl_read_pggs() - Ioctl function for reading persistent global general
+ *                        storage (pggs).
+ * @index: PGGS register index.
+ * @value: Register value.
  *
  * This function returns PGGS register value.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return      Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_read_pggs(uint32_t index,
 					     uint32_t *value)
@@ -584,12 +595,12 @@
 }
 
 /**
- * pm_ioctl_ulpi_reset() - Ioctl function for performing ULPI reset
+ * pm_ioctl_ulpi_reset() - Ioctl function for performing ULPI reset.
+ *
+ * Return: Returns status, either success or error+reason.
  *
  * This function peerforms the ULPI reset sequence for resetting
  * the ULPI transceiver.
- *
- * @return      Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_ulpi_reset(void)
 {
@@ -620,12 +631,14 @@
 }
 
 /**
- * pm_ioctl_set_boot_health_status() - Ioctl for setting healthy boot status
+ * pm_ioctl_set_boot_health_status() - Ioctl for setting healthy boot status.
+ * @value: Value to write.
  *
  * This function sets healthy bit value to indicate boot health status
  * to firmware.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return      Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_set_boot_health_status(uint32_t value)
 {
@@ -634,16 +647,17 @@
 }
 
 /**
- * pm_api_ioctl() -  PM IOCTL API for device control and configs
- * @node_id	Node ID of the device
- * @ioctl_id	ID of the requested IOCTL
- * @arg1	Argument 1 to requested IOCTL call
- * @arg2	Argument 2 to requested IOCTL call
- * @value	Returned output value
+ * pm_api_ioctl() -  PM IOCTL API for device control and configs.
+ * @nid: Node ID of the device.
+ * @ioctl_id: ID of the requested IOCTL.
+ * @arg1: Argument 1 to requested IOCTL call.
+ * @arg2: Argument 2 to requested IOCTL call.
+ * @value: Returned output value.
  *
  * This function calls IOCTL to firmware for device control and configuration.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_api_ioctl(enum pm_node_id nid,
 				uint32_t ioctl_id,
@@ -724,8 +738,11 @@
 }
 
 /**
- * pm_update_ioctl_bitmask() -  API to get supported IOCTL ID mask
- * @bit_mask		Returned bit mask of supported IOCTL IDs
+ * tfa_ioctl_bitmask() -  API to get supported IOCTL ID mask.
+ * @bit_mask: Returned bit mask of supported IOCTL IDs.
+ *
+ * Return: 0 success, negative value for errors.
+ *
  */
 enum pm_ret_status tfa_ioctl_bitmask(uint32_t *bit_mask)
 {
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
index 6afadef..2d8c23b 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
@@ -1946,12 +1946,13 @@
 };
 
 /**
- * pm_api_pinctrl_get_num_pins() - PM call to request number of pins
- * @npins	Number of pins
+ * pm_api_pinctrl_get_num_pins() - PM call to request number of pins.
+ * @npins: Number of pins.
  *
- * This function is used by master to get number of pins
+ * This function is used by master to get number of pins.
  *
- * @return	Returns success.
+ * Return: Returns success.
+ *
  */
 enum pm_ret_status pm_api_pinctrl_get_num_pins(uint32_t *npins)
 {
@@ -1961,12 +1962,13 @@
 }
 
 /**
- * pm_api_pinctrl_get_num_functions() - PM call to request number of functions
- * @nfuncs	Number of functions
+ * pm_api_pinctrl_get_num_functions() - PM call to request number of functions.
+ * @nfuncs: Number of functions.
  *
- * This function is used by master to get number of functions
+ * This function is used by master to get number of functions.
  *
- * @return	Returns success.
+ * Return: Returns success.
+ *
  */
 enum pm_ret_status pm_api_pinctrl_get_num_functions(uint32_t *nfuncs)
 {
@@ -1977,13 +1979,14 @@
 
 /**
  * pm_api_pinctrl_get_num_func_groups() - PM call to request number of
- *					  function groups
- * @fid		Function Id
- * @ngroups	Number of function groups
+ *					  function groups.
+ * @fid: Function Id.
+ * @ngroups: Number of function groups.
+ *
+ * This function is used by master to get number of function groups.
  *
- * This function is used by master to get number of function groups
+ * Return: Returns success.
  *
- * @return	Returns success.
  */
 enum pm_ret_status pm_api_pinctrl_get_num_func_groups(uint32_t fid,
 						      uint32_t *ngroups)
@@ -1998,12 +2001,13 @@
 }
 
 /**
- * pm_api_pinctrl_get_function_name() - PM call to request a function name
- * @fid		Function ID
- * @name	Name of function (max 16 bytes)
+ * pm_api_pinctrl_get_function_name() - PM call to request a function name.
+ * @fid: Function ID.
+ * @name: Name of function (max 16 bytes).
  *
  * This function is used by master to get name of function specified
  * by given function ID.
+ *
  */
 void pm_api_pinctrl_get_function_name(uint32_t fid, char *name)
 {
@@ -2016,10 +2020,10 @@
 
 /**
  * pm_api_pinctrl_get_function_groups() - PM call to request first 6 function
- *					  groups of function Id
- * @fid		Function ID
- * @index	Index of next function groups
- * @groups	Function groups
+ *					  groups of function Id.
+ * @fid: Function ID.
+ * @index: Index of next function groups.
+ * @groups: Function groups.
  *
  * This function is used by master to get function groups specified
  * by given function Id. This API will return 6 function groups with
@@ -2031,6 +2035,7 @@
  * function groups 6, 7, 8, 9, 10 and 11 and so on.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_api_pinctrl_get_function_groups(uint32_t fid,
 						      uint32_t index,
@@ -2061,10 +2066,10 @@
 
 /**
  * pm_api_pinctrl_get_pin_groups() - PM call to request first 6 pin
- *				     groups of pin
- * @pin		Pin
- * @index	Index of next pin groups
- * @groups	pin groups
+ *                                   groups of pin.
+ * @pin: Pin.
+ * @index: Index of next pin groups.
+ * @groups: pin groups.
  *
  * This function is used by master to get pin groups specified
  * by given pin Id. This API will return 6 pin groups with
@@ -2076,6 +2081,7 @@
  * pin groups 6, 7, 8, 9, 10 and 11 and so on.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_api_pinctrl_get_pin_groups(uint32_t pin,
 						 uint32_t index,
diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.c b/plat/xilinx/zynqmp/pm_service/pm_client.c
index 853e9e1..4afa01d 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.c
@@ -21,9 +21,9 @@
 #include <lib/utils.h>
 
 #include <plat_ipi.h>
-#include <zynqmp_def.h>
 #include "pm_client.h"
 #include "pm_ipi.h"
+#include <zynqmp_def.h>
 #include "zynqmp_pm_api_sys.h"
 
 #define IRQ_MAX		84U
@@ -157,10 +157,11 @@
 };
 
 /**
- * irq_to_pm_node - Get PM node ID corresponding to the interrupt number
- * @irq:	Interrupt number
+ * irq_to_pm_node - Get PM node ID corresponding to the interrupt number.
+ * @irq: Interrupt number.
  *
- * Return:	PM node ID corresponding to the specified interrupt
+ * Return: PM node ID corresponding to the specified interrupt.
+ *
  */
 static enum pm_node_id irq_to_pm_node(uint32_t irq)
 {
@@ -170,7 +171,8 @@
 
 /**
  * pm_client_set_wakeup_sources - Set all slaves with enabled interrupts as wake
- *				sources in the PMU firmware
+ *                                sources in the PMU firmware.
+ *
  */
 static void pm_client_set_wakeup_sources(void)
 {
@@ -227,10 +229,11 @@
 }
 
 /**
- * pm_get_proc() - returns pointer to the proc structure
- * @cpuid:	id of the cpu whose proc struct pointer should be returned
+ * pm_get_proc() - returns pointer to the proc structure.
+ * @cpuid: id of the cpu whose proc struct pointer should be returned.
+ *
+ * Return: pointer to a proc structure if proc is found, otherwise NULL.
  *
- * Return: pointer to a proc structure if proc is found, otherwise NULL
  */
 const struct pm_proc *pm_get_proc(uint32_t cpuid)
 {
@@ -242,10 +245,11 @@
 }
 
 /**
- * pm_get_proc_by_node() - returns pointer to the proc structure
- * @nid:	node id of the processor
+ * pm_get_proc_by_node() - returns pointer to the proc structure.
+ * @nid: node id of the processor.
  *
- * Return: pointer to a proc structure if proc is found, otherwise NULL
+ * Return: pointer to a proc structure if proc is found, otherwise NULL.
+ *
  */
 const struct pm_proc *pm_get_proc_by_node(enum pm_node_id nid)
 {
@@ -258,10 +262,11 @@
 }
 
 /**
- * pm_get_cpuid() - get the local cpu ID for a global node ID
- * @nid:	node id of the processor
+ * pm_get_cpuid() - get the local cpu ID for a global node ID.
+ * @nid: node id of the processor.
  *
- * Return: the cpu ID (starting from 0) for the subsystem
+ * Return: the cpu ID (starting from 0) for the subsystem.
+ *
  */
 static uint32_t pm_get_cpuid(enum pm_node_id nid)
 {
@@ -276,11 +281,14 @@
 const struct pm_proc *primary_proc = &pm_procs_all[0];
 
 /**
- * pm_client_suspend() - Client-specific suspend actions
+ * pm_client_suspend() - Client-specific suspend actions.
+ * @proc: processor which need to suspend.
+ * @state: desired suspend state.
  *
  * This function should contain any PU-specific actions
  * 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, uint32_t state)
 {
@@ -298,10 +306,11 @@
 
 
 /**
- * pm_client_abort_suspend() - Client-specific abort-suspend actions
+ * pm_client_abort_suspend() - Client-specific abort-suspend actions.
  *
  * This function should contain any PU-specific actions
- * required for aborting a prior suspend request
+ * required for aborting a prior suspend request.
+ *
  */
 void pm_client_abort_suspend(void)
 {
@@ -318,10 +327,12 @@
 }
 
 /**
- * pm_client_wakeup() - Client-specific wakeup actions
+ * pm_client_wakeup() - Client-specific wakeup actions.
+ * @proc: Processor which need to wakeup.
  *
  * This function should contain any PU-specific actions
- * required for waking up another APU core
+ * required for waking up another APU core.
+ *
  */
 void pm_client_wakeup(const struct pm_proc *proc)
 {
diff --git a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c
index 75cb54f..ece5954 100644
--- a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c
@@ -37,11 +37,11 @@
 	(1ULL << (uint64_t)PM_QID_CLOCK_GET_MAX_DIVISOR))
 
 /**
- * struct eemi_api_dependency - Dependent EEMI APIs which are implemented
- * on both the TF-A and firmware
+ * typedef eemi_api_dependency - Dependent EEMI APIs which are implemented
+ *                               on both the TF-A and firmware.
+ * @id: EEMI API id or IOCTL id to be checked.
+ * @api_id: Dependent EEMI API.
  *
- * @id:		EEMI API id or IOCTL id to be checked
- * @api_id:	Dependent EEMI API
  */
 typedef struct __attribute__((packed)) {
 	uint8_t id;
@@ -244,9 +244,10 @@
 static uint32_t pm_shutdown_scope = PMF_SHUTDOWN_SUBTYPE_SYSTEM;
 
 /**
- * pm_get_shutdown_scope() - Get the currently set shutdown scope
+ * pm_get_shutdown_scope() - Get the currently set shutdown scope.
  *
- * @return	Shutdown scope value
+ * Return: Shutdown scope value.
+ *
  */
 uint32_t pm_get_shutdown_scope(void)
 {
@@ -254,16 +255,17 @@
 }
 
 /**
- * pm_self_suspend() - PM call for processor to suspend itself
- * @nid		Node id of the processor or subsystem
- * @latency	Requested maximum wakeup latency (not supported)
- * @state	Requested state
- * @address	Resume address
+ * pm_self_suspend() - PM call for processor to suspend itself.
+ * @nid: Node id of the processor or subsystem.
+ * @latency: Requested maximum wakeup latency (not supported).
+ * @state: Requested state.
+ * @address: Resume address.
  *
  * This is a blocking call, it will return only once PMU has responded.
  * On a wakeup, resume address will be automatically set by PMU.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_self_suspend(enum pm_node_id nid,
 				   uint32_t latency,
@@ -287,13 +289,14 @@
 
 /**
  * pm_req_suspend() - PM call to request for another PU or subsystem to
- *		      be suspended gracefully.
- * @target	Node id of the targeted PU or subsystem
- * @ack		Flag to specify whether acknowledge is requested
- * @latency	Requested wakeup latency (not supported)
- * @state	Requested state (not supported)
+ *                    be suspended gracefully.
+ * @target: Node id of the targeted PU or subsystem.
+ * @ack: Flag to specify whether acknowledge is requested.
+ * @latency: Requested wakeup latency (not supported).
+ * @state: Requested state (not supported).
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_req_suspend(enum pm_node_id target,
 				  enum pm_request_ack ack,
@@ -312,19 +315,20 @@
 
 /**
  * pm_req_wakeup() - PM call for processor to wake up selected processor
- *		     or subsystem
- * @target	Node id of the processor or subsystem to wake up
- * @ack		Flag to specify whether acknowledge requested
- * @set_address	Resume address presence indicator
- *				1 resume address specified, 0 otherwise
- * @address	Resume address
+ *		     or subsystem.
+ * @target: Node id of the processor or subsystem to wake up.
+ * @ack: Flag to specify whether acknowledge requested.
+ * @set_address: Resume address presence indicator.
+ *               1 resume address specified, 0 otherwise.
+ * @address: Resume address.
  *
  * This API function is either used to power up another APU core for SMP
  * (by PSCI) or to power up an entirely different PU or subsystem, such
  * as RPU0, RPU, or PL_CORE_xx. Resume address for the target PU will be
  * automatically set by PMU.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_req_wakeup(enum pm_node_id target,
 				 uint32_t set_address,
@@ -352,11 +356,12 @@
 
 /**
  * pm_force_powerdown() - PM call to request for another PU or subsystem to
- *			  be powered down forcefully
- * @target	Node id of the targeted PU or subsystem
- * @ack		Flag to specify whether acknowledge is requested
+ *                        be powered down forcefully.
+ * @target: Node id of the targeted PU or subsystem.
+ * @ack: Flag to specify whether acknowledge is requested.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_force_powerdown(enum pm_node_id target,
 				      enum pm_request_ack ack)
@@ -375,13 +380,14 @@
 
 /**
  * pm_abort_suspend() - PM call to announce that a prior suspend request
- *			is to be aborted.
- * @reason	Reason for the abort
+ *                      is to be aborted.
+ * @reason: Reason for the abort.
  *
  * Calling PU expects the PMU to abort the initiated suspend procedure.
  * This is a non-blocking call without any acknowledge.
  *
+ * Return: Returns status, either success or error+reason
+ *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_abort_suspend(enum pm_abort_reason reason)
 {
@@ -400,12 +406,14 @@
 }
 
 /**
- * pm_set_wakeup_source() - PM call to specify the wakeup source while suspended
- * @target	Node id of the targeted PU or subsystem
- * @wkup_node	Node id of the wakeup peripheral
- * @enable	Enable or disable the specified peripheral as wake source
+ * pm_set_wakeup_source() - PM call to specify the wakeup source while
+ *                          suspended.
+ * @target: Node id of the targeted PU or subsystem.
+ * @wkup_node: Node id of the wakeup peripheral.
+ * @enable: Enable or disable the specified peripheral as wake source.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_set_wakeup_source(enum pm_node_id target,
 					enum pm_node_id wkup_node,
@@ -419,11 +427,12 @@
 }
 
 /**
- * pm_system_shutdown() - PM call to request a system shutdown or restart
- * @type	Shutdown or restart? 0=shutdown, 1=restart, 2=setscope
- * @subtype	Scope: 0=APU-subsystem, 1=PS, 2=system
+ * pm_system_shutdown() - PM call to request a system shutdown or restart.
+ * @type: Shutdown or restart? 0=shutdown, 1=restart, 2=setscope.
+ * @subtype: Scope: 0=APU-subsystem, 1=PS, 2=system.
+ *
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_system_shutdown(uint32_t type, uint32_t subtype)
 {
@@ -442,13 +451,14 @@
 /* APIs for managing PM slaves: */
 
 /**
- * pm_req_node() - PM call to request a node with specific capabilities
- * @nid		Node id of the slave
- * @capabilities Requested capabilities of the slave
- * @qos		Quality of service (not supported)
- * @ack		Flag to specify whether acknowledge is requested
+ * pm_req_node() - PM call to request a node with specific capabilities.
+ * @nid: Node id of the slave.
+ * @capabilities: Requested capabilities of the slave.
+ * @qos: Quality of service (not supported).
+ * @ack: Flag to specify whether acknowledge is requested.
+ *
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_req_node(enum pm_node_id nid,
 			       uint32_t capabilities,
@@ -467,15 +477,16 @@
 }
 
 /**
- * pm_set_requirement() - PM call to set requirement for PM slaves
- * @nid		Node id of the slave
- * @capabilities Requested capabilities of the slave
- * @qos		Quality of service (not supported)
- * @ack		Flag to specify whether acknowledge is requested
+ * pm_set_requirement() - PM call to set requirement for PM slaves.
+ * @nid: Node id of the slave.
+ * @capabilities: Requested capabilities of the slave.
+ * @qos: Quality of service (not supported).
+ * @ack: Flag to specify whether acknowledge is requested.
  *
- * This API function is to be used for slaves a PU already has requested
+ * This API function is to be used for slaves a PU already has requested.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_set_requirement(enum pm_node_id nid,
 				      uint32_t capabilities,
@@ -497,10 +508,11 @@
 /* Miscellaneous API functions */
 
 /**
- * pm_get_api_version() - Get version number of PMU PM firmware
- * @version	Returns 32-bit version number of PMU Power Management Firmware
+ * pm_get_api_version() - Get version number of PMU PM firmware.
+ * @version: Returns 32-bit version number of PMU Power Management Firmware.
+ *
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_get_api_version(uint32_t *version)
 {
@@ -512,14 +524,15 @@
 }
 
 /**
- * pm_get_node_status() - PM call to request a node's current status
- * @nid		Node id
- * @ret_buff	Buffer for the return values:
- *		[0] - Current power state of the node
- *		[1] - Current requirements for the node (slave nodes only)
- *		[2] - Current usage status for the node (slave nodes only)
+ * pm_get_node_status() - PM call to request a node's current status.
+ * @nid: Node id.
+ * @ret_buff: Buffer for the return values
+ *            [0] - Current power state of the node
+ *            [1] - Current requirements for the node (slave nodes only)
+ *            [2] - Current usage status for the node (slave nodes only)
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_get_node_status(enum pm_node_id nid,
 				      uint32_t *ret_buff)
@@ -531,15 +544,16 @@
 }
 
 /**
- * pm_mmio_write() - Perform write to protected mmio
- * @address	Address to write to
- * @mask	Mask to apply
- * @value	Value to write
+ * pm_mmio_write() - Perform write to protected mmio.
+ * @address: Address to write to.
+ * @mask: Mask to apply.
+ * @value: Value to write.
  *
  * This function provides access to PM-related control registers
  * that may not be directly accessible by a particular PU.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_mmio_write(uintptr_t address,
 				 uint32_t mask,
@@ -553,14 +567,15 @@
 }
 
 /**
- * pm_mmio_read() - Read value from protected mmio
- * @address	Address to write to
- * @value	Value to write
+ * pm_mmio_read() - Read value from protected mmio.
+ * @address: Address to write to.
+ * @value: Value to write.
  *
  * This function provides access to PM-related control registers
  * that may not be directly accessible by a particular PU.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_mmio_read(uintptr_t address, uint32_t *value)
 {
@@ -572,18 +587,16 @@
 }
 
 /**
- * pm_fpga_load() - Load the bitstream into the PL.
+ * pm_fpga_load() - Load the bitstream into the PL. This function provides
+ *                  access to the xilfpga library to load the Bit-stream
+ *                  into PL.
+ * @address_low: lower 32-bit Linear memory space address.
+ * @address_high: higher 32-bit Linear memory space address.
+ * @size: Number of 32bit words.
+ * @flags: Additional flags or settings for the fpga operation.
  *
- * This function provides access to the xilfpga library to load
- * the Bit-stream into PL.
- *
- * address_low: lower 32-bit Linear memory space address
- *
- * address_high: higher 32-bit Linear memory space address
- *
- * size:	Number of 32bit words
+ * Return: Returns status, either success or error+reason.
  *
- * @return      Returns status, either success or error+reason
  */
 enum pm_ret_status pm_fpga_load(uint32_t address_low,
 				uint32_t address_high,
@@ -599,12 +612,14 @@
 }
 
 /**
- * pm_fpga_get_status() - Read value from fpga status register
- * @value       Value to read
+ * pm_fpga_get_status() - Read value from fpga status register.
+ * @value: Value to read.
  *
  * This function provides access to the xilfpga library to get
- * the fpga status
- * @return      Returns status, either success or error+reason
+ * the fpga status.
+ *
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_fpga_get_status(uint32_t *value)
 {
@@ -616,11 +631,11 @@
 }
 
 /**
- * pm_get_chipid() - Read silicon ID registers
- * @value       Buffer for return values. Must be large enough
- *		to hold 8 bytes.
+ * pm_get_chipid() - Read silicon ID registers.
+ * @value: Buffer for return values. Must be large enough to hold 8 bytes.
  *
- * @return      Returns silicon ID registers
+ * Return: Returns silicon ID registers.
+ *
  */
 enum pm_ret_status pm_get_chipid(uint32_t *value)
 {
@@ -633,17 +648,16 @@
 
 /**
  * pm_secure_rsaaes() - Load the secure images.
- *
- * This function provides access to the xilsecure library to load
- * the authenticated, encrypted, and authenticated/encrypted images.
- *
- * address_low: lower 32-bit Linear memory space address
+ * @address_low: lower 32-bit Linear memory space address.
+ * @address_high: higher 32-bit Linear memory space address.
+ * @size: Number of 32bit words.
+ * @flags: Additional flags or settings for the fpga operation.
  *
- * address_high: higher 32-bit Linear memory space address
+ * This function provides access to the xilsecure library to load the
+ * authenticated, encrypted, and authenticated/encrypted images.
  *
- * size:	Number of 32bit words
+ * Return: Returns status, either success or error+reason.
  *
- * @return      Returns status, either success or error+reason
  */
 enum pm_ret_status pm_secure_rsaaes(uint32_t address_low,
 				uint32_t address_high,
@@ -659,17 +673,16 @@
 }
 
 /**
- * pm_aes_engine() - Aes data blob encryption/decryption
+ * pm_aes_engine() - Aes data blob encryption/decryption.
+ * @address_low: lower 32-bit address of the AesParams structure.
+ * @address_high: higher 32-bit address of the AesParams structure.
+ * @value: Returned output value.
+ *
  * This function provides access to the xilsecure library to
  * encrypt/decrypt data blobs.
  *
- * address_low: lower 32-bit address of the AesParams structure
- *
- * address_high: higher 32-bit address of the AesParams structure
- *
- * value:        Returned output value
+ * Return: Returns status, either success or error+reason.
  *
- * @return       Returns status, either success or error+reason
  */
 enum pm_ret_status pm_aes_engine(uint32_t address_high,
 				 uint32_t address_low,
@@ -683,11 +696,13 @@
 }
 
 /**
- * pm_get_callbackdata() - Read from IPI response buffer
- * @data - array of PAYLOAD_ARG_CNT elements
+ * pm_get_callbackdata() - Read from IPI response buffer.
+ * @data: array of PAYLOAD_ARG_CNT elements.
+ * @count: Number of values to return.
  *
  * Read value from ipi buffer response buffer.
- * @return      Returns status, either success or error
+ * Return: Returns status, either success or error.
+ *
  */
 enum pm_ret_status pm_get_callbackdata(uint32_t *data, size_t count)
 {
@@ -703,16 +718,17 @@
 }
 
 /**
- * pm_ioctl() -  PM IOCTL API for device control and configs
- * @node_id	Node ID of the device
- * @ioctl_id	ID of the requested IOCTL
- * @arg1	Argument 1 to requested IOCTL call
- * @arg2	Argument 2 to requested IOCTL call
- * @out		Returned output value
+ * pm_ioctl() - PM IOCTL API for device control and configs.
+ * @nid: Node ID of the device.
+ * @ioctl_id: ID of the requested IOCTL.
+ * @arg1: Argument 1 to requested IOCTL call.
+ * @arg2: Argument 2 to requested IOCTL call.
+ * @value: Returned output value.
  *
  * This function calls IOCTL to firmware for device control and configuration.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_ioctl(enum pm_node_id nid,
 			    uint32_t ioctl_id,
@@ -724,12 +740,13 @@
 }
 
 /**
- * fw_api_version() - Returns API version implemented in firmware
- * @api_id	API ID to check
- * @version	Returned supported API version
- * @len		Number of words to be returned
+ * fw_api_version() - Returns API version implemented in firmware.
+ * @id: API ID to check.
+ * @version: Returned supported API version.
+ * @len: Number of words to be returned.
+ *
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status fw_api_version(uint32_t id, uint32_t *version,
 					 uint32_t len)
@@ -741,10 +758,11 @@
 }
 
 /**
- * check_api_dependency() -  API to check dependent EEMI API version
- * @id		EEMI API ID to check
+ * check_api_dependency() -  API to check dependent EEMI API version.
+ * @id: EEMI API ID to check.
+ *
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status check_api_dependency(uint8_t id)
 {
@@ -775,11 +793,13 @@
 }
 
 /**
- * feature_check_tfa() - These are API's completely implemented in TF-A
- * @api_id	API ID to check
- * @version	Returned supported API version
+ * feature_check_tfa() - These are API's completely implemented in TF-A.
+ * @api_id: API ID to check.
+ * @version: Returned supported API version.
+ * @bit_mask: Returned supported IOCTL id version.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status feature_check_tfa(uint32_t api_id, uint32_t *version,
 					    uint32_t *bit_mask)
@@ -801,12 +821,13 @@
 }
 
 /**
- * get_tfa_version_for_partial_apis() - Return TF-A version for partially
- * implemented APIs
- * @api_id	API ID to check
- * @version	Returned supported API version
+ * get_tfa_version_for_partial_apis() - Return TF-A version for partially.
+ *                                      implemented APIs
+ * @api_id: API ID to check.
+ * @version: Returned supported API version.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status get_tfa_version_for_partial_apis(uint32_t api_id,
 							   uint32_t *version)
@@ -842,11 +863,12 @@
 
 /**
  * feature_check_partial() - These are API's partially implemented in
- * TF-A and firmware both
- * @api_id	API ID to check
- * @version	Returned supported API version
+ *                           TF-A and firmware both.
+ * @api_id: API ID to check.
+ * @version: Returned supported API version.
+ *
+ * Return: Returns status, either success or error+reason.
  *
- * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status feature_check_partial(uint32_t api_id,
 						uint32_t *version)
@@ -884,13 +906,14 @@
 }
 
 /**
- * pm_feature_check() - Returns the supported API version if supported
- * @api_id	API ID to check
- * @version	Returned supported API version
- * @bit_mask	Returned supported IOCTL id version
- * @len		Number of bytes to be returned in bit_mask variable
+ * pm_feature_check() - Returns the supported API version if supported.
+ * @api_id: API ID to check.
+ * @version: Returned supported API version.
+ * @bit_mask: Returned supported IOCTL id version.
+ * @len: Number of bytes to be returned in bit_mask variable.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_feature_check(uint32_t api_id, uint32_t *version,
 				    uint32_t *bit_mask, uint8_t len)
@@ -940,14 +963,15 @@
 }
 
 /**
- * pm_clock_get_max_divisor - PM call to get max divisor
- * @clock_id	Clock ID
- * @div_type	Divisor ID (TYPE_DIV1 or TYPE_DIV2)
- * @max_div	Maximum supported divisor
+ * pm_clock_get_max_divisor - PM call to get max divisor.
+ * @clock_id: Clock ID.
+ * @div_type: Divisor ID (TYPE_DIV1 or TYPE_DIV2).
+ * @max_div: Maximum supported divisor.
  *
  * This function is used by master to get maximum supported value.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_clock_get_max_divisor(uint32_t clock_id,
 						   uint8_t div_type,
@@ -957,12 +981,13 @@
 }
 
 /**
- * pm_clock_get_num_clocks - PM call to request number of clocks
- * @nclockss: Number of clocks
+ * pm_clock_get_num_clocks - PM call to request number of clocks.
+ * @nclocks: Number of clocks.
  *
  * This function is used by master to get number of clocks.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_clock_get_num_clocks(uint32_t *nclocks)
 {
@@ -970,12 +995,13 @@
 }
 
 /**
- * pm_clock_get_name() - PM call to request a clock's name
- * @clock_id	Clock ID
- * @name	Name of clock (max 16 bytes)
+ * pm_clock_get_name() - PM call to request a clock's name.
+ * @clock_id: Clock ID.
+ * @name: Name of clock (max 16 bytes).
  *
  * This function is used by master to get nmae of clock specified
  * by given clock ID.
+ *
  */
 static void pm_clock_get_name(uint32_t clock_id, char *name)
 {
@@ -983,17 +1009,18 @@
 }
 
 /**
- * pm_clock_get_topology() - PM call to request a clock's topology
- * @clock_id	Clock ID
- * @index	Topology index for next toplogy node
- * @topology	Buffer to store nodes in topology and flags
+ * pm_clock_get_topology() - PM call to request a clock's topology.
+ * @clock_id: Clock ID.
+ * @index: Topology index for next toplogy node.
+ * @topology: Buffer to store nodes in topology and flags.
  *
  * This function is used by master to get topology information for the
  * clock specified by given clock ID. Each response would return 3
  * topology nodes. To get next nodes, caller needs to call this API with
  * index of next node. Index starts from 0.
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_clock_get_topology(uint32_t clock_id,
 						uint32_t index,
@@ -1004,15 +1031,16 @@
 
 /**
  * pm_clock_get_fixedfactor_params() - PM call to request a clock's fixed factor
- *				 parameters for fixed clock
- * @clock_id	Clock ID
- * @mul		Multiplication value
- * @div		Divisor value
+ *                                     parameters for fixed clock.
+ * @clock_id: Clock ID.
+ * @mul: Multiplication value.
+ * @div: Divisor value.
  *
  * This function is used by master to get fixed factor parameers for the
  * fixed clock. This API is application only for the fixed clock.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_clock_get_fixedfactor_params(uint32_t clock_id,
 							  uint32_t *mul,
@@ -1022,10 +1050,10 @@
 }
 
 /**
- * pm_clock_get_parents() - PM call to request a clock's first 3 parents
- * @clock_id	Clock ID
- * @index	Index of next parent
- * @parents	Parents of the given clock
+ * pm_clock_get_parents() - PM call to request a clock's first 3 parents.
+ * @clock_id: Clock ID.
+ * @index: Index of next parent.
+ * @parents: Parents of the given clock.
  *
  * This function is used by master to get clock's parents information.
  * This API will return 3 parents with a single response. To get other
@@ -1036,7 +1064,8 @@
  * 2. Next call, index should be 3 which will return parent 3,4 and 5 and
  * so on.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_clock_get_parents(uint32_t clock_id,
 					       uint32_t index,
@@ -1046,14 +1075,15 @@
 }
 
 /**
- * pm_clock_get_attributes() - PM call to request a clock's attributes
- * @clock_id	Clock ID
- * @attr	Clock attributes
+ * pm_clock_get_attributes() - PM call to request a clock's attributes.
+ * @clock_id: Clock ID.
+ * @attr: Clock attributes.
  *
  * This function is used by master to get clock's attributes
  * (e.g. valid, clock type, etc).
  *
+ * Return: Returns status, either success or error+reason.
+ *
- * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_clock_get_attributes(uint32_t clock_id,
 						  uint32_t *attr)
@@ -1062,12 +1092,13 @@
 }
 
 /**
- * pm_clock_gate() - Configure clock gate
- * @clock_id	Id of the clock to be configured
- * @enable	Flag 0=disable (gate the clock), !0=enable (activate the clock)
+ * pm_clock_gate() - Configure clock gate.
+ * @clock_id: Id of the clock to be configured.
+ * @enable: Flag 0=disable (gate the clock), !0=enable (activate the clock).
  *
- * @return	Error if an argument is not valid or status as returned by the
- *		PM controller (PMU)
+ * 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(uint32_t clock_id,
 					uint8_t enable)
@@ -1101,14 +1132,15 @@
 }
 
 /**
- * pm_clock_enable() - Enable the clock for given id
- * @clock_id: Id of the clock to be enabled
+ * pm_clock_enable() - Enable the clock for given id.
+ * @clock_id: Id of the clock to be enabled.
  *
  * This function is used by master to enable the clock
  * including peripherals and PLL clocks.
  *
- * @return:	Error if an argument is not valid or status as returned by the
- *		pm_clock_gate
+ * Return: Error if an argument is not valid or status as returned by the
+ *         pm_clock_gate.
+ *
  */
 enum pm_ret_status pm_clock_enable(uint32_t clock_id)
 {
@@ -1125,14 +1157,15 @@
 }
 
 /**
- * pm_clock_disable - Disable the clock for given id
- * @clock_id: Id of the clock to be disable
+ * pm_clock_disable - Disable the clock for given id.
+ * @clock_id: Id of the clock to be disable.
  *
  * This function is used by master to disable the clock
  * including peripherals and PLL clocks.
  *
+ * Return: Error if an argument is not valid or status as returned by the
+ *         pm_clock_gate
+ *
- * @return:	Error if an argument is not valid or status as returned by the
- *		pm_clock_gate
  */
 enum pm_ret_status pm_clock_disable(uint32_t clock_id)
 {
@@ -1149,14 +1182,15 @@
 }
 
 /**
- * pm_clock_getstate - Get the clock state for given id
- * @clock_id: Id of the clock to be queried
- * @state: 1/0 (Enabled/Disabled)
+ * pm_clock_getstate - Get the clock state for given id.
+ * @clock_id: Id of the clock to be queried.
+ * @state: 1/0 (Enabled/Disabled).
  *
  * This function is used by master to get the state of clock
  * including peripherals and PLL clocks.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_clock_getstate(uint32_t clock_id,
 				     uint32_t *state)
@@ -1182,14 +1216,15 @@
 }
 
 /**
- * pm_clock_setdivider - Set the clock divider for given id
- * @clock_id: Id of the clock
- * @divider: divider value
+ * pm_clock_setdivider - Set the clock divider for given id.
+ * @clock_id: Id of the clock.
+ * @divider: divider value.
  *
  * This function is used by master to set divider for any clock
  * to achieve desired rate.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_clock_setdivider(uint32_t clock_id,
 				       uint32_t divider)
@@ -1230,14 +1265,15 @@
 }
 
 /**
- * pm_clock_getdivider - Get the clock divider for given id
- * @clock_id: Id of the clock
- * @divider: divider value
+ * pm_clock_getdivider - Get the clock divider for given id.
+ * @clock_id: Id of the clock.
+ * @divider: divider value.
  *
  * This function is used by master to get divider values
  * for any clock.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_clock_getdivider(uint32_t clock_id,
 				       uint32_t *divider)
@@ -1285,13 +1321,14 @@
 }
 
 /**
- * pm_clock_setrate - Set the clock rate for given id
- * @clock_id: Id of the clock
- * @rate: rate value in hz
+ * pm_clock_setrate - Set the clock rate for given id.
+ * @clock_id: Id of the clock.
+ * @rate: rate value in hz.
  *
  * This function is used by master to set rate for any clock.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_clock_setrate(uint32_t clock_id,
 				    uint64_t rate)
@@ -1300,14 +1337,15 @@
 }
 
 /**
- * pm_clock_getrate - Get the clock rate for given id
- * @clock_id: Id of the clock
- * @rate: rate value in hz
+ * pm_clock_getrate - Get the clock rate for given id.
+ * @clock_id: Id of the clock.
+ * @rate: rate value in hz.
  *
  * This function is used by master to get rate
  * for any clock.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_clock_getrate(uint32_t clock_id,
 				    uint64_t *rate)
@@ -1316,13 +1354,14 @@
 }
 
 /**
- * pm_clock_setparent - Set the clock parent for given id
- * @clock_id: Id of the clock
- * @parent_index: Index of the parent clock into clock's parents array
+ * pm_clock_setparent - Set the clock parent for given id.
+ * @clock_id: Id of the clock.
+ * @parent_index: Index of the parent clock into clock's parents array.
  *
  * This function is used by master to set parent for any clock.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_clock_setparent(uint32_t clock_id,
 				      uint32_t parent_index)
@@ -1349,14 +1388,15 @@
 }
 
 /**
- * pm_clock_getparent - Get the clock parent for given id
- * @clock_id: Id of the clock
- * @parent_index: parent index
+ * pm_clock_getparent - Get the clock parent for given id.
+ * @clock_id: Id of the clock.
+ * @parent_index: parent index.
  *
  * This function is used by master to get parent index
  * for any clock.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_clock_getparent(uint32_t clock_id,
 				      uint32_t *parent_index)
@@ -1383,12 +1423,13 @@
 }
 
 /**
- * pm_pinctrl_get_num_pins - PM call to request number of pins
- * @npins: Number of pins
+ * pm_pinctrl_get_num_pins - PM call to request number of pins.
+ * @npins: Number of pins.
  *
- * This function is used by master to get number of pins
+ * This function is used by master to get number of pins.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_pinctrl_get_num_pins(uint32_t *npins)
 {
@@ -1396,12 +1437,13 @@
 }
 
 /**
- * pm_pinctrl_get_num_functions - PM call to request number of functions
- * @nfuncs: Number of functions
+ * pm_pinctrl_get_num_functions - PM call to request number of functions.
+ * @nfuncs: Number of functions.
  *
- * This function is used by master to get number of functions
+ * This function is used by master to get number of functions.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_pinctrl_get_num_functions(uint32_t *nfuncs)
 {
@@ -1410,14 +1452,15 @@
 
 /**
  * pm_pinctrl_get_num_function_groups - PM call to request number of
- *					function groups
- * @fid: Id of function
- * @ngroups: Number of function groups
+ *                                      function groups.
+ * @fid: Id of function.
+ * @ngroups: Number of function groups.
  *
  * This function is used by master to get number of function groups specified
- * by given function Id
+ * by given function Id.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_pinctrl_get_num_function_groups(uint32_t fid,
 							     uint32_t *ngroups)
@@ -1426,12 +1469,13 @@
 }
 
 /**
- * pm_pinctrl_get_function_name - PM call to request function name
- * @fid: Id of function
- * @name: Name of function
+ * pm_pinctrl_get_function_name - PM call to request function name.
+ * @fid: Id of function.
+ * @name: Name of function.
  *
  * This function is used by master to get name of function specified
- * by given function Id
+ * by given function Id.
+ *
  */
 static void pm_pinctrl_get_function_name(uint32_t fid, char *name)
 {
@@ -1439,10 +1483,10 @@
 }
 
 /**
- * pm_pinctrl_get_function_groups - PM call to request function groups
- * @fid: Id of function
- * @index: Index of next function groups
- * @groups: Function groups
+ * pm_pinctrl_get_function_groups - PM call to request function groups.
+ * @fid: Id of function.
+ * @index: Index of next function groups.
+ * @groups: Function groups.
  *
  * This function is used by master to get function groups specified
  * by given function Id. This API will return 6 function groups with
@@ -1454,6 +1498,7 @@
  * function groups 6, 7, 8, 9, 10 and 11 and so on.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_pinctrl_get_function_groups(uint32_t fid,
 							 uint32_t index,
@@ -1463,10 +1508,10 @@
 }
 
 /**
- * pm_pinctrl_get_pin_groups - PM call to request pin groups
- * @pin_id: Id of pin
- * @index: Index of next pin groups
- * @groups: pin groups
+ * pm_pinctrl_get_pin_groups - PM call to request pin groups.
+ * @pin_id: Id of pin.
+ * @index: Index of next pin groups.
+ * @groups: pin groups.
  *
  * This function is used by master to get pin groups specified
  * by given pin Id. This API will return 6 pin groups with
@@ -1478,6 +1523,7 @@
  * pin groups 6, 7, 8, 9, 10 and 11 and so on.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 static enum pm_ret_status pm_pinctrl_get_pin_groups(uint32_t pin_id,
 						    uint32_t index,
@@ -1487,14 +1533,15 @@
 }
 
 /**
- * pm_query_data() -  PM API for querying firmware data
- * @arg1	Argument 1 to requested IOCTL call
- * @arg2	Argument 2 to requested IOCTL call
- * @arg3	Argument 3 to requested IOCTL call
- * @arg4	Argument 4 to requested IOCTL call
- * @data	Returned output data
+ * pm_query_data() - PM API for querying firmware data.
+ * @qid:  represents the query identifiers for PM.
+ * @arg1: Argument 1 to requested IOCTL call.
+ * @arg2: Argument 2 to requested IOCTL call.
+ * @arg3: Argument 3 to requested IOCTL call.
+ * @data: Returned output data.
  *
  * This function returns requested data.
+ *
  */
 void pm_query_data(enum pm_query_ids qid, uint32_t arg1, uint32_t arg2,
 		   uint32_t arg3, uint32_t *data)
@@ -1591,20 +1638,20 @@
 }
 
 /**
- * pm_fpga_read - Perform the fpga configuration readback
- *
- * @reg_numframes: Configuration register offset (or) Number of frames to read
- * @address_low: lower 32-bit Linear memory space address
- * @address_high: higher 32-bit Linear memory space address
- * @readback_type: Type of fpga readback operation
- *		   0 -- Configuration Register readback
- *		   1 -- Configuration Data readback
- * @value:	Value to read
+ * pm_fpga_read - Perform the fpga configuration readback.
+ * @reg_numframes: Configuration register offset (or) Number of frames to read.
+ * @address_low: lower 32-bit Linear memory space address.
+ * @address_high: higher 32-bit Linear memory space address.
+ * @readback_type: Type of fpga readback operation.
+ *		   0 -- Configuration Register readback.
+ *		   1 -- Configuration Data readback.
+ * @value: Value to read.
  *
  * This function provides access to the xilfpga library to read
  * the PL configuration.
  *
  * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_fpga_read(uint32_t reg_numframes,
 				uint32_t address_low,
@@ -1621,16 +1668,17 @@
 }
 
 /*
- * pm_pll_set_parameter() - Set the PLL parameter value
- * @nid		Node id of the target PLL
- * @param_id	ID of the PLL parameter
- * @value	Parameter value to be set
+ * pm_pll_set_parameter() - Set the PLL parameter value.
+ * @nid: Node id of the target PLL.
+ * @param_id: ID of the PLL parameter.
+ * @value: Parameter value to be set.
  *
  * Setting the parameter will have physical effect once the PLL mode is set to
  * integer or fractional.
  *
- * @return	Error if an argument is not valid or status as returned by the
- *		PM controller (PMU)
+ * Return: Error if an argument is not valid or status as returned by the
+ *         PM controller (PMU).
+ *
  */
 enum pm_ret_status pm_pll_set_parameter(enum pm_node_id nid,
 					enum pm_pll_param param_id,
@@ -1654,13 +1702,14 @@
 }
 
 /**
- * pm_pll_get_parameter() - Get the PLL parameter value
- * @nid		Node id of the target PLL
- * @param_id	ID of the PLL parameter
- * @value	Location to store the parameter value
+ * pm_pll_get_parameter() - Get the PLL parameter value.
+ * @nid: Node id of the target PLL.
+ * @param_id: ID of the PLL parameter.
+ * @value: Location to store the parameter value.
  *
- * @return	Error if an argument is not valid or status as returned by the
- *		PM controller (PMU)
+ * Return: Error if an argument is not valid or status as returned by the
+ *         PM controller (PMU).
+ *
  */
 enum pm_ret_status pm_pll_get_parameter(enum pm_node_id nid,
 					enum pm_pll_param param_id,
@@ -1684,17 +1733,18 @@
 }
 
 /**
- * pm_pll_set_mode() - Set the PLL mode
- * @nid		Node id of the target PLL
- * @mode	PLL mode to be set
+ * pm_pll_set_mode() - Set the PLL mode.
+ * @nid: Node id of the target PLL.
+ * @mode: PLL mode to be set.
  *
  * If reset mode is set the PM controller will first bypass the PLL and then
  * assert the reset. If integer or fractional mode is set the PM controller will
  * ensure that the complete PLL programming sequence is satisfied. After this
  * function returns success the PLL is locked and its bypass is deasserted.
  *
- * @return	Error if an argument is not valid or status as returned by the
- *		PM controller (PMU)
+ * Return: Error if an argument is not valid or status as returned by the
+ *         PM controller (PMU).
+ *
  */
 enum pm_ret_status pm_pll_set_mode(enum pm_node_id nid, enum pm_pll_mode mode)
 {
@@ -1716,12 +1766,13 @@
 }
 
 /**
- * pm_pll_get_mode() - Get the PLL mode
- * @nid		Node id of the target PLL
- * @mode	Location to store the mode of the PLL
+ * pm_pll_get_mode() - Get the PLL mode.
+ * @nid: Node id of the target PLL.
+ * @mode: Location to store the mode of the PLL.
  *
- * @return	Error if an argument is not valid or status as returned by the
- *		PM controller (PMU)
+ * Return: Error if an argument is not valid or status as returned by the
+ *         PM controller (PMU).
+ *
  */
 enum pm_ret_status pm_pll_get_mode(enum pm_node_id nid, enum pm_pll_mode *mode)
 {
@@ -1738,21 +1789,17 @@
 }
 
 /**
- * pm_register_access() -  PM API for register read/write access data
- *
- * @register_access_id	Register_access_id which says register read/write
- *
- * @address		Address of the register to be accessed
- *
- * @mask		Mask value to be used while writing value
- *
- * @value		Value to be written to register
- *
- * @out			Returned output data
+ * pm_register_access() -  PM API for register read/write access data.
+ * @register_access_id: Register_access_id which says register read/write.
+ * @address: Address of the register to be accessed.
+ * @mask: Mask value to be used while writing value.
+ * @value: Value to be written to register.
+ * @out: Returned output data.
  *
  * This function returns requested data.
  *
- * @return	Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
+ *
  */
 enum pm_ret_status pm_register_access(uint32_t register_access_id,
 				      uint32_t address,
@@ -1785,17 +1832,14 @@
 }
 
 /**
- * pm_efuse_access() - To program or read efuse bits.
- *
- * This function provides access to the xilskey library to program/read
- * efuse bits.
+ * pm_efuse_access() - To program or read efuse bits. This function provides
+ *                     access to the xilskey library to program/read
+ *                     efuse bits.
+ * @address_low: lower 32-bit Linear memory space address.
+ * @address_high: higher 32-bit Linear memory space address.
+ * @value: Returned output value.
  *
- * address_low: lower 32-bit Linear memory space address
- * address_high: higher 32-bit Linear memory space address
- *
- * value: Returned output value
- *
- * @return  Returns status, either success or error+reason
+ * Return: Returns status, either success or error+reason.
  *
  */
 enum pm_ret_status pm_efuse_access(uint32_t address_high,
diff --git a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.h b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.h
index 2baad3d..a2597bc 100644
--- a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.h
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.h
@@ -35,7 +35,7 @@
 	CONFIG_REG_READ,
 };
 
-/**
+/*
  * Assigning of argument values into array elements.
  */
 #define PM_PACK_PAYLOAD1(pl, arg0) {	\
diff --git a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_defs.h b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_defs.h
index 6dff07e..af75c5c 100644
--- a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_defs.h
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_defs.h
@@ -23,9 +23,10 @@
 
 #define PM_VERSION	((PM_VERSION_MAJOR << 16U) | PM_VERSION_MINOR)
 
-/**
+/*
  * PM API versions
  */
+
 /* Expected version of firmware APIs */
 #define FW_API_BASE_VERSION		(1U)
 /* Expected version of firmware API for feature check */
@@ -153,9 +154,11 @@
 };
 
 /**
- * @PM_INITIAL_BOOT:	boot is a fresh system startup
- * @PM_RESUME:		boot is a resume
- * @PM_BOOT_ERROR:	error, boot cause cannot be identified
+ * enum pm_boot_status - enum represents the boot status of the PM.
+ * @PM_INITIAL_BOOT: boot is a fresh system startup.
+ * @PM_RESUME: boot is a resume.
+ * @PM_BOOT_ERROR: error, boot cause cannot be identified.
+ *
  */
 enum pm_boot_status {
 	PM_INITIAL_BOOT,
@@ -164,9 +167,11 @@
 };
 
 /**
- * @PMF_SHUTDOWN_TYPE_SHUTDOWN:		shutdown
- * @PMF_SHUTDOWN_TYPE_RESET:		reset/reboot
- * @PMF_SHUTDOWN_TYPE_SETSCOPE_ONLY:	set the shutdown/reboot scope
+ * enum pm_shutdown_type - enum represents the shutdown type of the PM.
+ * @PMF_SHUTDOWN_TYPE_SHUTDOWN: shutdown.
+ * @PMF_SHUTDOWN_TYPE_RESET: reset/reboot.
+ * @PMF_SHUTDOWN_TYPE_SETSCOPE_ONLY: set the shutdown/reboot scope.
+ *
  */
 enum pm_shutdown_type {
 	PMF_SHUTDOWN_TYPE_SHUTDOWN,
@@ -175,9 +180,11 @@
 };
 
 /**
- * @PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM:	shutdown/reboot APU subsystem only
- * @PMF_SHUTDOWN_SUBTYPE_PS_ONLY:	shutdown/reboot entire PS (but not PL)
- * @PMF_SHUTDOWN_SUBTYPE_SYSTEM:	shutdown/reboot entire system
+ * enum pm_shutdown_subtype - enum represents the shutdown subtype of the PM.
+ * @PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM: shutdown/reboot APU subsystem only.
+ * @PMF_SHUTDOWN_SUBTYPE_PS_ONLY: shutdown/reboot entire PS (but not PL).
+ * @PMF_SHUTDOWN_SUBTYPE_SYSTEM: shutdown/reboot entire system.
+ *
  */
 enum pm_shutdown_subtype {
 	PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM,
@@ -186,9 +193,11 @@
 };
 
 /**
- * @PM_PLL_MODE_RESET:		PLL is in reset (not locked)
- * @PM_PLL_MODE_INTEGER:	PLL is locked in integer mode
- * @PM_PLL_MODE_FRACTIONAL:	PLL is locked in fractional mode
+ * enum pm_pll_mode - enum represents the mode of the PLL.
+ * @PM_PLL_MODE_RESET: PLL is in reset (not locked).
+ * @PM_PLL_MODE_INTEGER: PLL is locked in integer mode.
+ * @PM_PLL_MODE_FRACTIONAL: PLL is locked in fractional mode.
+ * @PM_PLL_MODE_MAX: Represents the maximum mode value for the PLL.
  */
 enum pm_pll_mode {
 	PM_PLL_MODE_RESET,
@@ -198,8 +207,10 @@
 };
 
 /**
- * @PM_CLOCK_DIV0_ID:		Clock divider 0
- * @PM_CLOCK_DIV1_ID:		Clock divider 1
+ * enum pm_clock_div_id - enum represents the clock division identifiers in the
+ *                        PM.
+ * @PM_CLOCK_DIV0_ID: Clock divider 0.
+ * @PM_CLOCK_DIV1_ID: Clock divider 1.
  */
 enum pm_clock_div_id {
 	PM_CLOCK_DIV0_ID,
diff --git a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c
index 54b0007..e297ecb 100644
--- a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c
@@ -12,14 +12,12 @@
 
 #include <errno.h>
 
-#include <common/runtime_svc.h>
-#if ZYNQMP_WDT_RESTART
 #include <arch_helpers.h>
+#include <common/runtime_svc.h>
 #include <drivers/arm/gicv2.h>
 #include <lib/mmio.h>
 #include <lib/spinlock.h>
 #include <plat/common/platform.h>
-#endif
 
 #include <plat_private.h>
 #include "pm_client.h"
@@ -36,22 +34,26 @@
 #endif
 
 /**
- * pm_context - Structure which contains data for power management
- * @api_version		version of PM API, must match with one on PMU side
- * @payload		payload array used to store received
- *			data from ipi buffer registers
+ * typedef pm_ctx_t - Structure which contains data for power management.
+ * @api_version: version of PM API, must match with one on PMU side.
+ * @payload: payload array used to store received.
+ *           data from ipi buffer registers.
+ *
  */
-static struct {
+typedef struct {
 	uint32_t api_version;
 	uint32_t payload[PAYLOAD_ARG_CNT];
-} pm_ctx;
+} pm_ctx_t;
+
+static pm_ctx_t pm_ctx;
 
 #if ZYNQMP_WDT_RESTART
 /**
- * trigger_wdt_restart() - Trigger warm restart event to APU cores
+ * trigger_wdt_restart() - Trigger warm restart event to APU cores.
  *
  * This function triggers SGI for all active APU CPUs. SGI handler then
  * power down CPU and call system reset.
+ *
  */
 static void trigger_wdt_restart(void)
 {
@@ -83,15 +85,15 @@
 }
 
 /**
- * ttc_fiq_handler() - TTC Handler for timer event
- * @id         number of the highest priority pending interrupt of the type
- *             that this handler was registered for
- * @flags      security state, bit[0]
- * @handler    pointer to 'cpu_context' structure of the current CPU for the
- *             security state specified in the 'flags' parameter
- * @cookie     unused
+ * ttc_fiq_handler() - TTC Handler for timer event.
+ * @id: number of the highest priority pending interrupt of the type
+ *      that this handler was registered for.
+ * @flags: security state, bit[0].
+ * @handle: pointer to 'cpu_context' structure of the current CPU for the
+ *           security state specified in the 'flags' parameter.
+ * @cookie: unused.
  *
- * Function registered as INTR_TYPE_EL3 interrupt handler
+ * Function registered as INTR_TYPE_EL3 interrupt handler.
  *
  * When WDT event is received in PMU, PMU needs to notify master to do cleanup
  * if required. PMU sets up timer and starts timer to overflow in zero time upon
@@ -101,6 +103,9 @@
  * In presence of non-secure software layers (EL1/2) sets the interrupt
  * at registered entrance in GIC and informs that PMU responded or demands
  * action.
+ *
+ * Return: 0 on success.
+ *
  */
 static uint64_t ttc_fiq_handler(uint32_t id, uint32_t flags, void *handle,
 				void *cookie)
@@ -121,19 +126,21 @@
 }
 
 /**
- * zynqmp_sgi7_irq() - Handler for SGI7 IRQ
- * @id         number of the highest priority pending interrupt of the type
- *             that this handler was registered for
- * @flags      security state, bit[0]
- * @handler    pointer to 'cpu_context' structure of the current CPU for the
- *             security state specified in the 'flags' parameter
- * @cookie     unused
+ * zynqmp_sgi7_irq() - Handler for SGI7 IRQ.
+ * @id: number of the highest priority pending interrupt of the type
+ *      that this handler was registered for.
+ * @flags: security state, bit[0].
+ * @handle: pointer to 'cpu_context' structure of the current CPU for the
+ *           security state specified in the 'flags' parameter.
+ * @cookie: unused.
  *
  * Function registered as INTR_TYPE_EL3 interrupt handler
  *
  * On receiving WDT event from PMU, TF-A generates SGI7 to all running CPUs.
  * In response to SGI7 interrupt, each CPUs do clean up if required and last
  * running CPU calls system restart.
+ *
+ * Return: This function does not return a value and it enters into wfi.
  */
 static uint64_t __unused __dead2 zynqmp_sgi7_irq(uint32_t id, uint32_t flags,
 						 void *handle, void *cookie)
@@ -168,7 +175,9 @@
 }
 
 /**
- * pm_wdt_restart_setup() - Setup warm restart interrupts
+ * pm_wdt_restart_setup() - Setup warm restart interrupts.
+ *
+ * Return: Returns status, 0 on success or error+reason.
  *
  * This function sets up handler for SGI7 and TTC interrupts
  * used for warm restart.
@@ -194,17 +203,18 @@
 #endif
 
 /**
- * pm_setup() - PM service setup
+ * pm_setup() - PM service setup.
  *
- * @return	On success, the initialization function must return 0.
- *		Any other return value will cause the framework to ignore
- *		the service
+ * Return: On success, the initialization function must return 0.
+ *         Any other return value will cause the framework to ignore
+ *         the service.
  *
  * Initialization functions for ZynqMP power management for
  * communicaton with PMU.
  *
  * Called from sip_svc_setup initialization function with the
  * rt_svc_init signature.
+ *
  */
 int32_t pm_setup(void)
 {
@@ -249,19 +259,24 @@
 
 /**
  * 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
+ * @smc_fid: Function Identifier.
+ * @x1: Arguments.
+ * @x2: Arguments.
+ * @x3: Arguments.
+ * @x4: Arguments.
+ * @cookie: Unused.
+ * @handle: Pointer to caller's context structure.
+ * @flags: SECURE_FLAG or NON_SECURE_FLAG.
  *
  * 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 unknown SMC Function ID.
  *
  * The SMC calls for PM service are forwarded from SIP Service SMC handler
- * function with rt_svc_handle signature
+ * function with rt_svc_handle signature.
+ *
+ * Return: Unused.
+ *
  */
 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)
diff --git a/plat/xilinx/zynqmp/sip_svc_setup.c b/plat/xilinx/zynqmp/sip_svc_setup.c
index 7ddd28c..6a8555e 100644
--- a/plat/xilinx/zynqmp/sip_svc_setup.c
+++ b/plat/xilinx/zynqmp/sip_svc_setup.c
@@ -40,9 +40,13 @@
 	0xb9, 0x25, 0x82, 0x2d, 0xe3, 0xa5);
 
 /**
- * sip_svc_setup() - Setup SiP Service
+ * sip_svc_setup() - Setup SiP Service.
  *
- * Invokes PM setup
+ * Return: On success, the initialization function must return 0.
+ *         Any other return value will cause the framework to ignore
+ *         the service.
+ *
+ * Invokes PM setup.
  */
 static int32_t sip_svc_setup(void)
 {
@@ -52,9 +56,19 @@
 
 /**
  * sip_svc_smc_handler() - Top-level SiP Service SMC handler
+ * @smc_fid: Function Identifier.
+ * @x1: SMC64 Arguments 1 from kernel.
+ * @x2: SMC64 Arguments 2 from kernel.
+ * @x3: SMC64 Arguments 3 from kernel(upper 32-bits).
+ * @x4: SMC64 Arguments 4 from kernel.
+ * @cookie: Unused
+ * @handle: Pointer to caller's context structure.
+ * @flags: SECURE_FLAG or NON_SECURE_FLAG.
  *
  * Handler for all SiP SMC calls. Handles standard SIP requests
  * and calls PM SMC handler if the call is for a PM-API function.
+ *
+ * Return: Unused.
  */
 static uintptr_t sip_svc_smc_handler(uint32_t smc_fid,
 			      u_register_t x1,
diff --git a/plat/xilinx/zynqmp/tsp/tsp_plat_setup.c b/plat/xilinx/zynqmp/tsp/tsp_plat_setup.c
index eaecb89..ed3300b 100644
--- a/plat/xilinx/zynqmp/tsp/tsp_plat_setup.c
+++ b/plat/xilinx/zynqmp/tsp/tsp_plat_setup.c
@@ -8,9 +8,10 @@
 #include <common/debug.h>
 #include <drivers/console.h>
 #include <plat/arm/common/plat_arm.h>
-#include <plat_private.h>
 #include <platform_tsp.h>
 
+#include <plat_private.h>
+
 /*******************************************************************************
  * Initialize the UART
  ******************************************************************************/
diff --git a/plat/xilinx/zynqmp/zynqmp_ehf.c b/plat/xilinx/zynqmp/zynqmp_ehf.c
index 56dc79c..18c3749 100644
--- a/plat/xilinx/zynqmp/zynqmp_ehf.c
+++ b/plat/xilinx/zynqmp/zynqmp_ehf.c
@@ -5,10 +5,10 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <platform_def.h>
-
 #include <bl31/ehf.h>
 
+#include <platform_def.h>
+
 /*
  * Enumeration of priority levels on ARM platforms.
  */
diff --git a/plat/xilinx/zynqmp/zynqmp_ipi.c b/plat/xilinx/zynqmp/zynqmp_ipi.c
index c7d2c08..439bd6f 100644
--- a/plat/xilinx/zynqmp/zynqmp_ipi.c
+++ b/plat/xilinx/zynqmp/zynqmp_ipi.c
@@ -85,7 +85,7 @@
 };
 
 /**
- * zynqmp_ipi_config_table_init() - Initialize ZynqMP IPI configuration data
+ * zynqmp_ipi_config_table_init() - Initialize ZynqMP IPI configuration data.
  *
  */
 void zynqmp_ipi_config_table_init(void)
diff --git a/plat/xilinx/zynqmp/zynqmp_sdei.c b/plat/xilinx/zynqmp/zynqmp_sdei.c
index 984252e..ce68e6a 100644
--- a/plat/xilinx/zynqmp/zynqmp_sdei.c
+++ b/plat/xilinx/zynqmp/zynqmp_sdei.c
@@ -9,9 +9,9 @@
 
 #include <bl31/ehf.h>
 #include <common/debug.h>
+#include <plat/common/platform.h>
 #include <services/sdei.h>
 
-#include <plat/common/platform.h>
 #include <platform_def.h>
 
 int arm_validate_ns_entrypoint(uintptr_t entrypoint)
diff --git a/services/std_svc/errata_abi/errata_abi_main.c b/services/std_svc/errata_abi/errata_abi_main.c
index bc176c6..d7f501b 100644
--- a/services/std_svc/errata_abi/errata_abi_main.c
+++ b/services/std_svc/errata_abi/errata_abi_main.c
@@ -399,7 +399,8 @@
 	.cpu_errata_list = {
 		[0] = {2719103, 0x00, 0x01, ERRATA_V2_2719103, \
 			ERRATA_NON_ARM_INTERCONNECT},
-		[1 ... ERRATA_LIST_END] = UNDEF_ERRATA,
+		[1] = {2801372, 0x00, 0x01, ERRATA_V2_2801372},
+		[2 ... ERRATA_LIST_END] = UNDEF_ERRATA,
 	}
 },
 #endif /* NEOVERSE_V2_H_INC */
diff --git a/services/std_svc/rmmd/rmmd_main.c b/services/std_svc/rmmd/rmmd_main.c
index 24f6c41..a929ea2 100644
--- a/services/std_svc/rmmd/rmmd_main.c
+++ b/services/std_svc/rmmd/rmmd_main.c
@@ -18,6 +18,8 @@
 #include <context.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/el3_runtime/pubsub.h>
+#include <lib/extensions/pmuv3.h>
+#include <lib/extensions/sys_reg_trace.h>
 #include <lib/gpt_rme/gpt_rme.h>
 
 #include <lib/spinlock.h>
@@ -125,6 +127,13 @@
 	 */
 		sve_enable(ctx);
 	}
+
+	/* NS can access this but Realm shouldn't */
+	if (is_feat_sys_reg_trace_supported()) {
+		sys_reg_trace_disable(ctx);
+	}
+
+	pmuv3_enable(ctx);
 }
 
 /*******************************************************************************
diff --git a/services/std_svc/spm/el3_spmc/spmc.h b/services/std_svc/spm/el3_spmc/spmc.h
index 61afee3..13875b9 100644
--- a/services/std_svc/spm/el3_spmc/spmc.h
+++ b/services/std_svc/spm/el3_spmc/spmc.h
@@ -131,6 +131,9 @@
 
 	/* Track the current runtime model of the SP. */
 	enum sp_runtime_model rt_model;
+
+	/* Track the source partition ID to validate a direct response. */
+	uint16_t dir_req_origin_id;
 };
 
 /*
diff --git a/services/std_svc/spm/el3_spmc/spmc_main.c b/services/std_svc/spm/el3_spmc/spmc_main.c
index 08e7218..ada6f45 100644
--- a/services/std_svc/spm/el3_spmc/spmc_main.c
+++ b/services/std_svc/spm/el3_spmc/spmc_main.c
@@ -260,6 +260,65 @@
 }
 
 /*******************************************************************************
+ * Helper function to validate the destination ID of a direct response.
+ ******************************************************************************/
+static bool direct_msg_validate_dst_id(uint16_t dst_id)
+{
+	struct secure_partition_desc *sp;
+
+	/* Check if we're targeting a normal world partition. */
+	if (ffa_is_normal_world_id(dst_id)) {
+		return true;
+	}
+
+	/* Or directed to the SPMC itself.*/
+	if (dst_id == FFA_SPMC_ID) {
+		return true;
+	}
+
+	/* Otherwise ensure the SP exists. */
+	sp = spmc_get_sp_ctx(dst_id);
+	if (sp != NULL) {
+		return true;
+	}
+
+	return false;
+}
+
+/*******************************************************************************
+ * Helper function to validate the response from a Logical Partition.
+ ******************************************************************************/
+static bool direct_msg_validate_lp_resp(uint16_t origin_id, uint16_t lp_id,
+					void *handle)
+{
+	/* Retrieve populated Direct Response Arguments. */
+	uint64_t x1 = SMC_GET_GP(handle, CTX_GPREG_X1);
+	uint64_t x2 = SMC_GET_GP(handle, CTX_GPREG_X2);
+	uint16_t src_id = ffa_endpoint_source(x1);
+	uint16_t dst_id = ffa_endpoint_destination(x1);
+
+	if (src_id != lp_id) {
+		ERROR("Invalid EL3 LP source ID (0x%x).\n", src_id);
+		return false;
+	}
+
+	/*
+	 * Check the destination ID is valid and ensure the LP is responding to
+	 * the original request.
+	 */
+	if ((!direct_msg_validate_dst_id(dst_id)) || (dst_id != origin_id)) {
+		ERROR("Invalid EL3 LP destination ID (0x%x).\n", dst_id);
+		return false;
+	}
+
+	if (!direct_msg_validate_arg2(x2)) {
+		ERROR("Invalid EL3 LP message encoding.\n");
+		return false;
+	}
+	return true;
+}
+
+/*******************************************************************************
  * Handle direct request messages and route to the appropriate destination.
  ******************************************************************************/
 static uint64_t direct_req_smc_handler(uint32_t smc_fid,
@@ -272,6 +331,7 @@
 				       void *handle,
 				       uint64_t flags)
 {
+	uint16_t src_id = ffa_endpoint_source(x1);
 	uint16_t dst_id = ffa_endpoint_destination(x1);
 	struct el3_lp_desc *el3_lp_descs;
 	struct secure_partition_desc *sp;
@@ -283,14 +343,29 @@
 					     FFA_ERROR_INVALID_PARAMETER);
 	}
 
+	/* Validate Sender is either the current SP or from the normal world. */
+	if ((secure_origin && src_id != spmc_get_current_sp_ctx()->sp_id) ||
+		(!secure_origin && !ffa_is_normal_world_id(src_id))) {
+		ERROR("Invalid direct request source ID (0x%x).\n", src_id);
+		return spmc_ffa_error_return(handle,
+					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);
+			uint64_t ret = el3_lp_descs[i].direct_req(
+						smc_fid, secure_origin, x1, x2,
+						x3, x4, cookie, handle, flags);
+			if (!direct_msg_validate_lp_resp(src_id, dst_id,
+							 handle)) {
+				panic();
+			}
+
+			/* Message checks out. */
+			return ret;
 		}
 	}
 
@@ -332,6 +407,7 @@
 	 */
 	sp->ec[idx].rt_state = RT_STATE_RUNNING;
 	sp->ec[idx].rt_model = RT_MODEL_DIR_REQ;
+	sp->ec[idx].dir_req_origin_id = src_id;
 	return spmc_smc_return(smc_fid, secure_origin, x1, x2, x3, x4,
 			       handle, cookie, flags, dst_id);
 }
@@ -370,7 +446,7 @@
 	 * Check that the response is either targeted to the Normal world or the
 	 * SPMC e.g. a PM response.
 	 */
-	if ((dst_id != FFA_SPMC_ID) && ffa_is_secure_world_id(dst_id)) {
+	if (!direct_msg_validate_dst_id(dst_id)) {
 		VERBOSE("Direct response to invalid partition ID (0x%x).\n",
 			dst_id);
 		return spmc_ffa_error_return(handle,
@@ -397,9 +473,18 @@
 		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
 	}
 
+	if (sp->ec[idx].dir_req_origin_id != dst_id) {
+		WARN("Invalid direct resp partition ID 0x%x != 0x%x on core%u.\n",
+		     dst_id, sp->ec[idx].dir_req_origin_id, idx);
+		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
+	}
+
 	/* Update the state of the SP execution context. */
 	sp->ec[idx].rt_state = RT_STATE_WAITING;
 
+	/* Clear the ongoing direct request ID. */
+	sp->ec[idx].dir_req_origin_id = INV_SP_ID;
+
 	/*
 	 * If the receiver is not the SPMC then forward the response to the
 	 * Normal world.
diff --git a/services/std_svc/spm/el3_spmc/spmc_pm.c b/services/std_svc/spm/el3_spmc/spmc_pm.c
index d25344c..c7e864f 100644
--- a/services/std_svc/spm/el3_spmc/spmc_pm.c
+++ b/services/std_svc/spm/el3_spmc/spmc_pm.c
@@ -83,6 +83,7 @@
 	/* Update the runtime model and state of the partition. */
 	ec->rt_model = RT_MODEL_INIT;
 	ec->rt_state = RT_STATE_RUNNING;
+	ec->dir_req_origin_id = INV_SP_ID;
 
 	INFO("SP (0x%x) init start on core%u.\n", sp->sp_id, linear_id);
 
@@ -132,6 +133,7 @@
 	/* Update the runtime model and state of the partition. */
 	ec->rt_model = RT_MODEL_DIR_REQ;
 	ec->rt_state = RT_STATE_RUNNING;
+	ec->dir_req_origin_id = FFA_SPMC_ID;
 
 	rc = spmc_sp_synchronous_entry(ec);
 	if (rc != 0ULL) {
diff --git a/services/std_svc/spm/el3_spmc/spmc_shared_mem.c b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
index 8183a0a..5263c04 100644
--- a/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
+++ b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
@@ -743,10 +743,12 @@
 	/*
 	 * Overflow is impossible: the arithmetic happens in at least 64-bit
 	 * precision, but all of the operands are bounded by UINT32_MAX, and
-	 * ((2^32 - 1)^2 + (2^32 - 1) + (2^32 - 1)) = ((2^32 - 1) * (2^32 + 1))
+	 *   ((2^32 - 1) * (2^32 - 1) + (2^32 - 1) + (2^32 - 1))
+	 * = ((2^32 - 1) * ((2^32 - 1) + 1 + 1))
+	 * = ((2^32 - 1) * (2^32 + 1))
 	 * = (2^64 - 1).
 	 */
-	CASSERT(sizeof(desc->emad_count == 4), assert_emad_count_max_too_large);
+	CASSERT(sizeof(desc->emad_count) == 4, assert_emad_count_max_too_large);
 	emad_end = (desc->emad_count * (unsigned long long)emad_size) +
 		   (unsigned long long)sizeof(struct ffa_comp_mrd) +
 		   (unsigned long long)emad_offset;
@@ -777,10 +779,16 @@
 static int spmc_shmem_check_obj(struct spmc_shmem_obj *obj,
 				uint32_t ffa_version)
 {
+	unsigned long long total_page_count;
 	const struct ffa_emad_v1_0 *first_emad;
 	const struct ffa_emad_v1_0 *end_emad;
 	size_t emad_size;
-	uint32_t comp_mrd_offset = 0;
+	uint32_t comp_mrd_offset;
+	size_t header_emad_size;
+	size_t size;
+	size_t count;
+	size_t expected_size;
+	const struct ffa_comp_mrd *comp;
 
 	if (obj->desc_filled != obj->desc_size) {
 		ERROR("BUG: %s called on incomplete object (%zu != %zu)\n",
@@ -798,22 +806,12 @@
 	first_emad = spmc_shmem_obj_get_emad(&obj->desc, 0,
 					     ffa_version, &emad_size);
 	end_emad = emad_advance(first_emad, obj->desc.emad_count * emad_size);
+	comp_mrd_offset = first_emad->comp_mrd_offset;
 
 	/* Loop through the endpoint descriptors, validating each of them. */
-	for (const struct ffa_emad_v1_0 *emad = first_emad;
-	     emad < end_emad;
-	     emad = emad_advance(emad, emad_size)) {
-		size_t size;
-		size_t count;
-		size_t expected_size;
-		uint64_t total_page_count;
-		size_t header_emad_size;
-		uint32_t offset;
-		struct ffa_comp_mrd *comp;
+	for (const struct ffa_emad_v1_0 *emad = first_emad; emad < end_emad;) {
 		ffa_endpoint_id16_t ep_id;
 
-		offset = emad->comp_mrd_offset;
-
 		/*
 		 * If a partition ID resides in the secure world validate that
 		 * the partition ID is for a known partition. Ignore any
@@ -831,82 +829,121 @@
 
 		/*
 		 * The offset provided to the composite memory region descriptor
-		 * should be consistent across endpoint descriptors. Store the
-		 * first entry and compare against subsequent entries.
+		 * should be consistent across endpoint descriptors.
+		 */
+		if (comp_mrd_offset != emad->comp_mrd_offset) {
+			ERROR("%s: mismatching offsets provided, %u != %u\n",
+			       __func__, emad->comp_mrd_offset, comp_mrd_offset);
+			return FFA_ERROR_INVALID_PARAMETER;
+		}
+
+		/* Advance to the next endpoint descriptor */
+		emad = emad_advance(emad, emad_size);
+
+		/*
+		 * Ensure neither this emad nor any subsequent emads have
+		 * the same partition ID as the previous emad.
 		 */
-		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);
+		for (const struct ffa_emad_v1_0 *other_emad = emad;
+		     other_emad < end_emad;
+		     other_emad = emad_advance(other_emad, emad_size)) {
+			if (ep_id == other_emad->mapd.endpoint_id) {
+				WARN("%s: Duplicated endpoint id 0x%x\n",
+				     __func__, emad->mapd.endpoint_id);
 				return FFA_ERROR_INVALID_PARAMETER;
 			}
-			continue; /* Remainder only executed on first iteration. */
 		}
+	}
 
-		header_emad_size = (size_t)((uint8_t *)emad - (uint8_t *)&obj->desc) +
-			(obj->desc.emad_count * emad_size);
+	header_emad_size = (size_t)((const uint8_t *)end_emad -
+				    (const uint8_t *)&obj->desc);
 
-		if (offset < header_emad_size) {
-			WARN("%s: invalid object, offset %u < header + emad %zu\n",
-			     __func__, offset, header_emad_size);
-			return FFA_ERROR_INVALID_PARAMETER;
-		}
+	/*
+	 * Check that the composite descriptor
+	 * is after the endpoint descriptors.
+	 */
+	if (comp_mrd_offset < header_emad_size) {
+		WARN("%s: invalid object, offset %u < header + emad %zu\n",
+		     __func__, comp_mrd_offset, header_emad_size);
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
 
-		size = obj->desc_size;
+	/* Ensure the composite descriptor offset is aligned. */
+	if (!is_aligned(comp_mrd_offset, 16)) {
+		WARN("%s: invalid object, unaligned composite memory "
+		     "region descriptor offset %u.\n",
+		     __func__, comp_mrd_offset);
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
 
-		if (offset > size) {
-			WARN("%s: invalid object, offset %u > total size %zu\n",
-			     __func__, offset, obj->desc_size);
-			return FFA_ERROR_INVALID_PARAMETER;
-		}
-		size -= offset;
+	size = obj->desc_size;
 
-		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 FFA_ERROR_INVALID_PARAMETER;
-		}
-		size -= sizeof(struct ffa_comp_mrd);
+	/* Check that the composite descriptor is in bounds. */
+	if (comp_mrd_offset > size) {
+		WARN("%s: invalid object, offset %u > total size %zu\n",
+		     __func__, comp_mrd_offset, obj->desc_size);
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
+	size -= comp_mrd_offset;
 
-		count = size / sizeof(struct ffa_cons_mrd);
+	/* Check that there is enough space for the composite descriptor. */
+	if (size < sizeof(struct ffa_comp_mrd)) {
+		WARN("%s: invalid object, offset %u, total size %zu, no header space.\n",
+		     __func__, comp_mrd_offset, obj->desc_size);
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
+	size -= sizeof(*comp);
 
-		comp = spmc_shmem_obj_get_comp_mrd(obj, ffa_version);
+	count = size / sizeof(struct ffa_cons_mrd);
 
-		if (comp == NULL) {
-			WARN("%s: invalid comp_mrd offset\n", __func__);
-			return FFA_ERROR_INVALID_PARAMETER;
-		}
+	comp = (const struct ffa_comp_mrd *)
+	       ((const uint8_t *)(&obj->desc) + comp_mrd_offset);
 
-		if (comp->address_range_count != count) {
-			WARN("%s: invalid object, desc count %u != %zu\n",
-			     __func__, comp->address_range_count, count);
-			return FFA_ERROR_INVALID_PARAMETER;
-		}
+	if (comp->address_range_count != count) {
+		WARN("%s: invalid object, desc count %u != %zu\n",
+		     __func__, comp->address_range_count, count);
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
 
-		expected_size = offset + sizeof(*comp) +
-			count * sizeof(struct ffa_cons_mrd);
+	/* Ensure that the expected and actual sizes are equal. */
+	expected_size = comp_mrd_offset + sizeof(*comp) +
+		count * sizeof(struct ffa_cons_mrd);
 
-		if (expected_size != obj->desc_size) {
-			WARN("%s: invalid object, computed size %zu != size %zu\n",
-			       __func__, expected_size, obj->desc_size);
-			return FFA_ERROR_INVALID_PARAMETER;
-		}
+	if (expected_size != obj->desc_size) {
+		WARN("%s: invalid object, computed size %zu != size %zu\n",
+		       __func__, expected_size, obj->desc_size);
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
 
-		total_page_count = 0;
+	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 != %" PRIu64 "\n",
-			     __func__, comp->total_page_count,
-			total_page_count);
-			return FFA_ERROR_INVALID_PARAMETER;
+	/*
+	 * comp->address_range_count is 32-bit, so 'count' must fit in a
+	 * uint32_t at this point.
+	 */
+	for (size_t i = 0; i < count; i++) {
+		const struct ffa_cons_mrd *mrd = comp->address_range_array + i;
+
+		if (!is_aligned(mrd->address, PAGE_SIZE)) {
+			WARN("%s: invalid object, address in region descriptor "
+			     "%zu not 4K aligned (got 0x%016llx)",
+			     __func__, i, (unsigned long long)mrd->address);
 		}
+
+		/*
+		 * No overflow possible: total_page_count can hold at
+		 * least 2^64 - 1, but will be have at most 2^32 - 1.
+		 * values added to it, each of which cannot exceed 2^32 - 1.
+		 */
+		total_page_count += mrd->page_count;
+	}
+
+	if (comp->total_page_count != total_page_count) {
+		WARN("%s: invalid object, desc total_page_count %u != %llu\n",
+		     __func__, comp->total_page_count, total_page_count);
+		return FFA_ERROR_INVALID_PARAMETER;
 	}
+
 	return 0;
 }
 
@@ -969,11 +1006,8 @@
 			       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__);
@@ -1056,26 +1090,6 @@
 		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);
-
-		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 (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__);
diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index 6b16373..587e60f 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c
@@ -273,13 +273,16 @@
 
 	assert(plat_ic_get_pending_interrupt_type() == INTR_TYPE_EL3);
 
-	intid = plat_ic_get_pending_interrupt_id();
+	intid = plat_ic_acknowledge_interrupt();
 
 	if (plat_spmd_handle_group0_interrupt(intid) < 0) {
 		ERROR("Group0 interrupt %u not handled\n", intid);
 		panic();
 	}
 
+	/* Deactivate the corresponding Group0 interrupt. */
+	plat_ic_end_of_interrupt(intid);
+
 	return 0U;
 }
 #endif
@@ -300,7 +303,7 @@
 
 	assert(plat_ic_get_pending_interrupt_type() == INTR_TYPE_EL3);
 
-	intid = plat_ic_get_pending_interrupt_id();
+	intid = plat_ic_acknowledge_interrupt();
 
 	/*
 	 * TODO: Currently due to a limitation in SPMD implementation, the
@@ -313,6 +316,9 @@
 		panic();
 	}
 
+	/* Deactivate the corresponding Group0 interrupt. */
+	plat_ic_end_of_interrupt(intid);
+
 	/* Return success. */
 	SMC_RET8(handle, FFA_SUCCESS_SMC32, FFA_PARAM_MBZ, FFA_PARAM_MBZ,
 		 FFA_PARAM_MBZ, FFA_PARAM_MBZ, FFA_PARAM_MBZ, FFA_PARAM_MBZ,
@@ -1187,7 +1193,7 @@
 		if (secure_origin) {
 			return spmd_handle_group0_intr_swd(handle);
 		} else {
-			return spmd_ffa_error_return(handle, FFA_ERROR_DENIED);
+			return spmd_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
 		}
 	default:
 		WARN("SPM: Unsupported call 0x%08x\n", smc_fid);
diff --git a/tools/cert_create/Makefile b/tools/cert_create/Makefile
index 042e844..b911d19 100644
--- a/tools/cert_create/Makefile
+++ b/tools/cert_create/Makefile
@@ -85,9 +85,9 @@
 
 .PHONY: all clean realclean --openssl
 
-all: ${BINARY}
+all: --openssl ${BINARY}
 
-${BINARY}: --openssl ${OBJECTS} Makefile
+${BINARY}: ${OBJECTS} Makefile
 	@echo "  HOSTLD  $@"
 	@echo 'const char build_msg[] = "Built : "__TIME__", "__DATE__; \
                 const char platform_msg[] = "${PLAT_MSG}";' | \
diff --git a/tools/conventional-changelog-tf-a/index.js b/tools/conventional-changelog-tf-a/index.js
index 2a9d5b4..7d57c15 100644
--- a/tools/conventional-changelog-tf-a/index.js
+++ b/tools/conventional-changelog-tf-a/index.js
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -167,6 +167,17 @@
 
             writerOpts.transform = function (commit, context) {
                 /*
+                 * Feedback on the generated changelog has shown that having build system changes
+                 * appear at the top of a section throws some people off. We make an exception for
+                 * scopeless `build`-type changes and treat them as though they actually have the
+                 * `build` scope.
+                 */
+
+                if ((commit.type === "build") && (commit.scope == null)) {
+                    commit.scope = "build";
+                }
+
+                /*
                  * Fix up commit trailers, which for some reason are not correctly recognized and
                  * end up showing up in the breaking changes.
                  */
diff --git a/tools/encrypt_fw/Makefile b/tools/encrypt_fw/Makefile
index 2939b14..924e5fe 100644
--- a/tools/encrypt_fw/Makefile
+++ b/tools/encrypt_fw/Makefile
@@ -65,9 +65,9 @@
 
 .PHONY: all clean realclean --openssl
 
-all: ${BINARY}
+all: --openssl ${BINARY}
 
-${BINARY}: --openssl ${OBJECTS} Makefile
+${BINARY}: ${OBJECTS} Makefile
 	@echo "  HOSTLD  $@"
 	@echo 'const char build_msg[] = "Built : "__TIME__", "__DATE__;' | \
                 ${HOSTCC} -c ${HOSTCCFLAGS} -xc - -o src/build_msg.o
diff --git a/tools/fiptool/Makefile b/tools/fiptool/Makefile
index 2ebee33..4bdebd9 100644
--- a/tools/fiptool/Makefile
+++ b/tools/fiptool/Makefile
@@ -68,9 +68,9 @@
 
 .PHONY: all clean distclean --openssl
 
-all: ${PROJECT}
+all: --openssl ${PROJECT}
 
-${PROJECT}: --openssl ${OBJECTS} Makefile
+${PROJECT}: ${OBJECTS} Makefile
 	@echo "  HOSTLD  $@"
 	${Q}${HOSTCC} ${OBJECTS} -o $@ ${LDLIBS}
 	@${ECHO_BLANK_LINE}