Adding upstream version 2.8.0+dfsg.upstream/2.8.0+dfsgupstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

22 files changed, 1850 insertions, 0 deletions

diff --git a/bl2/aarch32/bl2_arch_setup.c b/bl2/aarch32/bl2_arch_setup.c
new file mode 100644
index 0000000..4fd8d07
--- /dev/null
+++ b/bl2/aarch32/bl2_arch_setup.c
@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "../bl2_private.h"
+
+/*******************************************************************************
+ * Place holder function to perform any Secure SVC specific architectural
+ * setup. At the moment there is nothing to do.
+ ******************************************************************************/
+void bl2_arch_setup(void)
+{
+
+}
diff --git a/bl2/aarch32/bl2_el3_entrypoint.S b/bl2/aarch32/bl2_el3_entrypoint.S
new file mode 100644
index 0000000..40154aa
--- /dev/null
+++ b/bl2/aarch32/bl2_el3_entrypoint.S
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2017-2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <el3_common_macros.S>
+
+	.globl	bl2_entrypoint
+
+
+func bl2_entrypoint
+	/* Save arguments x0-x3 from previous Boot loader */
+	mov	r9, r0
+	mov	r10, r1
+	mov	r11, r2
+	mov	r12, r3
+
+	el3_entrypoint_common                                   \
+                _init_sctlr=1                                   \
+                _warm_boot_mailbox=!PROGRAMMABLE_RESET_ADDRESS  \
+                _secondary_cold_boot=!COLD_BOOT_SINGLE_CPU      \
+                _init_memory=1                                  \
+                _init_c_runtime=1                               \
+                _exception_vectors=bl2_vector_table		\
+		_pie_fixup_size=0
+
+	/*
+	 * Restore parameters of boot rom
+	 */
+	mov	r0, r9
+	mov	r1, r10
+	mov	r2, r11
+	mov	r3, r12
+
+	/* ---------------------------------------------
+	 * Perform BL2 setup
+	 * ---------------------------------------------
+	 */
+	bl	bl2_el3_setup
+
+	/* ---------------------------------------------
+	 * Jump to main function.
+	 * ---------------------------------------------
+	 */
+	bl	bl2_main
+
+	/* ---------------------------------------------
+	 * Should never reach this point.
+	 * ---------------------------------------------
+	 */
+	no_ret	plat_panic_handler
+
+endfunc bl2_entrypoint
diff --git a/bl2/aarch32/bl2_el3_exceptions.S b/bl2/aarch32/bl2_el3_exceptions.S
new file mode 100644
index 0000000..eaa2582
--- /dev/null
+++ b/bl2/aarch32/bl2_el3_exceptions.S
@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+
+	.globl	bl2_vector_table
+
+vector_base bl2_vector_table
+	b	bl2_entrypoint
+	b	report_exception	/* Undef */
+	b	report_exception	/* SVC call */
+	b	report_prefetch_abort	/* Prefetch abort */
+	b	report_data_abort	/* Data abort */
+	b	report_exception	/* Reserved */
+	b	report_exception	/* IRQ */
+	b	report_exception	/* FIQ */
diff --git a/bl2/aarch32/bl2_entrypoint.S b/bl2/aarch32/bl2_entrypoint.S
new file mode 100644
index 0000000..678d9c2
--- /dev/null
+++ b/bl2/aarch32/bl2_entrypoint.S
@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+
+	.globl	bl2_vector_table
+	.globl	bl2_entrypoint
+
+
+vector_base bl2_vector_table
+	b	bl2_entrypoint
+	b	report_exception	/* Undef */
+	b	report_exception	/* SVC call */
+	b	report_prefetch_abort	/* Prefetch abort */
+	b	report_data_abort	/* Data abort */
+	b	report_exception	/* Reserved */
+	b	report_exception	/* IRQ */
+	b	report_exception	/* FIQ */
+
+
+func bl2_entrypoint
+	/*---------------------------------------------
+	 * Save arguments x0 - x3 from BL1 for future
+	 * use.
+	 * ---------------------------------------------
+	 */
+	mov	r9, r0
+	mov	r10, r1
+	mov	r11, r2
+	mov	r12, r3
+
+	/* ---------------------------------------------
+	 * Set the exception vector to something sane.
+	 * ---------------------------------------------
+	 */
+	ldr	r0, =bl2_vector_table
+	stcopr	r0, VBAR
+	isb
+
+	/* --------------------------------------------------------
+	 * Enable the instruction cache - disable speculative loads
+	 * --------------------------------------------------------
+	 */
+	ldcopr	r0, SCTLR
+	orr	r0, r0, #SCTLR_I_BIT
+	bic	r0, r0, #SCTLR_DSSBS_BIT
+	stcopr	r0, SCTLR
+	isb
+
+	/* ---------------------------------------------
+	 * Since BL2 executes after BL1, it is assumed
+	 * here that BL1 has already has done the
+	 * necessary register initializations.
+	 * ---------------------------------------------
+	 */
+
+	/* ---------------------------------------------
+	 * Invalidate the RW memory used by the BL2
+	 * image. This includes the data and NOBITS
+	 * sections. This is done to safeguard against
+	 * possible corruption of this memory by dirty
+	 * cache lines in a system cache as a result of
+	 * use by an earlier boot loader stage.
+	 * ---------------------------------------------
+	 */
+	ldr	r0, =__RW_START__
+	ldr	r1, =__RW_END__
+	sub	r1, r1, r0
+	bl	inv_dcache_range
+
+	/* ---------------------------------------------
+	 * Zero out NOBITS sections. There are 2 of them:
+	 *   - the .bss section;
+	 *   - the coherent memory section.
+	 * ---------------------------------------------
+	 */
+	ldr	r0, =__BSS_START__
+	ldr	r1, =__BSS_END__
+	sub 	r1, r1, r0
+	bl	zeromem
+
+#if USE_COHERENT_MEM
+	ldr	r0, =__COHERENT_RAM_START__
+	ldr	r1, =__COHERENT_RAM_END_UNALIGNED__
+	sub 	r1, r1, r0
+	bl	zeromem
+#endif
+
+	/* --------------------------------------------
+	 * Allocate a stack whose memory will be marked
+	 * as Normal-IS-WBWA when the MMU is enabled.
+	 * There is no risk of reading stale stack
+	 * memory after enabling the MMU as only the
+	 * primary cpu is running at the moment.
+	 * --------------------------------------------
+	 */
+	bl	plat_set_my_stack
+
+	/* ---------------------------------------------
+	 * Initialize the stack protector canary before
+	 * any C code is called.
+	 * ---------------------------------------------
+	 */
+#if STACK_PROTECTOR_ENABLED
+	bl	update_stack_protector_canary
+#endif
+
+	/* ---------------------------------------------
+	 * Perform BL2 setup
+	 * ---------------------------------------------
+	 */
+	mov	r0, r9
+	mov	r1, r10
+	mov	r2, r11
+	mov	r3, r12
+
+	bl	bl2_setup
+
+	/* ---------------------------------------------
+	 * Jump to main function.
+	 * ---------------------------------------------
+	 */
+	bl	bl2_main
+
+	/* ---------------------------------------------
+	 * Should never reach this point.
+	 * ---------------------------------------------
+	 */
+	no_ret	plat_panic_handler
+
+endfunc bl2_entrypoint
diff --git a/bl2/aarch32/bl2_run_next_image.S b/bl2/aarch32/bl2_run_next_image.S
new file mode 100644
index 0000000..0b3554e
--- /dev/null
+++ b/bl2/aarch32/bl2_run_next_image.S
@@ -0,0 +1,46 @@
+/*
+ * 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>
+
+	.globl	bl2_run_next_image
+
+
+func bl2_run_next_image
+	mov	r8,r0
+
+	/*
+	 * MMU needs to be disabled because both BL2 and BL32 execute
+	 * in PL1, and therefore share the same address space.
+	 * BL32 will initialize the address space according to its
+	 * own requirement.
+	 */
+	bl	disable_mmu_icache_secure
+	stcopr	r0, TLBIALL
+	dsb	sy
+	isb
+	mov	r0, r8
+	bl	bl2_el3_plat_prepare_exit
+
+	/*
+	 * Extract PC and SPSR based on struct `entry_point_info_t`
+	 * and load it in LR and SPSR registers respectively.
+	 */
+	ldr	lr, [r8, #ENTRY_POINT_INFO_PC_OFFSET]
+	ldr	r1, [r8, #(ENTRY_POINT_INFO_PC_OFFSET + 4)]
+	msr	spsr_xc, r1
+
+	/* Some BL32 stages expect lr_svc to provide the BL33 entry address */
+	cps	#MODE32_svc
+	ldr	lr, [r8, #ENTRY_POINT_INFO_LR_SVC_OFFSET]
+	cps	#MODE32_mon
+
+	add	r8, r8, #ENTRY_POINT_INFO_ARGS_OFFSET
+	ldm	r8, {r0, r1, r2, r3}
+	exception_return
+endfunc bl2_run_next_image
diff --git a/bl2/aarch64/bl2_arch_setup.c b/bl2/aarch64/bl2_arch_setup.c
new file mode 100644
index 0000000..54052f7
--- /dev/null
+++ b/bl2/aarch64/bl2_arch_setup.c
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2013-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include "../bl2_private.h"
+
+/*******************************************************************************
+ * Place holder function to perform any S-EL1 specific architectural setup. At
+ * the moment there is nothing to do.
+ ******************************************************************************/
+void bl2_arch_setup(void)
+{
+	/* Give access to FP/SIMD registers */
+	write_cpacr(CPACR_EL1_FPEN(CPACR_EL1_FP_TRAP_NONE));
+}
diff --git a/bl2/aarch64/bl2_el3_entrypoint.S b/bl2/aarch64/bl2_el3_entrypoint.S
new file mode 100644
index 0000000..45bac7d
--- /dev/null
+++ b/bl2/aarch64/bl2_el3_entrypoint.S
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2017-2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <el3_common_macros.S>
+
+	.globl	bl2_entrypoint
+
+#if BL2_IN_XIP_MEM
+#define FIXUP_SIZE	0
+#else
+#define FIXUP_SIZE	((BL2_LIMIT) - (BL2_BASE))
+#endif
+
+func bl2_entrypoint
+	/* Save arguments x0-x3 from previous Boot loader */
+	mov	x20, x0
+	mov	x21, x1
+	mov	x22, x2
+	mov	x23, x3
+
+	el3_entrypoint_common                                   \
+		_init_sctlr=1                                   \
+		_warm_boot_mailbox=!PROGRAMMABLE_RESET_ADDRESS  \
+		_secondary_cold_boot=!COLD_BOOT_SINGLE_CPU      \
+		_init_memory=1                                  \
+		_init_c_runtime=1                               \
+		_exception_vectors=bl2_el3_exceptions		\
+		_pie_fixup_size=FIXUP_SIZE
+
+	/* ---------------------------------------------
+	 * Restore parameters of boot rom
+	 * ---------------------------------------------
+	 */
+	mov	x0, x20
+	mov	x1, x21
+	mov	x2, x22
+	mov	x3, x23
+
+	/* ---------------------------------------------
+	 * Perform BL2 setup
+	 * ---------------------------------------------
+	 */
+	bl	bl2_el3_setup
+
+#if ENABLE_PAUTH
+	/* ---------------------------------------------
+	 * Program APIAKey_EL1 and enable pointer authentication.
+	 * ---------------------------------------------
+	 */
+	bl	pauth_init_enable_el3
+#endif /* ENABLE_PAUTH */
+
+	/* ---------------------------------------------
+	 * Jump to main function.
+	 * ---------------------------------------------
+	 */
+	bl	bl2_main
+
+	/* ---------------------------------------------
+	 * Should never reach this point.
+	 * ---------------------------------------------
+	 */
+	no_ret	plat_panic_handler
+endfunc bl2_entrypoint
diff --git a/bl2/aarch64/bl2_el3_exceptions.S b/bl2/aarch64/bl2_el3_exceptions.S
new file mode 100644
index 0000000..3d58051
--- /dev/null
+++ b/bl2/aarch64/bl2_el3_exceptions.S
@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <bl1/bl1.h>
+#include <common/bl_common.h>
+#include <context.h>
+
+/* -----------------------------------------------------------------------------
+ * Very simple stackless exception handlers used by BL2.
+ * -----------------------------------------------------------------------------
+ */
+	.globl	bl2_el3_exceptions
+
+vector_base bl2_el3_exceptions
+
+	/* -----------------------------------------------------
+	 * Current EL with SP0 : 0x0 - 0x200
+	 * -----------------------------------------------------
+	 */
+vector_entry SynchronousExceptionSP0
+	mov	x0, #SYNC_EXCEPTION_SP_EL0
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry SynchronousExceptionSP0
+
+vector_entry IrqSP0
+	mov	x0, #IRQ_SP_EL0
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry IrqSP0
+
+vector_entry FiqSP0
+	mov	x0, #FIQ_SP_EL0
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry FiqSP0
+
+vector_entry SErrorSP0
+	mov	x0, #SERROR_SP_EL0
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry SErrorSP0
+
+	/* -----------------------------------------------------
+	 * Current EL with SPx: 0x200 - 0x400
+	 * -----------------------------------------------------
+	 */
+vector_entry SynchronousExceptionSPx
+	mov	x0, #SYNC_EXCEPTION_SP_ELX
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry SynchronousExceptionSPx
+
+vector_entry IrqSPx
+	mov	x0, #IRQ_SP_ELX
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry IrqSPx
+
+vector_entry FiqSPx
+	mov	x0, #FIQ_SP_ELX
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry FiqSPx
+
+vector_entry SErrorSPx
+	mov	x0, #SERROR_SP_ELX
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry SErrorSPx
+
+	/* -----------------------------------------------------
+	 * Lower EL using AArch64 : 0x400 - 0x600
+	 * -----------------------------------------------------
+	 */
+vector_entry SynchronousExceptionA64
+	mov	x0, #SYNC_EXCEPTION_AARCH64
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry SynchronousExceptionA64
+
+vector_entry IrqA64
+	mov	x0, #IRQ_AARCH64
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry IrqA64
+
+vector_entry FiqA64
+	mov	x0, #FIQ_AARCH64
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry FiqA64
+
+vector_entry SErrorA64
+	mov	x0, #SERROR_AARCH64
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry SErrorA64
+
+	/* -----------------------------------------------------
+	 * Lower EL using AArch32 : 0x600 - 0x800
+	 * -----------------------------------------------------
+	 */
+vector_entry SynchronousExceptionA32
+	mov	x0, #SYNC_EXCEPTION_AARCH32
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry SynchronousExceptionA32
+
+vector_entry IrqA32
+	mov	x0, #IRQ_AARCH32
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry IrqA32
+
+vector_entry FiqA32
+	mov	x0, #FIQ_AARCH32
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry FiqA32
+
+vector_entry SErrorA32
+	mov	x0, #SERROR_AARCH32
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+end_vector_entry SErrorA32
diff --git a/bl2/aarch64/bl2_entrypoint.S b/bl2/aarch64/bl2_entrypoint.S
new file mode 100644
index 0000000..a021e42
--- /dev/null
+++ b/bl2/aarch64/bl2_entrypoint.S
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+
+
+	.globl	bl2_entrypoint
+
+
+
+func bl2_entrypoint
+	/*---------------------------------------------
+	 * Save arguments x0 - x3 from BL1 for future
+	 * use.
+	 * ---------------------------------------------
+	 */
+	mov	x20, x0
+	mov	x21, x1
+	mov	x22, x2
+	mov	x23, x3
+
+	/* ---------------------------------------------
+	 * Set the exception vector to something sane.
+	 * ---------------------------------------------
+	 */
+	adr	x0, early_exceptions
+	msr	vbar_el1, x0
+	isb
+
+	/* ---------------------------------------------
+	 * Enable the SError interrupt now that the
+	 * exception vectors have been setup.
+	 * ---------------------------------------------
+	 */
+	msr	daifclr, #DAIF_ABT_BIT
+
+	/* ---------------------------------------------
+	 * Enable the instruction cache, stack pointer
+	 * and data access alignment checks and disable
+	 * speculative loads.
+	 * ---------------------------------------------
+	 */
+	mov	x1, #(SCTLR_I_BIT | SCTLR_A_BIT | SCTLR_SA_BIT)
+	mrs	x0, sctlr_el1
+	orr	x0, x0, x1
+	bic	x0, x0, #SCTLR_DSSBS_BIT
+	msr	sctlr_el1, x0
+	isb
+
+	/* ---------------------------------------------
+	 * Invalidate the RW memory used by the BL2
+	 * image. This includes the data and NOBITS
+	 * sections. This is done to safeguard against
+	 * possible corruption of this memory by dirty
+	 * cache lines in a system cache as a result of
+	 * use by an earlier boot loader stage.
+	 * ---------------------------------------------
+	 */
+	adr	x0, __RW_START__
+	adr	x1, __RW_END__
+	sub	x1, x1, x0
+	bl	inv_dcache_range
+
+	/* ---------------------------------------------
+	 * Zero out NOBITS sections. There are 2 of them:
+	 *   - the .bss section;
+	 *   - the coherent memory section.
+	 * ---------------------------------------------
+	 */
+	adrp	x0, __BSS_START__
+	add	x0, x0, :lo12:__BSS_START__
+	adrp	x1, __BSS_END__
+	add	x1, x1, :lo12:__BSS_END__
+	sub	x1, x1, x0
+	bl	zeromem
+
+#if USE_COHERENT_MEM
+	adrp	x0, __COHERENT_RAM_START__
+	add	x0, x0, :lo12:__COHERENT_RAM_START__
+	adrp	x1, __COHERENT_RAM_END_UNALIGNED__
+	add	x1, x1, :lo12:__COHERENT_RAM_END_UNALIGNED__
+	sub	x1, x1, x0
+	bl	zeromem
+#endif
+
+	/* --------------------------------------------
+	 * Allocate a stack whose memory will be marked
+	 * as Normal-IS-WBWA when the MMU is enabled.
+	 * There is no risk of reading stale stack
+	 * memory after enabling the MMU as only the
+	 * primary cpu is running at the moment.
+	 * --------------------------------------------
+	 */
+	bl	plat_set_my_stack
+
+	/* ---------------------------------------------
+	 * Initialize the stack protector canary before
+	 * any C code is called.
+	 * ---------------------------------------------
+	 */
+#if STACK_PROTECTOR_ENABLED
+	bl	update_stack_protector_canary
+#endif
+
+	/* ---------------------------------------------
+	 * Perform BL2 setup
+	 * ---------------------------------------------
+	 */
+	mov	x0, x20
+	mov	x1, x21
+	mov	x2, x22
+	mov	x3, x23
+	bl	bl2_setup
+
+#if ENABLE_PAUTH
+	/* ---------------------------------------------
+	 * Program APIAKey_EL1
+	 * and enable pointer authentication.
+	 * ---------------------------------------------
+	 */
+	bl	pauth_init_enable_el1
+#endif /* ENABLE_PAUTH */
+
+	/* ---------------------------------------------
+	 * Jump to main function.
+	 * ---------------------------------------------
+	 */
+	bl	bl2_main
+
+	/* ---------------------------------------------
+	 * Should never reach this point.
+	 * ---------------------------------------------
+	 */
+	no_ret	plat_panic_handler
+
+endfunc bl2_entrypoint
diff --git a/bl2/aarch64/bl2_rme_entrypoint.S b/bl2/aarch64/bl2_rme_entrypoint.S
new file mode 100644
index 0000000..076e326
--- /dev/null
+++ b/bl2/aarch64/bl2_rme_entrypoint.S
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <el3_common_macros.S>
+
+	.globl	bl2_entrypoint
+
+
+func bl2_entrypoint
+	/* Save arguments x0-x3 from previous Boot loader */
+	mov	x20, x0
+	mov	x21, x1
+	mov	x22, x2
+	mov	x23, x3
+
+	el3_entrypoint_common                                   \
+		_init_sctlr=0                                   \
+		_warm_boot_mailbox=0                            \
+		_secondary_cold_boot=0                          \
+		_init_memory=0                                  \
+		_init_c_runtime=1                               \
+		_exception_vectors=bl2_el3_exceptions           \
+		_pie_fixup_size=0
+
+	/* ---------------------------------------------
+	 * Restore parameters of boot rom
+	 * ---------------------------------------------
+	 */
+	mov	x0, x20
+	mov	x1, x21
+	mov	x2, x22
+	mov	x3, x23
+
+	/* ---------------------------------------------
+	 * Perform BL2 setup
+	 * ---------------------------------------------
+	 */
+	bl	bl2_setup
+
+#if ENABLE_PAUTH
+	/* ---------------------------------------------
+	 * Program APIAKey_EL1 and enable pointer authentication.
+	 * ---------------------------------------------
+	 */
+	bl	pauth_init_enable_el3
+#endif /* ENABLE_PAUTH */
+
+	/* ---------------------------------------------
+	 * Jump to main function.
+	 * ---------------------------------------------
+	 */
+	bl	bl2_main
+
+	/* ---------------------------------------------
+	 * Should never reach this point.
+	 * ---------------------------------------------
+	 */
+	no_ret	plat_panic_handler
+endfunc bl2_entrypoint
diff --git a/bl2/aarch64/bl2_run_next_image.S b/bl2/aarch64/bl2_run_next_image.S
new file mode 100644
index 0000000..f0a8be8
--- /dev/null
+++ b/bl2/aarch64/bl2_run_next_image.S
@@ -0,0 +1,45 @@
+/*
+ * 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>
+
+	.globl	bl2_run_next_image
+
+
+func bl2_run_next_image
+	mov	x20,x0
+	/* ---------------------------------------------
+	 * MMU needs to be disabled because both BL2 and BL31 execute
+	 * in EL3, and therefore share the same address space.
+	 * BL31 will initialize the address space according to its
+	 * own requirement.
+	 * ---------------------------------------------
+	 */
+	bl	disable_mmu_icache_el3
+	tlbi	alle3
+	bl	bl2_el3_plat_prepare_exit
+
+#if ENABLE_PAUTH
+	/* ---------------------------------------------
+	 * Disable pointer authentication before jumping
+	 * to next boot image.
+	 * ---------------------------------------------
+	 */
+	bl	pauth_disable_el3
+#endif /* ENABLE_PAUTH */
+
+	ldp	x0, x1, [x20, #ENTRY_POINT_INFO_PC_OFFSET]
+	msr	elr_el3, x0
+	msr	spsr_el3, x1
+
+	ldp	x6, x7, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x30)]
+	ldp	x4, x5, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x20)]
+	ldp	x2, x3, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x10)]
+	ldp	x0, x1, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x0)]
+	exception_return
+endfunc bl2_run_next_image
diff --git a/bl2/bl2.ld.S b/bl2/bl2.ld.S
new file mode 100644
index 0000000..80cf7db
--- /dev/null
+++ b/bl2/bl2.ld.S
@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/bl_common.ld.h>
+#include <lib/xlat_tables/xlat_tables_defs.h>
+
+OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
+OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
+ENTRY(bl2_entrypoint)
+
+MEMORY {
+    RAM (rwx): ORIGIN = BL2_BASE, LENGTH = BL2_LIMIT - BL2_BASE
+}
+
+
+SECTIONS
+{
+    . = BL2_BASE;
+    ASSERT(. == ALIGN(PAGE_SIZE),
+           "BL2_BASE address is not aligned on a page boundary.")
+
+#if SEPARATE_CODE_AND_RODATA
+    .text . : {
+        __TEXT_START__ = .;
+#if ENABLE_RME
+        *bl2_rme_entrypoint.o(.text*)
+#else /* ENABLE_RME */
+        *bl2_entrypoint.o(.text*)
+#endif /* ENABLE_RME */
+        *(SORT_BY_ALIGNMENT(.text*))
+        *(.vectors)
+        . = ALIGN(PAGE_SIZE);
+        __TEXT_END__ = .;
+    } >RAM
+
+    /* .ARM.extab and .ARM.exidx are only added because Clang need them */
+    .ARM.extab . : {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } >RAM
+
+    .ARM.exidx . : {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } >RAM
+
+    .rodata . : {
+        __RODATA_START__ = .;
+        *(SORT_BY_ALIGNMENT(.rodata*))
+
+	RODATA_COMMON
+
+        . = ALIGN(PAGE_SIZE);
+        __RODATA_END__ = .;
+    } >RAM
+#else
+    ro . : {
+        __RO_START__ = .;
+        *bl2_entrypoint.o(.text*)
+        *(SORT_BY_ALIGNMENT(.text*))
+        *(SORT_BY_ALIGNMENT(.rodata*))
+
+	RODATA_COMMON
+
+        *(.vectors)
+        __RO_END_UNALIGNED__ = .;
+        /*
+         * Memory page(s) mapped to this section will be marked as
+         * read-only, executable.  No RW data from the next section must
+         * creep in.  Ensure the rest of the current memory page is unused.
+         */
+        . = ALIGN(PAGE_SIZE);
+        __RO_END__ = .;
+    } >RAM
+#endif
+
+    /*
+     * Define a linker symbol to mark start of the RW memory area for this
+     * image.
+     */
+    __RW_START__ = . ;
+
+    DATA_SECTION >RAM
+    STACK_SECTION >RAM
+    BSS_SECTION >RAM
+    XLAT_TABLE_SECTION >RAM
+
+#if USE_COHERENT_MEM
+    /*
+     * The base address of the coherent memory section must be page-aligned (4K)
+     * to guarantee that the coherent data are stored on their own pages and
+     * are not mixed with normal data.  This is required to set up the correct
+     * memory attributes for the coherent data page tables.
+     */
+    coherent_ram (NOLOAD) : ALIGN(PAGE_SIZE) {
+        __COHERENT_RAM_START__ = .;
+        *(tzfw_coherent_mem)
+        __COHERENT_RAM_END_UNALIGNED__ = .;
+        /*
+         * Memory page(s) mapped to this section will be marked
+         * as device memory.  No other unexpected data must creep in.
+         * Ensure the rest of the current memory page is unused.
+         */
+        . = ALIGN(PAGE_SIZE);
+        __COHERENT_RAM_END__ = .;
+    } >RAM
+#endif
+
+    /*
+     * Define a linker symbol to mark end of the RW memory area for this
+     * image.
+     */
+    __RW_END__ = .;
+    __BL2_END__ = .;
+
+    __BSS_SIZE__ = SIZEOF(.bss);
+
+#if USE_COHERENT_MEM
+    __COHERENT_RAM_UNALIGNED_SIZE__ =
+        __COHERENT_RAM_END_UNALIGNED__ - __COHERENT_RAM_START__;
+#endif
+
+    ASSERT(. <= BL2_LIMIT, "BL2 image has exceeded its limit.")
+}
diff --git a/bl2/bl2.mk b/bl2/bl2.mk
new file mode 100644
index 0000000..7a973e5
--- /dev/null
+++ b/bl2/bl2.mk
@@ -0,0 +1,50 @@
+#
+# Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+BL2_SOURCES		+=	bl2/bl2_image_load_v2.c			\
+				bl2/bl2_main.c				\
+				bl2/${ARCH}/bl2_arch_setup.c		\
+				lib/locks/exclusive/${ARCH}/spinlock.S	\
+				plat/common/${ARCH}/platform_up_stack.S	\
+				${MBEDTLS_SOURCES}
+
+ifeq (${ARCH},aarch64)
+BL2_SOURCES		+=	common/aarch64/early_exceptions.S
+endif
+
+ifeq (${ENABLE_RME},1)
+# Using RME, run BL2 at EL3
+include lib/gpt_rme/gpt_rme.mk
+
+BL2_SOURCES		+=      bl2/${ARCH}/bl2_rme_entrypoint.S	\
+				bl2/${ARCH}/bl2_el3_exceptions.S	\
+				bl2/${ARCH}/bl2_run_next_image.S	\
+				${GPT_LIB_SRCS}
+BL2_LINKERFILE		:=	bl2/bl2.ld.S
+
+else ifeq (${BL2_AT_EL3},0)
+# Normal operation, no RME, no BL2 at EL3
+BL2_SOURCES		+=	bl2/${ARCH}/bl2_entrypoint.S
+BL2_LINKERFILE		:=	bl2/bl2.ld.S
+
+else
+# BL2 at EL3, no RME
+BL2_SOURCES		+=	bl2/${ARCH}/bl2_el3_entrypoint.S	\
+				bl2/${ARCH}/bl2_el3_exceptions.S	\
+				bl2/${ARCH}/bl2_run_next_image.S        \
+				lib/cpus/${ARCH}/cpu_helpers.S		\
+				lib/cpus/errata_report.c
+
+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
+
+BL2_LINKERFILE		:=	bl2/bl2_el3.ld.S
+endif
diff --git a/bl2/bl2_el3.ld.S b/bl2/bl2_el3.ld.S
new file mode 100644
index 0000000..c95706c
--- /dev/null
+++ b/bl2/bl2_el3.ld.S
@@ -0,0 +1,187 @@
+/*
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/bl_common.ld.h>
+#include <lib/xlat_tables/xlat_tables_defs.h>
+
+OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
+OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
+ENTRY(bl2_entrypoint)
+
+MEMORY {
+#if BL2_IN_XIP_MEM
+    ROM (rx): ORIGIN = BL2_RO_BASE, LENGTH = BL2_RO_LIMIT - BL2_RO_BASE
+    RAM (rwx): ORIGIN = BL2_RW_BASE, LENGTH = BL2_RW_LIMIT - BL2_RW_BASE
+#else
+    RAM (rwx): ORIGIN = BL2_BASE, LENGTH = BL2_LIMIT - BL2_BASE
+#endif
+#if SEPARATE_BL2_NOLOAD_REGION
+    RAM_NOLOAD (rw!a): ORIGIN = BL2_NOLOAD_START, LENGTH = BL2_NOLOAD_LIMIT - BL2_NOLOAD_START
+#else
+#define RAM_NOLOAD RAM
+#endif
+}
+
+#if !BL2_IN_XIP_MEM
+#define ROM RAM
+#endif
+
+SECTIONS
+{
+#if BL2_IN_XIP_MEM
+    . = BL2_RO_BASE;
+    ASSERT(. == ALIGN(PAGE_SIZE),
+           "BL2_RO_BASE address is not aligned on a page boundary.")
+#else
+    . = BL2_BASE;
+    ASSERT(. == ALIGN(PAGE_SIZE),
+           "BL2_BASE address is not aligned on a page boundary.")
+#endif
+
+#if SEPARATE_CODE_AND_RODATA
+    .text . : {
+        __TEXT_START__ = .;
+	__TEXT_RESIDENT_START__ = .;
+	*bl2_el3_entrypoint.o(.text*)
+	*(.text.asm.*)
+	__TEXT_RESIDENT_END__ = .;
+        *(SORT_BY_ALIGNMENT(.text*))
+        *(.vectors)
+        . = ALIGN(PAGE_SIZE);
+        __TEXT_END__ = .;
+     } >ROM
+
+    .rodata . : {
+        __RODATA_START__ = .;
+        *(SORT_BY_ALIGNMENT(.rodata*))
+
+	RODATA_COMMON
+
+        . = ALIGN(PAGE_SIZE);
+        __RODATA_END__ = .;
+    } >ROM
+
+    ASSERT(__TEXT_RESIDENT_END__ - __TEXT_RESIDENT_START__ <= PAGE_SIZE,
+          "Resident part of BL2 has exceeded its limit.")
+#else
+    ro . : {
+        __RO_START__ = .;
+	__TEXT_RESIDENT_START__ = .;
+	*bl2_el3_entrypoint.o(.text*)
+	*(.text.asm.*)
+	__TEXT_RESIDENT_END__ = .;
+        *(SORT_BY_ALIGNMENT(.text*))
+        *(SORT_BY_ALIGNMENT(.rodata*))
+
+	RODATA_COMMON
+
+        *(.vectors)
+        __RO_END_UNALIGNED__ = .;
+        /*
+         * Memory page(s) mapped to this section will be marked as
+         * read-only, executable.  No RW data from the next section must
+         * creep in.  Ensure the rest of the current memory page is unused.
+         */
+        . = ALIGN(PAGE_SIZE);
+
+        __RO_END__ = .;
+    } >ROM
+#endif
+
+    ASSERT(__CPU_OPS_END__ > __CPU_OPS_START__,
+          "cpu_ops not defined for this platform.")
+
+#if BL2_IN_XIP_MEM
+    . = BL2_RW_BASE;
+    ASSERT(BL2_RW_BASE == ALIGN(PAGE_SIZE),
+           "BL2_RW_BASE address is not aligned on a page boundary.")
+#endif
+
+    /*
+     * Define a linker symbol to mark start of the RW memory area for this
+     * image.
+     */
+    __RW_START__ = . ;
+
+    DATA_SECTION >RAM AT>ROM
+    __DATA_RAM_START__ = __DATA_START__;
+    __DATA_RAM_END__ = __DATA_END__;
+
+    RELA_SECTION >RAM
+#if SEPARATE_BL2_NOLOAD_REGION
+    SAVED_ADDR = .;
+    . = BL2_NOLOAD_START;
+    __BL2_NOLOAD_START__ = .;
+#endif
+    STACK_SECTION >RAM_NOLOAD
+    BSS_SECTION >RAM_NOLOAD
+    XLAT_TABLE_SECTION >RAM_NOLOAD
+#if SEPARATE_BL2_NOLOAD_REGION
+    __BL2_NOLOAD_END__ = .;
+    . = SAVED_ADDR;
+#endif
+
+#if USE_COHERENT_MEM
+    /*
+     * The base address of the coherent memory section must be page-aligned (4K)
+     * to guarantee that the coherent data are stored on their own pages and
+     * are not mixed with normal data.  This is required to set up the correct
+     * memory attributes for the coherent data page tables.
+     */
+    coherent_ram (NOLOAD) : ALIGN(PAGE_SIZE) {
+        __COHERENT_RAM_START__ = .;
+        *(tzfw_coherent_mem)
+        __COHERENT_RAM_END_UNALIGNED__ = .;
+        /*
+         * Memory page(s) mapped to this section will be marked
+         * as device memory.  No other unexpected data must creep in.
+         * Ensure the rest of the current memory page is unused.
+         */
+        . = ALIGN(PAGE_SIZE);
+        __COHERENT_RAM_END__ = .;
+    } >RAM
+#endif
+
+    /*
+     * Define a linker symbol to mark end of the RW memory area for this
+     * image.
+     */
+    __RW_END__ = .;
+    __BL2_END__ = .;
+
+    /DISCARD/ : {
+        *(.dynsym .dynstr .hash .gnu.hash)
+    }
+
+#if BL2_IN_XIP_MEM
+    __BL2_RAM_START__ = ADDR(.data);
+    __BL2_RAM_END__ = .;
+
+    __DATA_ROM_START__ = LOADADDR(.data);
+    __DATA_SIZE__ = SIZEOF(.data);
+
+    /*
+     * The .data section is the last PROGBITS section so its end marks the end
+     * of BL2's RO content in XIP memory..
+     */
+    __BL2_ROM_END__ =  __DATA_ROM_START__ + __DATA_SIZE__;
+    ASSERT(__BL2_ROM_END__ <= BL2_RO_LIMIT,
+           "BL2's RO content has exceeded its limit.")
+#endif
+    __BSS_SIZE__ = SIZEOF(.bss);
+
+
+#if USE_COHERENT_MEM
+    __COHERENT_RAM_UNALIGNED_SIZE__ =
+        __COHERENT_RAM_END_UNALIGNED__ - __COHERENT_RAM_START__;
+#endif
+
+#if BL2_IN_XIP_MEM
+    ASSERT(. <= BL2_RW_LIMIT, "BL2's RW content has exceeded its limit.")
+#else
+    ASSERT(. <= BL2_LIMIT, "BL2 image has exceeded its limit.")
+#endif
+}
diff --git a/bl2/bl2_image_load_v2.c b/bl2/bl2_image_load_v2.c
new file mode 100644
index 0000000..dee3fc2
--- /dev/null
+++ b/bl2/bl2_image_load_v2.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include "bl2_private.h"
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <common/desc_image_load.h>
+#include <drivers/auth/auth_mod.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * This function loads SCP_BL2/BL3x images and returns the ep_info for
+ * the next executable image.
+ ******************************************************************************/
+struct entry_point_info *bl2_load_images(void)
+{
+	bl_params_t *bl2_to_next_bl_params;
+	bl_load_info_t *bl2_load_info;
+	const bl_load_info_node_t *bl2_node_info;
+	int plat_setup_done = 0;
+	int err;
+
+	/*
+	 * Get information about the images to load.
+	 */
+	bl2_load_info = plat_get_bl_image_load_info();
+	assert(bl2_load_info != NULL);
+	assert(bl2_load_info->head != NULL);
+	assert(bl2_load_info->h.type == PARAM_BL_LOAD_INFO);
+	assert(bl2_load_info->h.version >= VERSION_2);
+	bl2_node_info = bl2_load_info->head;
+
+	while (bl2_node_info != NULL) {
+		/*
+		 * Perform platform setup before loading the image,
+		 * if indicated in the image attributes AND if NOT
+		 * already done before.
+		 */
+		if ((bl2_node_info->image_info->h.attr &
+		    IMAGE_ATTRIB_PLAT_SETUP) != 0U) {
+			if (plat_setup_done != 0) {
+				WARN("BL2: Platform setup already done!!\n");
+			} else {
+				INFO("BL2: Doing platform setup\n");
+				bl2_platform_setup();
+				plat_setup_done = 1;
+			}
+		}
+
+		err = bl2_plat_handle_pre_image_load(bl2_node_info->image_id);
+		if (err != 0) {
+			ERROR("BL2: Failure in pre image load handling (%i)\n", err);
+			plat_error_handler(err);
+		}
+
+		if ((bl2_node_info->image_info->h.attr &
+		    IMAGE_ATTRIB_SKIP_LOADING) == 0U) {
+			INFO("BL2: Loading image id %u\n", bl2_node_info->image_id);
+			err = load_auth_image(bl2_node_info->image_id,
+				bl2_node_info->image_info);
+			if (err != 0) {
+				ERROR("BL2: Failed to load image id %u (%i)\n",
+				      bl2_node_info->image_id, err);
+				plat_error_handler(err);
+			}
+		} else {
+			INFO("BL2: Skip loading image id %u\n", bl2_node_info->image_id);
+		}
+
+		/* Allow platform to handle image information. */
+		err = bl2_plat_handle_post_image_load(bl2_node_info->image_id);
+		if (err != 0) {
+			ERROR("BL2: Failure in post image load handling (%i)\n", err);
+			plat_error_handler(err);
+		}
+
+		/* Go to next image */
+		bl2_node_info = bl2_node_info->next_load_info;
+	}
+
+	/*
+	 * Get information to pass to the next image.
+	 */
+	bl2_to_next_bl_params = plat_get_next_bl_params();
+	assert(bl2_to_next_bl_params != NULL);
+	assert(bl2_to_next_bl_params->head != NULL);
+	assert(bl2_to_next_bl_params->h.type == PARAM_BL_PARAMS);
+	assert(bl2_to_next_bl_params->h.version >= VERSION_2);
+	assert(bl2_to_next_bl_params->head->ep_info != NULL);
+
+	/* Populate arg0 for the next BL image if not already provided */
+	if (bl2_to_next_bl_params->head->ep_info->args.arg0 == (u_register_t)0)
+		bl2_to_next_bl_params->head->ep_info->args.arg0 =
+					(u_register_t)bl2_to_next_bl_params;
+
+	/* Flush the parameters to be passed to next image */
+	plat_flush_next_bl_params();
+
+	return bl2_to_next_bl_params->head->ep_info;
+}
diff --git a/bl2/bl2_main.c b/bl2/bl2_main.c
new file mode 100644
index 0000000..5da8037
--- /dev/null
+++ b/bl2/bl2_main.c
@@ -0,0 +1,149 @@
+/*
+ * Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <arch_features.h>
+#include <bl1/bl1.h>
+#include <bl2/bl2.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/auth/auth_mod.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/console.h>
+#include <drivers/fwu/fwu.h>
+#include <lib/extensions/pauth.h>
+#include <plat/common/platform.h>
+
+#include "bl2_private.h"
+
+#ifdef __aarch64__
+#define NEXT_IMAGE	"BL31"
+#else
+#define NEXT_IMAGE	"BL32"
+#endif
+
+#if BL2_AT_EL3
+/*******************************************************************************
+ * Setup function for BL2 when BL2_AT_EL3=1
+ ******************************************************************************/
+void bl2_el3_setup(u_register_t arg0, u_register_t arg1, u_register_t arg2,
+		   u_register_t arg3)
+{
+	/* Perform early platform-specific setup */
+	bl2_el3_early_platform_setup(arg0, arg1, arg2, arg3);
+
+	/* Perform late platform-specific setup */
+	bl2_el3_plat_arch_setup();
+
+#if CTX_INCLUDE_PAUTH_REGS
+	/*
+	 * Assert that the ARMv8.3-PAuth registers are present or an access
+	 * fault will be triggered when they are being saved or restored.
+	 */
+	assert(is_armv8_3_pauth_present());
+#endif /* CTX_INCLUDE_PAUTH_REGS */
+}
+#else /* BL2_AT_EL3 */
+/*******************************************************************************
+ * Setup function for BL2 when BL2_AT_EL3=0
+ ******************************************************************************/
+void bl2_setup(u_register_t arg0, u_register_t arg1, u_register_t arg2,
+	       u_register_t arg3)
+{
+	/* Perform early platform-specific setup */
+	bl2_early_platform_setup2(arg0, arg1, arg2, arg3);
+
+	/* Perform late platform-specific setup */
+	bl2_plat_arch_setup();
+
+#if CTX_INCLUDE_PAUTH_REGS
+	/*
+	 * Assert that the ARMv8.3-PAuth registers are present or an access
+	 * fault will be triggered when they are being saved or restored.
+	 */
+	assert(is_armv8_3_pauth_present());
+#endif /* CTX_INCLUDE_PAUTH_REGS */
+}
+#endif /* BL2_AT_EL3 */
+
+/*******************************************************************************
+ * The only thing to do in BL2 is to load further images and pass control to
+ * next BL. The memory occupied by BL2 will be reclaimed by BL3x stages. BL2
+ * runs entirely in S-EL1.
+ ******************************************************************************/
+void bl2_main(void)
+{
+	entry_point_info_t *next_bl_ep_info;
+
+	NOTICE("BL2: %s\n", version_string);
+	NOTICE("BL2: %s\n", build_message);
+
+	/* Perform remaining generic architectural setup in S-EL1 */
+	bl2_arch_setup();
+
+#if PSA_FWU_SUPPORT
+	fwu_init();
+#endif /* PSA_FWU_SUPPORT */
+
+	crypto_mod_init();
+
+	/* Initialize authentication module */
+	auth_mod_init();
+
+	/* Initialize the Measured Boot backend */
+	bl2_plat_mboot_init();
+
+	/* Initialize boot source */
+	bl2_plat_preload_setup();
+
+	/* Load the subsequent bootloader images. */
+	next_bl_ep_info = bl2_load_images();
+
+	/* Teardown the Measured Boot backend */
+	bl2_plat_mboot_finish();
+
+#if !BL2_AT_EL3 && !ENABLE_RME
+#ifndef __aarch64__
+	/*
+	 * For AArch32 state BL1 and BL2 share the MMU setup.
+	 * Given that BL2 does not map BL1 regions, MMU needs
+	 * to be disabled in order to go back to BL1.
+	 */
+	disable_mmu_icache_secure();
+#endif /* !__aarch64__ */
+
+	console_flush();
+
+#if ENABLE_PAUTH
+	/*
+	 * Disable pointer authentication before running next boot image
+	 */
+	pauth_disable_el1();
+#endif /* ENABLE_PAUTH */
+
+	/*
+	 * Run next BL image via an SMC to BL1. Information on how to pass
+	 * control to the BL32 (if present) and BL33 software images will
+	 * be passed to next BL image as an argument.
+	 */
+	smc(BL1_SMC_RUN_IMAGE, (unsigned long)next_bl_ep_info, 0, 0, 0, 0, 0, 0);
+#else /* if BL2_AT_EL3 || ENABLE_RME */
+	NOTICE("BL2: Booting " NEXT_IMAGE "\n");
+	print_entry_point_info(next_bl_ep_info);
+	console_flush();
+
+#if ENABLE_PAUTH
+	/*
+	 * Disable pointer authentication before running next boot image
+	 */
+	pauth_disable_el3();
+#endif /* ENABLE_PAUTH */
+
+	bl2_run_next_image(next_bl_ep_info);
+#endif /* BL2_AT_EL3 && ENABLE_RME */
+}
diff --git a/bl2/bl2_private.h b/bl2/bl2_private.h
new file mode 100644
index 0000000..b1704d2
--- /dev/null
+++ b/bl2/bl2_private.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2013-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BL2_PRIVATE_H
+#define BL2_PRIVATE_H
+
+#include <common/bl_common.h>
+
+/******************************************
+ * Forward declarations
+ *****************************************/
+struct entry_point_info;
+
+/******************************************
+ * Function prototypes
+ *****************************************/
+void bl2_arch_setup(void);
+struct entry_point_info *bl2_load_images(void);
+void bl2_run_next_image(const struct entry_point_info *bl_ep_info);
+
+#endif /* BL2_PRIVATE_H */
diff --git a/bl2u/aarch32/bl2u_entrypoint.S b/bl2u/aarch32/bl2u_entrypoint.S
new file mode 100644
index 0000000..1063789
--- /dev/null
+++ b/bl2u/aarch32/bl2u_entrypoint.S
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+
+	.globl	bl2u_vector_table
+	.globl	bl2u_entrypoint
+
+
+vector_base bl2u_vector_table
+	b	bl2u_entrypoint
+	b	report_exception	/* Undef */
+	b	report_exception	/* SVC call */
+	b	report_prefetch_abort	/* Prefetch abort */
+	b	report_data_abort	/* Data abort */
+	b	report_exception	/* Reserved */
+	b	report_exception	/* IRQ */
+	b	report_exception	/* FIQ */
+
+
+func bl2u_entrypoint
+	/*---------------------------------------------
+	 * Save from r1 the extents of the trusted ram
+	 * available to BL2U for future use.
+	 * r0 is not currently used.
+	 * ---------------------------------------------
+	 */
+	mov	r11, r1
+	mov	r10, r2
+
+	/* ---------------------------------------------
+	 * Set the exception vector to something sane.
+	 * ---------------------------------------------
+	 */
+	ldr	r0, =bl2u_vector_table
+	stcopr	r0, VBAR
+	isb
+
+	/* --------------------------------------------------------
+	 * Enable the instruction cache - disable speculative loads
+	 * --------------------------------------------------------
+	 */
+	ldcopr	r0, SCTLR
+	orr	r0, r0, #SCTLR_I_BIT
+	bic	r0, r0, #SCTLR_DSSBS_BIT
+	stcopr	r0, SCTLR
+	isb
+
+	/* ---------------------------------------------
+	 * Since BL2U executes after BL1, it is assumed
+	 * here that BL1 has already has done the
+	 * necessary register initializations.
+	 * ---------------------------------------------
+	 */
+
+	/* ---------------------------------------------
+	 * Invalidate the RW memory used by the BL2U
+	 * image. This includes the data and NOBITS
+	 * sections. This is done to safeguard against
+	 * possible corruption of this memory by dirty
+	 * cache lines in a system cache as a result of
+	 * use by an earlier boot loader stage.
+	 * ---------------------------------------------
+	 */
+	ldr	r0, =__RW_START__
+	ldr	r1, =__RW_END__
+	sub	r1, r1, r0
+	bl	inv_dcache_range
+
+	/* ---------------------------------------------
+	 * Zero out NOBITS sections. There are 2 of them:
+	 *   - the .bss section;
+	 *   - the coherent memory section.
+	 * ---------------------------------------------
+	 */
+	ldr	r0, =__BSS_START__
+	ldr	r1, =__BSS_END__
+	sub 	r1, r1, r0
+	bl	zeromem
+
+	/* --------------------------------------------
+	 * Allocate a stack whose memory will be marked
+	 * as Normal-IS-WBWA when the MMU is enabled.
+	 * There is no risk of reading stale stack
+	 * memory after enabling the MMU as only the
+	 * primary cpu is running at the moment.
+	 * --------------------------------------------
+	 */
+	bl	plat_set_my_stack
+
+	/* ---------------------------------------------
+	 * Initialize the stack protector canary before
+	 * any C code is called.
+	 * ---------------------------------------------
+	 */
+#if STACK_PROTECTOR_ENABLED
+	bl	update_stack_protector_canary
+#endif
+
+	/* ---------------------------------------------
+	 * Perform early platform setup & platform
+	 * specific early arch. setup e.g. mmu setup
+	 * ---------------------------------------------
+	 */
+	mov	r0, r11
+	mov	r1, r10
+	bl	bl2u_early_platform_setup
+	bl	bl2u_plat_arch_setup
+
+	/* ---------------------------------------------
+	 * Jump to main function.
+	 * ---------------------------------------------
+	 */
+	bl	bl2u_main
+
+	/* ---------------------------------------------
+	 * Should never reach this point.
+	 * ---------------------------------------------
+	 */
+	no_ret	plat_panic_handler
+
+endfunc bl2u_entrypoint
diff --git a/bl2u/aarch64/bl2u_entrypoint.S b/bl2u/aarch64/bl2u_entrypoint.S
new file mode 100644
index 0000000..15978b6
--- /dev/null
+++ b/bl2u/aarch64/bl2u_entrypoint.S
@@ -0,0 +1,129 @@
+/*
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+
+	.globl	bl2u_entrypoint
+
+
+func bl2u_entrypoint
+	/*---------------------------------------------
+	 * Store the extents of the tzram available to
+	 * BL2U and other platform specific information
+	 * for future use. x0 is currently not used.
+	 * ---------------------------------------------
+	 */
+	mov	x20, x1
+	mov	x21, x2
+
+	/* ---------------------------------------------
+	 * Set the exception vector to something sane.
+	 * ---------------------------------------------
+	 */
+	adr	x0, early_exceptions
+	msr	vbar_el1, x0
+	isb
+
+	/* ---------------------------------------------
+	 * Enable the SError interrupt now that the
+	 * exception vectors have been setup.
+	 * ---------------------------------------------
+	 */
+	msr	daifclr, #DAIF_ABT_BIT
+
+	/* ---------------------------------------------
+	 * Enable the instruction cache, stack pointer
+	 * and data access alignment checks and disable
+	 * speculative loads.
+	 * ---------------------------------------------
+	 */
+	mov	x1, #(SCTLR_I_BIT | SCTLR_A_BIT | SCTLR_SA_BIT)
+	mrs	x0, sctlr_el1
+	orr	x0, x0, x1
+	bic	x0, x0, #SCTLR_DSSBS_BIT
+	msr	sctlr_el1, x0
+	isb
+
+	/* ---------------------------------------------
+	 * Invalidate the RW memory used by the BL2U
+	 * image. This includes the data and NOBITS
+	 * sections. This is done to safeguard against
+	 * possible corruption of this memory by dirty
+	 * cache lines in a system cache as a result of
+	 * use by an earlier boot loader stage.
+	 * ---------------------------------------------
+	 */
+	adr	x0, __RW_START__
+	adr	x1, __RW_END__
+	sub	x1, x1, x0
+	bl	inv_dcache_range
+
+	/* ---------------------------------------------
+	 * Zero out NOBITS sections. There are 2 of them:
+	 *   - the .bss section;
+	 *   - the coherent memory section.
+	 * ---------------------------------------------
+	 */
+	adrp	x0, __BSS_START__
+	add	x0, x0, :lo12:__BSS_START__
+	adrp	x1, __BSS_END__
+	add	x1, x1, :lo12:__BSS_END__
+	sub	x1, x1, x0
+	bl	zeromem
+
+	/* --------------------------------------------
+	 * Allocate a stack whose memory will be marked
+	 * as Normal-IS-WBWA when the MMU is enabled.
+	 * There is no risk of reading stale stack
+	 * memory after enabling the MMU as only the
+	 * primary cpu is running at the moment.
+	 * --------------------------------------------
+	 */
+	bl	plat_set_my_stack
+
+	/* ---------------------------------------------
+	 * Initialize the stack protector canary before
+	 * any C code is called.
+	 * ---------------------------------------------
+	 */
+#if STACK_PROTECTOR_ENABLED
+	bl	update_stack_protector_canary
+#endif
+
+	/* ---------------------------------------------
+	 * Perform early platform setup & platform
+	 * specific early arch. setup e.g. mmu setup
+	 * ---------------------------------------------
+	 */
+	mov	x0, x20
+	mov	x1, x21
+	bl	bl2u_early_platform_setup
+	bl	bl2u_plat_arch_setup
+
+#if ENABLE_PAUTH
+	/* ---------------------------------------------
+	 * Program APIAKey_EL1
+	 * and enable pointer authentication.
+	 * ---------------------------------------------
+	 */
+	bl	pauth_init_enable_el1
+#endif
+
+	/* ---------------------------------------------
+	 * Jump to bl2u_main function.
+	 * ---------------------------------------------
+	 */
+	bl	bl2u_main
+
+	/* ---------------------------------------------
+	 * Should never reach this point.
+	 * ---------------------------------------------
+	 */
+	no_ret	plat_panic_handler
+
+endfunc bl2u_entrypoint
diff --git a/bl2u/bl2u.ld.S b/bl2u/bl2u.ld.S
new file mode 100644
index 0000000..a7752a4
--- /dev/null
+++ b/bl2u/bl2u.ld.S
@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <common/bl_common.ld.h>
+#include <lib/xlat_tables/xlat_tables_defs.h>
+
+OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
+OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
+ENTRY(bl2u_entrypoint)
+
+MEMORY {
+    RAM (rwx): ORIGIN = BL2U_BASE, LENGTH = BL2U_LIMIT - BL2U_BASE
+}
+
+
+SECTIONS
+{
+    . = BL2U_BASE;
+    ASSERT(. == ALIGN(PAGE_SIZE),
+           "BL2U_BASE address is not aligned on a page boundary.")
+
+#if SEPARATE_CODE_AND_RODATA
+    .text . : {
+        __TEXT_START__ = .;
+        *bl2u_entrypoint.o(.text*)
+        *(SORT_BY_ALIGNMENT(.text*))
+        *(.vectors)
+        . = ALIGN(PAGE_SIZE);
+        __TEXT_END__ = .;
+     } >RAM
+
+     /* .ARM.extab and .ARM.exidx are only added because Clang need them */
+     .ARM.extab . : {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+     } >RAM
+
+     .ARM.exidx . : {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+     } >RAM
+
+    .rodata . : {
+        __RODATA_START__ = .;
+        *(SORT_BY_ALIGNMENT(.rodata*))
+
+	RODATA_COMMON
+
+        . = ALIGN(PAGE_SIZE);
+        __RODATA_END__ = .;
+    } >RAM
+#else
+    ro . : {
+        __RO_START__ = .;
+        *bl2u_entrypoint.o(.text*)
+        *(SORT_BY_ALIGNMENT(.text*))
+        *(SORT_BY_ALIGNMENT(.rodata*))
+
+	RODATA_COMMON
+
+        *(.vectors)
+        __RO_END_UNALIGNED__ = .;
+        /*
+         * Memory page(s) mapped to this section will be marked as
+         * read-only, executable.  No RW data from the next section must
+         * creep in.  Ensure the rest of the current memory page is unused.
+         */
+        . = ALIGN(PAGE_SIZE);
+        __RO_END__ = .;
+    } >RAM
+#endif
+
+    /*
+     * Define a linker symbol to mark start of the RW memory area for this
+     * image.
+     */
+    __RW_START__ = . ;
+
+    DATA_SECTION >RAM
+    STACK_SECTION >RAM
+    BSS_SECTION >RAM
+    XLAT_TABLE_SECTION >RAM
+
+#if USE_COHERENT_MEM
+    /*
+     * The base address of the coherent memory section must be page-aligned (4K)
+     * to guarantee that the coherent data are stored on their own pages and
+     * are not mixed with normal data.  This is required to set up the correct
+     * memory attributes for the coherent data page tables.
+     */
+    coherent_ram (NOLOAD) : ALIGN(PAGE_SIZE) {
+        __COHERENT_RAM_START__ = .;
+        *(tzfw_coherent_mem)
+        __COHERENT_RAM_END_UNALIGNED__ = .;
+        /*
+         * Memory page(s) mapped to this section will be marked
+         * as device memory.  No other unexpected data must creep in.
+         * Ensure the rest of the current memory page is unused.
+         */
+        . = ALIGN(PAGE_SIZE);
+        __COHERENT_RAM_END__ = .;
+    } >RAM
+#endif
+
+    /*
+     * Define a linker symbol to mark end of the RW memory area for this
+     * image.
+     */
+    __RW_END__ = .;
+    __BL2U_END__ = .;
+
+    __BSS_SIZE__ = SIZEOF(.bss);
+
+    ASSERT(. <= BL2U_LIMIT, "BL2U image has exceeded its limit.")
+}
diff --git a/bl2u/bl2u.mk b/bl2u/bl2u.mk
new file mode 100644
index 0000000..b4d7634
--- /dev/null
+++ b/bl2u/bl2u.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+BL2U_SOURCES		+=	bl2u/bl2u_main.c			\
+				bl2u/${ARCH}/bl2u_entrypoint.S		\
+				plat/common/${ARCH}/platform_up_stack.S
+
+ifeq (${ARCH},aarch64)
+BL2U_SOURCES		+=	common/aarch64/early_exceptions.S
+endif
+
+BL2U_LINKERFILE		:=	bl2u/bl2u.ld.S
diff --git a/bl2u/bl2u_main.c b/bl2u/bl2u_main.c
new file mode 100644
index 0000000..fcb73b9
--- /dev/null
+++ b/bl2u/bl2u_main.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <bl1/bl1.h>
+#include <bl2u/bl2u.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/auth/auth_mod.h>
+#include <drivers/console.h>
+#include <plat/common/platform.h>
+
+/*******************************************************************************
+ * This function is responsible to:
+ * Load SCP_BL2U if platform has defined SCP_BL2U_BASE
+ * Perform platform setup.
+ * Go back to EL3.
+ ******************************************************************************/
+void bl2u_main(void)
+{
+	NOTICE("BL2U: %s\n", version_string);
+	NOTICE("BL2U: %s\n", build_message);
+
+#if SCP_BL2U_BASE
+	int rc;
+	/* Load the subsequent bootloader images */
+	rc = bl2u_plat_handle_scp_bl2u();
+	if (rc != 0) {
+		ERROR("Failed to load SCP_BL2U (%i)\n", rc);
+		panic();
+	}
+#endif
+
+	/* Perform platform setup in BL2U after loading SCP_BL2U */
+	bl2u_platform_setup();
+
+	console_flush();
+
+#ifndef __aarch64__
+	/*
+	 * For AArch32 state BL1 and BL2U share the MMU setup.
+	 * Given that BL2U does not map BL1 regions, MMU needs
+	 * to be disabled in order to go back to BL1.
+	 */
+	disable_mmu_icache_secure();
+#endif /* !__aarch64__ */
+
+	/*
+	 * Indicate that BL2U is done and resume back to
+	 * normal world via an SMC to BL1.
+	 * x1 could be passed to Normal world,
+	 * so DO NOT pass any secret information.
+	 */
+	smc(FWU_SMC_SEC_IMAGE_DONE, 0, 0, 0, 0, 0, 0, 0);
+	wfi();
+}