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

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

14 files changed, 2348 insertions, 0 deletions

diff --git a/bl1/aarch32/bl1_arch_setup.c b/bl1/aarch32/bl1_arch_setup.c
new file mode 100644
index 0000000..ce04aaa
--- /dev/null
+++ b/bl1/aarch32/bl1_arch_setup.c
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "../bl1_private.h"
+
+/*******************************************************************************
+ * TODO: Function that does the first bit of architectural setup.
+ ******************************************************************************/
+void bl1_arch_setup(void)
+{
+
+}
diff --git a/bl1/aarch32/bl1_context_mgmt.c b/bl1/aarch32/bl1_context_mgmt.c
new file mode 100644
index 0000000..85d35a7
--- /dev/null
+++ b/bl1/aarch32/bl1_context_mgmt.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <context.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <plat/common/platform.h>
+#include <smccc_helpers.h>
+
+#include "../bl1_private.h"
+
+/*
+ * Following arrays will be used for context management.
+ * There are 2 instances, for the Secure and Non-Secure contexts.
+ */
+static cpu_context_t bl1_cpu_context[2];
+static smc_ctx_t bl1_smc_context[2];
+
+/* Following contains the next cpu context pointer. */
+static void *bl1_next_cpu_context_ptr;
+
+/* Following contains the next smc context pointer. */
+static void *bl1_next_smc_context_ptr;
+
+/* Following functions are used for SMC context handling */
+void *smc_get_ctx(unsigned int security_state)
+{
+	assert(sec_state_is_valid(security_state));
+	return &bl1_smc_context[security_state];
+}
+
+void smc_set_next_ctx(unsigned int security_state)
+{
+	assert(sec_state_is_valid(security_state));
+	bl1_next_smc_context_ptr = &bl1_smc_context[security_state];
+}
+
+void *smc_get_next_ctx(void)
+{
+	return bl1_next_smc_context_ptr;
+}
+
+/* Following functions are used for CPU context handling */
+void *cm_get_context(uint32_t security_state)
+{
+	assert(sec_state_is_valid(security_state));
+	return &bl1_cpu_context[security_state];
+}
+
+void cm_set_next_context(void *context)
+{
+	assert(context != NULL);
+	bl1_next_cpu_context_ptr = context;
+}
+
+void *cm_get_next_context(void)
+{
+	return bl1_next_cpu_context_ptr;
+}
+
+/*******************************************************************************
+ * Following function copies GP regs r0-r4, lr and spsr,
+ * from the CPU context to the SMC context structures.
+ ******************************************************************************/
+static void copy_cpu_ctx_to_smc_ctx(const regs_t *cpu_reg_ctx,
+		smc_ctx_t *next_smc_ctx)
+{
+	next_smc_ctx->r0 = read_ctx_reg(cpu_reg_ctx, CTX_GPREG_R0);
+	next_smc_ctx->r1 = read_ctx_reg(cpu_reg_ctx, CTX_GPREG_R1);
+	next_smc_ctx->r2 = read_ctx_reg(cpu_reg_ctx, CTX_GPREG_R2);
+	next_smc_ctx->r3 = read_ctx_reg(cpu_reg_ctx, CTX_GPREG_R3);
+	next_smc_ctx->lr_mon = read_ctx_reg(cpu_reg_ctx, CTX_LR);
+	next_smc_ctx->spsr_mon = read_ctx_reg(cpu_reg_ctx, CTX_SPSR);
+	next_smc_ctx->scr = read_ctx_reg(cpu_reg_ctx, CTX_SCR);
+}
+
+/*******************************************************************************
+ * Following function flushes the SMC & CPU context pointer and its data.
+ ******************************************************************************/
+static void flush_smc_and_cpu_ctx(void)
+{
+	flush_dcache_range((uintptr_t)&bl1_next_smc_context_ptr,
+		sizeof(bl1_next_smc_context_ptr));
+	flush_dcache_range((uintptr_t)bl1_next_smc_context_ptr,
+		sizeof(smc_ctx_t));
+
+	flush_dcache_range((uintptr_t)&bl1_next_cpu_context_ptr,
+		sizeof(bl1_next_cpu_context_ptr));
+	flush_dcache_range((uintptr_t)bl1_next_cpu_context_ptr,
+		sizeof(cpu_context_t));
+}
+
+/*******************************************************************************
+ * This function prepares the context for Secure/Normal world images.
+ * Normal world images are transitioned to HYP(if supported) else SVC.
+ ******************************************************************************/
+void bl1_prepare_next_image(unsigned int image_id)
+{
+	unsigned int security_state, mode = MODE32_svc;
+	image_desc_t *desc;
+	entry_point_info_t *next_bl_ep;
+
+	/* Get the image descriptor. */
+	desc = bl1_plat_get_image_desc(image_id);
+	assert(desc != NULL);
+
+	/* Get the entry point info. */
+	next_bl_ep = &desc->ep_info;
+
+	/* Get the image security state. */
+	security_state = GET_SECURITY_STATE(next_bl_ep->h.attr);
+
+	/* Prepare the SPSR for the next BL image. */
+	if ((security_state != SECURE) && (GET_VIRT_EXT(read_id_pfr1()) != 0U)) {
+		mode = MODE32_hyp;
+	}
+
+	next_bl_ep->spsr = SPSR_MODE32(mode, SPSR_T_ARM,
+				SPSR_E_LITTLE, DISABLE_ALL_EXCEPTIONS);
+
+	/* Allow platform to make change */
+	bl1_plat_set_ep_info(image_id, next_bl_ep);
+
+	/* Prepare the cpu context for the next BL image. */
+	cm_init_my_context(next_bl_ep);
+	cm_prepare_el3_exit(security_state);
+	cm_set_next_context(cm_get_context(security_state));
+
+	/* Prepare the smc context for the next BL image. */
+	smc_set_next_ctx(security_state);
+	copy_cpu_ctx_to_smc_ctx(get_regs_ctx(cm_get_next_context()),
+		smc_get_next_ctx());
+
+	/*
+	 * If the next image is non-secure, then we need to program the banked
+	 * non secure sctlr. This is not required when the next image is secure
+	 * because in AArch32, we expect the secure world to have the same
+	 * SCTLR settings.
+	 */
+	if (security_state == NON_SECURE) {
+		cpu_context_t *ctx = cm_get_context(security_state);
+		u_register_t ns_sctlr;
+
+		/* Temporarily set the NS bit to access NS SCTLR */
+		write_scr(read_scr() | SCR_NS_BIT);
+		isb();
+
+		ns_sctlr = read_ctx_reg(get_regs_ctx(ctx), CTX_NS_SCTLR);
+		write_sctlr(ns_sctlr);
+		isb();
+
+		write_scr(read_scr() & ~SCR_NS_BIT);
+		isb();
+	}
+
+	/*
+	 * Flush the SMC & CPU context and the (next)pointers,
+	 * to access them after caches are disabled.
+	 */
+	flush_smc_and_cpu_ctx();
+
+	/* Indicate that image is in execution state. */
+	desc->state = IMAGE_STATE_EXECUTED;
+
+	print_entry_point_info(next_bl_ep);
+}
diff --git a/bl1/aarch32/bl1_entrypoint.S b/bl1/aarch32/bl1_entrypoint.S
new file mode 100644
index 0000000..b22015e
--- /dev/null
+++ b/bl1/aarch32/bl1_entrypoint.S
@@ -0,0 +1,99 @@
+/*
+ * 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>
+#include <context.h>
+#include <el3_common_macros.S>
+#include <smccc_helpers.h>
+#include <smccc_macros.S>
+
+	.globl	bl1_vector_table
+	.globl	bl1_entrypoint
+
+	/* -----------------------------------------------------
+	 * Setup the vector table to support SVC & MON mode.
+	 * -----------------------------------------------------
+	 */
+vector_base bl1_vector_table
+	b	bl1_entrypoint
+	b	report_exception	/* Undef */
+	b	bl1_aarch32_smc_handler	/* SMC 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 */
+
+	/* -----------------------------------------------------
+	 * bl1_entrypoint() is the entry point into the trusted
+	 * firmware code when a cpu is released from warm or
+	 * cold reset.
+	 * -----------------------------------------------------
+	 */
+
+func bl1_entrypoint
+/* ---------------------------------------------------------------------
+* If the reset address is programmable then bl1_entrypoint() is
+* executed only on the cold boot path. Therefore, we can skip the warm
+* boot mailbox mechanism.
+* ---------------------------------------------------------------------
+*/
+	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=bl1_vector_table		\
+		_pie_fixup_size=0
+
+	/* -----------------------------------------------------
+	 * Perform BL1 setup
+	 * -----------------------------------------------------
+	 */
+	bl	bl1_setup
+
+	/* -----------------------------------------------------
+	 * Jump to main function.
+	 * -----------------------------------------------------
+	 */
+	bl	bl1_main
+
+	/* -----------------------------------------------------
+	 * Jump to next image.
+	 * -----------------------------------------------------
+	 */
+
+	/*
+	 * Get the smc_context for next BL image,
+	 * program the gp/system registers and save it in `r4`.
+	 */
+	bl	smc_get_next_ctx
+	mov	r4, r0
+
+	/* Only turn-off MMU if going to secure world */
+	ldr	r5, [r4, #SMC_CTX_SCR]
+	tst	r5, #SCR_NS_BIT
+	bne	skip_mmu_off
+
+	/*
+	 * MMU needs to be disabled because both BL1 and BL2/BL2U execute
+	 * in PL1, and therefore share the same address space.
+	 * BL2/BL2U will initialize the address space according to its
+	 * own requirement.
+	 */
+	bl	disable_mmu_icache_secure
+	stcopr	r0, TLBIALL
+	dsb	sy
+	isb
+
+skip_mmu_off:
+	/* Restore smc_context from `r4` and exit secure monitor mode. */
+	mov	r0, r4
+	monitor_exit
+endfunc bl1_entrypoint
diff --git a/bl1/aarch32/bl1_exceptions.S b/bl1/aarch32/bl1_exceptions.S
new file mode 100644
index 0000000..4a6815f
--- /dev/null
+++ b/bl1/aarch32/bl1_exceptions.S
@@ -0,0 +1,165 @@
+/*
+ * 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 <bl1/bl1.h>
+#include <common/bl_common.h>
+#include <context.h>
+#include <lib/xlat_tables/xlat_tables.h>
+#include <smccc_helpers.h>
+#include <smccc_macros.S>
+
+	.globl	bl1_aarch32_smc_handler
+
+
+func bl1_aarch32_smc_handler
+	/* On SMC entry, `sp` points to `smc_ctx_t`. Save `lr`. */
+	str	lr, [sp, #SMC_CTX_LR_MON]
+
+	/* ------------------------------------------------
+	 * SMC in BL1 is handled assuming that the MMU is
+	 * turned off by BL2.
+	 * ------------------------------------------------
+	 */
+
+	/* ----------------------------------------------
+	 * Detect if this is a RUN_IMAGE or other SMC.
+	 * ----------------------------------------------
+	 */
+	mov	lr, #BL1_SMC_RUN_IMAGE
+	cmp	lr, r0
+	bne	smc_handler
+
+	/* ------------------------------------------------
+	 * Make sure only Secure world reaches here.
+	 * ------------------------------------------------
+	 */
+	ldcopr  r8, SCR
+	tst	r8, #SCR_NS_BIT
+	blne	report_exception
+
+	/* ---------------------------------------------------------------------
+	 * Pass control to next secure image.
+	 * Here it expects r1 to contain the address of a entry_point_info_t
+	 * structure describing the BL entrypoint.
+	 * ---------------------------------------------------------------------
+	 */
+	mov	r8, r1
+	mov	r0, r1
+	bl	bl1_print_next_bl_ep_info
+
+#if SPIN_ON_BL1_EXIT
+	bl	print_debug_loop_message
+debug_loop:
+	b	debug_loop
+#endif
+
+	mov	r0, r8
+	bl	bl1_plat_prepare_exit
+
+	stcopr	r0, TLBIALL
+	dsb	sy
+	isb
+
+	/*
+	 * 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 bl1_aarch32_smc_handler
+
+	/* -----------------------------------------------------
+	 * Save Secure/Normal world context and jump to
+	 * BL1 SMC handler.
+	 * -----------------------------------------------------
+	 */
+func smc_handler
+	/* -----------------------------------------------------
+	 * Save the GP registers.
+	 * -----------------------------------------------------
+	 */
+	smccc_save_gp_mode_regs
+
+	/*
+	 * `sp` still points to `smc_ctx_t`. Save it to a register
+	 * and restore the C runtime stack pointer to `sp`.
+	 */
+	mov	r6, sp
+	ldr	sp, [r6, #SMC_CTX_SP_MON]
+
+	ldr	r0, [r6, #SMC_CTX_SCR]
+	and	r7, r0, #SCR_NS_BIT		/* flags */
+
+	/* Switch to Secure Mode */
+	bic	r0, #SCR_NS_BIT
+	stcopr	r0, SCR
+	isb
+
+	/* If caller is from Secure world then turn on the MMU */
+	tst	r7, #SCR_NS_BIT
+	bne	skip_mmu_on
+
+	/* Turn on the MMU */
+	mov	r0, #DISABLE_DCACHE
+	bl	enable_mmu_svc_mon
+
+	/*
+	 * Invalidate `smc_ctx_t` in data cache to prevent dirty data being
+	 * used.
+	 */
+	mov	r0, r6
+	mov	r1, #SMC_CTX_SIZE
+	bl	inv_dcache_range
+
+	/* Enable the data cache. */
+	ldcopr	r9, SCTLR
+	orr	r9, r9, #SCTLR_C_BIT
+	stcopr	r9, SCTLR
+	isb
+
+skip_mmu_on:
+	/* Prepare arguments for BL1 SMC wrapper. */
+	ldr	r0, [r6, #SMC_CTX_GPREG_R0]	/* smc_fid */
+	mov	r1, #0				/* cookie */
+	mov	r2, r6				/* handle */
+	mov	r3, r7				/* flags */
+	bl	bl1_smc_wrapper
+
+	/* Get the smc_context for next BL image */
+	bl	smc_get_next_ctx
+	mov	r4, r0
+
+	/* Only turn-off MMU if going to secure world */
+	ldr	r5, [r4, #SMC_CTX_SCR]
+	tst	r5, #SCR_NS_BIT
+	bne	skip_mmu_off
+
+	/* Disable the MMU */
+	bl	disable_mmu_icache_secure
+	stcopr	r0, TLBIALL
+	dsb	sy
+	isb
+
+skip_mmu_off:
+	/* -----------------------------------------------------
+	 * Do the transition to next BL image.
+	 * -----------------------------------------------------
+	 */
+	mov	r0, r4
+	monitor_exit
+endfunc smc_handler
diff --git a/bl1/aarch64/bl1_arch_setup.c b/bl1/aarch64/bl1_arch_setup.c
new file mode 100644
index 0000000..f3de536
--- /dev/null
+++ b/bl1/aarch64/bl1_arch_setup.c
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include "../bl1_private.h"
+
+/*******************************************************************************
+ * Function that does the first bit of architectural setup that affects
+ * execution in the non-secure address space.
+ ******************************************************************************/
+void bl1_arch_setup(void)
+{
+	/* Set the next EL to be AArch64 */
+	write_scr_el3(read_scr_el3() | SCR_RW_BIT);
+}
diff --git a/bl1/aarch64/bl1_context_mgmt.c b/bl1/aarch64/bl1_context_mgmt.c
new file mode 100644
index 0000000..b9a7e5b
--- /dev/null
+++ b/bl1/aarch64/bl1_context_mgmt.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2015-2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <context.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <plat/common/platform.h>
+
+#include "../bl1_private.h"
+
+/* Following contains the cpu context pointers. */
+static void *bl1_cpu_context_ptr[2];
+entry_point_info_t *bl2_ep_info;
+
+
+void *cm_get_context(uint32_t security_state)
+{
+	assert(sec_state_is_valid(security_state));
+	return bl1_cpu_context_ptr[security_state];
+}
+
+void cm_set_context(void *context, uint32_t security_state)
+{
+	assert(sec_state_is_valid(security_state));
+	bl1_cpu_context_ptr[security_state] = context;
+}
+
+#if ENABLE_RME
+/*******************************************************************************
+ * This function prepares the entry point information to run BL2 in Root world,
+ * i.e. EL3, for the case when FEAT_RME is enabled.
+ ******************************************************************************/
+void bl1_prepare_next_image(unsigned int image_id)
+{
+	image_desc_t *bl2_desc;
+
+	assert(image_id == BL2_IMAGE_ID);
+
+	/* Get the image descriptor. */
+	bl2_desc = bl1_plat_get_image_desc(BL2_IMAGE_ID);
+	assert(bl2_desc != NULL);
+
+	/* Get the entry point info. */
+	bl2_ep_info = &bl2_desc->ep_info;
+
+	bl2_ep_info->spsr = (uint32_t)SPSR_64(MODE_EL3, MODE_SP_ELX,
+						DISABLE_ALL_EXCEPTIONS);
+
+	/*
+	 * Flush cache since bl2_ep_info is accessed after MMU is disabled
+	 * before jumping to BL2.
+	 */
+	flush_dcache_range((uintptr_t)bl2_ep_info, sizeof(entry_point_info_t));
+
+	/* Indicate that image is in execution state. */
+	bl2_desc->state = IMAGE_STATE_EXECUTED;
+
+	/* Print debug info and flush the console before running BL2. */
+	print_entry_point_info(bl2_ep_info);
+}
+#else
+/*******************************************************************************
+ * This function prepares the context for Secure/Normal world images.
+ * Normal world images are transitioned to EL2(if supported) else EL1.
+ ******************************************************************************/
+void bl1_prepare_next_image(unsigned int image_id)
+{
+
+	/*
+	 * Following array will be used for context management.
+	 * There are 2 instances, for the Secure and Non-Secure contexts.
+	 */
+	static cpu_context_t bl1_cpu_context[2];
+
+	unsigned int security_state, mode = MODE_EL1;
+	image_desc_t *desc;
+	entry_point_info_t *next_bl_ep;
+
+#if CTX_INCLUDE_AARCH32_REGS
+	/*
+	 * Ensure that the build flag to save AArch32 system registers in CPU
+	 * context is not set for AArch64-only platforms.
+	 */
+	if (el_implemented(1) == EL_IMPL_A64ONLY) {
+		ERROR("EL1 supports AArch64-only. Please set build flag "
+				"CTX_INCLUDE_AARCH32_REGS = 0\n");
+		panic();
+	}
+#endif
+
+	/* Get the image descriptor. */
+	desc = bl1_plat_get_image_desc(image_id);
+	assert(desc != NULL);
+
+	/* Get the entry point info. */
+	next_bl_ep = &desc->ep_info;
+
+	/* Get the image security state. */
+	security_state = GET_SECURITY_STATE(next_bl_ep->h.attr);
+
+	/* Setup the Secure/Non-Secure context if not done already. */
+	if (cm_get_context(security_state) == NULL)
+		cm_set_context(&bl1_cpu_context[security_state], security_state);
+
+	/* Prepare the SPSR for the next BL image. */
+	if ((security_state != SECURE) && (el_implemented(2) != EL_IMPL_NONE)) {
+		mode = MODE_EL2;
+	}
+
+	next_bl_ep->spsr = (uint32_t)SPSR_64((uint64_t) mode,
+		(uint64_t)MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+
+	/* Allow platform to make change */
+	bl1_plat_set_ep_info(image_id, next_bl_ep);
+
+	/* Prepare the context for the next BL image. */
+	cm_init_my_context(next_bl_ep);
+	cm_prepare_el3_exit(security_state);
+
+	/* Indicate that image is in execution state. */
+	desc->state = IMAGE_STATE_EXECUTED;
+
+	print_entry_point_info(next_bl_ep);
+}
+#endif /* ENABLE_RME */
diff --git a/bl1/aarch64/bl1_entrypoint.S b/bl1/aarch64/bl1_entrypoint.S
new file mode 100644
index 0000000..f61c060
--- /dev/null
+++ b/bl1/aarch64/bl1_entrypoint.S
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <common/bl_common.h>
+#include <el3_common_macros.S>
+
+	.globl	bl1_entrypoint
+	.globl	bl1_run_bl2_in_root
+
+
+	/* -----------------------------------------------------
+	 * bl1_entrypoint() is the entry point into the trusted
+	 * firmware code when a cpu is released from warm or
+	 * cold reset.
+	 * -----------------------------------------------------
+	 */
+
+func bl1_entrypoint
+	/* ---------------------------------------------------------------------
+	 * If the reset address is programmable then bl1_entrypoint() is
+	 * executed only on the cold boot path. Therefore, we can skip the warm
+	 * boot mailbox mechanism.
+	 * ---------------------------------------------------------------------
+	 */
+	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=bl1_exceptions		\
+		_pie_fixup_size=0
+
+	/* --------------------------------------------------------------------
+	 * Perform BL1 setup
+	 * --------------------------------------------------------------------
+	 */
+	bl	bl1_setup
+
+#if ENABLE_PAUTH
+	/* --------------------------------------------------------------------
+	 * Program APIAKey_EL1 and enable pointer authentication.
+	 * --------------------------------------------------------------------
+	 */
+	bl	pauth_init_enable_el3
+#endif /* ENABLE_PAUTH */
+
+	/* --------------------------------------------------------------------
+	 * Initialize platform and jump to our c-entry point
+	 * for this type of reset.
+	 * --------------------------------------------------------------------
+	 */
+	bl	bl1_main
+
+#if ENABLE_PAUTH
+	/* --------------------------------------------------------------------
+	 * Disable pointer authentication before jumping to next boot image.
+	 * --------------------------------------------------------------------
+	 */
+	bl	pauth_disable_el3
+#endif /* ENABLE_PAUTH */
+
+	/* --------------------------------------------------
+	 * Do the transition to next boot image.
+	 * --------------------------------------------------
+	 */
+#if ENABLE_RME
+	b	bl1_run_bl2_in_root
+#else
+	b	el3_exit
+#endif
+endfunc bl1_entrypoint
+
+	/* -----------------------------------------------------
+	 * void bl1_run_bl2_in_root();
+	 * This function runs BL2 in root/EL3 when RME is enabled.
+	 * -----------------------------------------------------
+	 */
+
+func bl1_run_bl2_in_root
+	/* read bl2_ep_info */
+	adrp	x20, bl2_ep_info
+	add	x20, x20, :lo12:bl2_ep_info
+	ldr	x20, [x20]
+
+	/* ---------------------------------------------
+	 * MMU needs to be disabled because BL2 executes
+	 * in EL3. It will initialize the address space
+	 * according to its own requirements.
+	 * ---------------------------------------------
+	 */
+	bl	disable_mmu_icache_el3
+	tlbi	alle3
+
+	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 bl1_run_bl2_in_root
diff --git a/bl1/aarch64/bl1_exceptions.S b/bl1/aarch64/bl1_exceptions.S
new file mode 100644
index 0000000..eaaf59a
--- /dev/null
+++ b/bl1/aarch64/bl1_exceptions.S
@@ -0,0 +1,287 @@
+/*
+ * Copyright (c) 2013-2022, 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 BL1.
+ * -----------------------------------------------------------------------------
+ */
+	.globl	bl1_exceptions
+
+vector_base bl1_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
+	/* Enable the SError interrupt */
+	msr	daifclr, #DAIF_ABT_BIT
+
+	str	x30, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_LR]
+
+	/* Expect only SMC exceptions */
+	mrs	x30, esr_el3
+	ubfx	x30, x30, #ESR_EC_SHIFT, #ESR_EC_LENGTH
+	cmp	x30, #EC_AARCH64_SMC
+	b.ne	unexpected_sync_exception
+
+	b	smc_handler64
+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
+
+
+func smc_handler64
+
+	/* ----------------------------------------------
+	 * Detect if this is a RUN_IMAGE or other SMC.
+	 * ----------------------------------------------
+	 */
+	mov	x30, #BL1_SMC_RUN_IMAGE
+	cmp	x30, x0
+	b.ne	smc_handler
+
+	/* ------------------------------------------------
+	 * Make sure only Secure world reaches here.
+	 * ------------------------------------------------
+	 */
+	mrs	x30, scr_el3
+	tst	x30, #SCR_NS_BIT
+	b.ne	unexpected_sync_exception
+
+	/* ----------------------------------------------
+	 * Handling RUN_IMAGE SMC. First switch back to
+	 * SP_EL0 for the C runtime stack.
+	 * ----------------------------------------------
+	 */
+	ldr	x30, [sp, #CTX_EL3STATE_OFFSET + CTX_RUNTIME_SP]
+	msr	spsel, #MODE_SP_EL0
+	mov	sp, x30
+
+	/* ---------------------------------------------------------------------
+	 * Pass EL3 control to next BL image.
+	 * Here it expects X1 with the address of a entry_point_info_t
+	 * structure describing the next BL image entrypoint.
+	 * ---------------------------------------------------------------------
+	 */
+	mov	x20, x1
+
+	mov	x0, x20
+	bl	bl1_print_next_bl_ep_info
+
+	ldp	x0, x1, [x20, #ENTRY_POINT_INFO_PC_OFFSET]
+	msr	elr_el3, x0
+	msr	spsr_el3, x1
+	ubfx	x0, x1, #MODE_EL_SHIFT, #2
+	cmp	x0, #MODE_EL3
+	b.ne	unexpected_sync_exception
+
+	bl	disable_mmu_icache_el3
+	tlbi	alle3
+	dsb	ish /* ERET implies ISB, so it is not needed here */
+
+#if SPIN_ON_BL1_EXIT
+	bl	print_debug_loop_message
+debug_loop:
+	b	debug_loop
+#endif
+
+	mov	x0, x20
+	bl	bl1_plat_prepare_exit
+
+	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 smc_handler64
+
+unexpected_sync_exception:
+	mov	x0, #SYNC_EXCEPTION_AARCH64
+	bl	plat_report_exception
+	no_ret	plat_panic_handler
+
+	/* -----------------------------------------------------
+	 * Save Secure/Normal world context and jump to
+	 * BL1 SMC handler.
+	 * -----------------------------------------------------
+	 */
+smc_handler:
+	/* -----------------------------------------------------
+	 * Save x0-x29 and ARMv8.3-PAuth (if enabled) registers.
+	 * Save PMCR_EL0 and disable Cycle Counter.
+	 * TODO: Revisit to store only SMCCC specified registers.
+	 * -----------------------------------------------------
+	 */
+	bl	prepare_el3_entry
+
+#if ENABLE_PAUTH
+	/* -----------------------------------------------------
+	 * Load and program stored APIAKey firmware key.
+	 * Re-enable pointer authentication in EL3, as it was
+	 * disabled before jumping to the next boot image.
+	 * -----------------------------------------------------
+	 */
+	bl	pauth_load_bl1_apiakey_enable
+#endif
+	/* -----------------------------------------------------
+	 * Populate the parameters for the SMC handler. We
+	 * already have x0-x4 in place. x5 will point to a
+	 * cookie (not used now). x6 will point to the context
+	 * structure (SP_EL3) and x7 will contain flags we need
+	 * to pass to the handler.
+	 * -----------------------------------------------------
+	 */
+	mov	x5, xzr
+	mov	x6, sp
+
+	/* -----------------------------------------------------
+	 * Restore the saved C runtime stack value which will
+	 * become the new SP_EL0 i.e. EL3 runtime stack. It was
+	 * saved in the 'cpu_context' structure prior to the last
+	 * ERET from EL3.
+	 * -----------------------------------------------------
+	 */
+	ldr	x12, [x6, #CTX_EL3STATE_OFFSET + CTX_RUNTIME_SP]
+
+	/* ---------------------------------------------
+	 * Switch back to SP_EL0 for the C runtime stack.
+	 * ---------------------------------------------
+	 */
+	msr	spsel, #MODE_SP_EL0
+	mov	sp, x12
+
+	/* -----------------------------------------------------
+	 * Save the SPSR_EL3, ELR_EL3, & SCR_EL3 in case there
+	 * is a world switch during SMC handling.
+	 * -----------------------------------------------------
+	 */
+	mrs	x16, spsr_el3
+	mrs	x17, elr_el3
+	mrs	x18, scr_el3
+	stp	x16, x17, [x6, #CTX_EL3STATE_OFFSET + CTX_SPSR_EL3]
+	str	x18, [x6, #CTX_EL3STATE_OFFSET + CTX_SCR_EL3]
+
+	/* Copy SCR_EL3.NS bit to the flag to indicate caller's security */
+	bfi	x7, x18, #0, #1
+
+	/* -----------------------------------------------------
+	 * Go to BL1 SMC handler.
+	 * -----------------------------------------------------
+	 */
+	bl	bl1_smc_handler
+
+	/* -----------------------------------------------------
+	 * Do the transition to next BL image.
+	 * -----------------------------------------------------
+	 */
+	b	el3_exit
diff --git a/bl1/bl1.ld.S b/bl1/bl1.ld.S
new file mode 100644
index 0000000..49dda85
--- /dev/null
+++ b/bl1/bl1.ld.S
@@ -0,0 +1,169 @@
+/*
+ * Copyright (c) 2013-2023, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * The .data section gets copied from ROM to RAM at runtime. Its LMA should be
+ * 16-byte aligned to allow efficient copying of 16-bytes aligned regions in it.
+ * Its VMA must be page-aligned as it marks the first read/write page.
+ */
+#define DATA_ALIGN	16
+
+#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(bl1_entrypoint)
+
+MEMORY {
+    ROM (rx): ORIGIN = BL1_RO_BASE, LENGTH = BL1_RO_LIMIT - BL1_RO_BASE
+    RAM (rwx): ORIGIN = BL1_RW_BASE, LENGTH = BL1_RW_LIMIT - BL1_RW_BASE
+}
+
+SECTIONS {
+    ROM_REGION_START = ORIGIN(ROM);
+    ROM_REGION_LENGTH = LENGTH(ROM);
+    RAM_REGION_START = ORIGIN(RAM);
+    RAM_REGION_LENGTH = LENGTH(RAM);
+
+    . = BL1_RO_BASE;
+
+    ASSERT(. == ALIGN(PAGE_SIZE),
+        "BL1_RO_BASE address is not aligned on a page boundary.")
+
+#if SEPARATE_CODE_AND_RODATA
+    .text . : {
+        __TEXT_START__ = .;
+
+        *bl1_entrypoint.o(.text*)
+        *(SORT_BY_ALIGNMENT(.text*))
+        *(.vectors)
+        __TEXT_END_UNALIGNED__ = .;
+
+        . = ALIGN(PAGE_SIZE);
+
+        __TEXT_END__ = .;
+    } >ROM
+
+    /* .ARM.extab and .ARM.exidx are only added because Clang needs them */
+    .ARM.extab . : {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } >ROM
+
+    .ARM.exidx . : {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } >ROM
+
+    .rodata . : {
+        __RODATA_START__ = .;
+
+        *(SORT_BY_ALIGNMENT(.rodata*))
+
+        RODATA_COMMON
+
+        /*
+         * No need to pad out the .rodata section to a page boundary. Next is
+         * the .data section, which can mapped in ROM with the same memory
+         * attributes as the .rodata section.
+         *
+         * Pad out to 16 bytes though as .data section needs to be 16-byte
+         * aligned and lld does not align the LMA to the alignment specified
+         * on the .data section.
+         */
+        __RODATA_END_UNALIGNED__ = .;
+        __RODATA_END__ = .;
+
+        . = ALIGN(16);
+    } >ROM
+#else /* SEPARATE_CODE_AND_RODATA */
+    .ro . : {
+        __RO_START__ = .;
+
+        *bl1_entrypoint.o(.text*)
+        *(SORT_BY_ALIGNMENT(.text*))
+        *(SORT_BY_ALIGNMENT(.rodata*))
+
+        RODATA_COMMON
+
+        *(.vectors)
+
+        __RO_END__ = .;
+
+        /*
+         * Pad out to 16 bytes as the .data section needs to be 16-byte aligned
+         * and lld does not align the LMA to the alignment specified on the
+         * .data section.
+         */
+        . = ALIGN(16);
+    } >ROM
+#endif /* SEPARATE_CODE_AND_RODATA */
+
+    ASSERT(__CPU_OPS_END__ > __CPU_OPS_START__,
+        "cpu_ops not defined for this platform.")
+
+    ROM_REGION_END = .;
+    . = BL1_RW_BASE;
+
+    ASSERT(BL1_RW_BASE == ALIGN(PAGE_SIZE),
+        "BL1_RW_BASE address is not aligned on a page boundary.")
+
+    DATA_SECTION >RAM AT>ROM
+
+    __DATA_RAM_START__ = __DATA_START__;
+    __DATA_RAM_END__ = __DATA_END__;
+
+    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 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 /* USE_COHERENT_MEM */
+
+    __BL1_RAM_START__ = ADDR(.data);
+    __BL1_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 BL1's actual content in Trusted ROM.
+     */
+    __BL1_ROM_END__ =  __DATA_ROM_START__ + __DATA_SIZE__;
+
+    ASSERT(__BL1_ROM_END__ <= BL1_RO_LIMIT,
+        "BL1's ROM content has exceeded its limit.")
+
+    __BSS_SIZE__ = SIZEOF(.bss);
+
+#if USE_COHERENT_MEM
+    __COHERENT_RAM_UNALIGNED_SIZE__ =
+        __COHERENT_RAM_END_UNALIGNED__ - __COHERENT_RAM_START__;
+#endif /* USE_COHERENT_MEM */
+
+    ASSERT(. <= BL1_RW_LIMIT, "BL1's RW section has exceeded its limit.")
+    RAM_REGION_END = .;
+}
diff --git a/bl1/bl1.mk b/bl1/bl1.mk
new file mode 100644
index 0000000..53946ab
--- /dev/null
+++ b/bl1/bl1.mk
@@ -0,0 +1,38 @@
+#
+# Copyright (c) 2013-2023, Arm Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+BL1_SOURCES		+=	bl1/${ARCH}/bl1_arch_setup.c		\
+				bl1/${ARCH}/bl1_context_mgmt.c		\
+				bl1/${ARCH}/bl1_entrypoint.S		\
+				bl1/${ARCH}/bl1_exceptions.S		\
+				bl1/bl1_main.c				\
+				lib/cpus/${ARCH}/cpu_helpers.S		\
+				lib/cpus/errata_report.c		\
+				lib/el3_runtime/${ARCH}/context_mgmt.c	\
+				plat/common/plat_bl1_common.c		\
+				plat/common/${ARCH}/platform_up_stack.S \
+				${MBEDTLS_SOURCES}
+
+ifeq (${ARCH},aarch64)
+BL1_SOURCES		+=	lib/cpus/aarch64/dsu_helpers.S		\
+				lib/el3_runtime/aarch64/context.S
+endif
+
+ifeq (${TRUSTED_BOARD_BOOT},1)
+BL1_SOURCES		+=	bl1/bl1_fwu.c
+endif
+
+ifeq (${ENABLE_PMF},1)
+BL1_SOURCES		+=	lib/pmf/pmf_main.c
+endif
+
+ifneq ($(findstring gcc,$(notdir $(LD))),)
+        BL1_LDFLAGS	+=	-Wl,--sort-section=alignment
+else ifneq ($(findstring ld,$(notdir $(LD))),)
+        BL1_LDFLAGS	+=	--sort-section=alignment
+endif
+
+BL1_DEFAULT_LINKER_SCRIPT_SOURCE := bl1/bl1.ld.S
diff --git a/bl1/bl1_fwu.c b/bl1/bl1_fwu.c
new file mode 100644
index 0000000..6d4dc7e
--- /dev/null
+++ b/bl1/bl1_fwu.c
@@ -0,0 +1,745 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <bl1/bl1.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <context.h>
+#include <drivers/auth/auth_mod.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+#include <smccc_helpers.h>
+
+#include "bl1_private.h"
+
+/*
+ * Function declarations.
+ */
+static int bl1_fwu_image_copy(unsigned int image_id,
+			uintptr_t image_src,
+			unsigned int block_size,
+			unsigned int image_size,
+			unsigned int flags);
+static int bl1_fwu_image_auth(unsigned int image_id,
+			uintptr_t image_src,
+			unsigned int image_size,
+			unsigned int flags);
+static int bl1_fwu_image_execute(unsigned int image_id,
+			void **handle,
+			unsigned int flags);
+static register_t bl1_fwu_image_resume(register_t image_param,
+			void **handle,
+			unsigned int flags);
+static int bl1_fwu_sec_image_done(void **handle,
+			unsigned int flags);
+static int bl1_fwu_image_reset(unsigned int image_id,
+			unsigned int flags);
+__dead2 static void bl1_fwu_done(void *client_cookie, void *reserved);
+
+/*
+ * This keeps track of last executed secure image id.
+ */
+static unsigned int sec_exec_image_id = INVALID_IMAGE_ID;
+
+/*******************************************************************************
+ * Top level handler for servicing FWU SMCs.
+ ******************************************************************************/
+u_register_t bl1_fwu_smc_handler(unsigned int smc_fid,
+			u_register_t x1,
+			u_register_t x2,
+			u_register_t x3,
+			u_register_t x4,
+			void *cookie,
+			void *handle,
+			unsigned int flags)
+{
+
+	switch (smc_fid) {
+	case FWU_SMC_IMAGE_COPY:
+		SMC_RET1(handle, bl1_fwu_image_copy((uint32_t)x1, x2,
+			(uint32_t)x3, (uint32_t)x4, flags));
+
+	case FWU_SMC_IMAGE_AUTH:
+		SMC_RET1(handle, bl1_fwu_image_auth((uint32_t)x1, x2,
+			(uint32_t)x3, flags));
+
+	case FWU_SMC_IMAGE_EXECUTE:
+		SMC_RET1(handle, bl1_fwu_image_execute((uint32_t)x1, &handle,
+			flags));
+
+	case FWU_SMC_IMAGE_RESUME:
+		SMC_RET1(handle, bl1_fwu_image_resume((register_t)x1, &handle,
+			flags));
+
+	case FWU_SMC_SEC_IMAGE_DONE:
+		SMC_RET1(handle, bl1_fwu_sec_image_done(&handle, flags));
+
+	case FWU_SMC_IMAGE_RESET:
+		SMC_RET1(handle, bl1_fwu_image_reset((uint32_t)x1, flags));
+
+	case FWU_SMC_UPDATE_DONE:
+		bl1_fwu_done((void *)x1, NULL);
+
+	default:
+		assert(false); /* Unreachable */
+		break;
+	}
+
+	SMC_RET1(handle, SMC_UNK);
+}
+
+/*******************************************************************************
+ * Utility functions to keep track of the images that are loaded at any time.
+ ******************************************************************************/
+
+#ifdef PLAT_FWU_MAX_SIMULTANEOUS_IMAGES
+#define FWU_MAX_SIMULTANEOUS_IMAGES	PLAT_FWU_MAX_SIMULTANEOUS_IMAGES
+#else
+#define FWU_MAX_SIMULTANEOUS_IMAGES	10
+#endif
+
+static unsigned int bl1_fwu_loaded_ids[FWU_MAX_SIMULTANEOUS_IMAGES] = {
+	[0 ... FWU_MAX_SIMULTANEOUS_IMAGES-1] = INVALID_IMAGE_ID
+};
+
+/*
+ * Adds an image_id to the bl1_fwu_loaded_ids array.
+ * Returns 0 on success, 1 on error.
+ */
+static int bl1_fwu_add_loaded_id(unsigned int image_id)
+{
+	int i;
+
+	/* Check if the ID is already in the list */
+	for (i = 0; i < FWU_MAX_SIMULTANEOUS_IMAGES; i++) {
+		if (bl1_fwu_loaded_ids[i] == image_id)
+			return 0;
+	}
+
+	/* Find an empty slot */
+	for (i = 0; i < FWU_MAX_SIMULTANEOUS_IMAGES; i++) {
+		if (bl1_fwu_loaded_ids[i] == INVALID_IMAGE_ID) {
+			bl1_fwu_loaded_ids[i] = image_id;
+			return 0;
+		}
+	}
+
+	return 1;
+}
+
+/*
+ * Removes an image_id from the bl1_fwu_loaded_ids array.
+ * Returns 0 on success, 1 on error.
+ */
+static int bl1_fwu_remove_loaded_id(unsigned int image_id)
+{
+	int i;
+
+	/* Find the ID */
+	for (i = 0; i < FWU_MAX_SIMULTANEOUS_IMAGES; i++) {
+		if (bl1_fwu_loaded_ids[i] == image_id) {
+			bl1_fwu_loaded_ids[i] = INVALID_IMAGE_ID;
+			return 0;
+		}
+	}
+
+	return 1;
+}
+
+/*******************************************************************************
+ * This function checks if the specified image overlaps another image already
+ * loaded. It returns 0 if there is no overlap, a negative error code otherwise.
+ ******************************************************************************/
+static int bl1_fwu_image_check_overlaps(unsigned int image_id)
+{
+	const image_desc_t *desc, *checked_desc;
+	const image_info_t *info, *checked_info;
+
+	uintptr_t image_base, image_end;
+	uintptr_t checked_image_base, checked_image_end;
+
+	checked_desc = bl1_plat_get_image_desc(image_id);
+	checked_info = &checked_desc->image_info;
+
+	/* Image being checked mustn't be empty. */
+	assert(checked_info->image_size != 0);
+
+	checked_image_base = checked_info->image_base;
+	checked_image_end = checked_image_base + checked_info->image_size - 1;
+	/* No need to check for overflows, it's done in bl1_fwu_image_copy(). */
+
+	for (int i = 0; i < FWU_MAX_SIMULTANEOUS_IMAGES; i++) {
+
+		/* Skip INVALID_IMAGE_IDs and don't check image against itself */
+		if ((bl1_fwu_loaded_ids[i] == INVALID_IMAGE_ID) ||
+				(bl1_fwu_loaded_ids[i] == image_id))
+			continue;
+
+		desc = bl1_plat_get_image_desc(bl1_fwu_loaded_ids[i]);
+
+		/* Only check images that are loaded or being loaded. */
+		assert ((desc != NULL) && (desc->state != IMAGE_STATE_RESET));
+
+		info = &desc->image_info;
+
+		/* There cannot be overlaps with an empty image. */
+		if (info->image_size == 0)
+			continue;
+
+		image_base = info->image_base;
+		image_end = image_base + info->image_size - 1;
+		/*
+		 * Overflows cannot happen. It is checked in
+		 * bl1_fwu_image_copy() when the image goes from RESET to
+		 * COPYING or COPIED.
+		 */
+		assert (image_end > image_base);
+
+		/* Check if there are overlaps. */
+		if (!((image_end < checked_image_base) ||
+		    (checked_image_end < image_base))) {
+			VERBOSE("Image with ID %d overlaps existing image with ID %d",
+				checked_desc->image_id, desc->image_id);
+			return -EPERM;
+		}
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function is responsible for copying secure images in AP Secure RAM.
+ ******************************************************************************/
+static int bl1_fwu_image_copy(unsigned int image_id,
+			uintptr_t image_src,
+			unsigned int block_size,
+			unsigned int image_size,
+			unsigned int flags)
+{
+	uintptr_t dest_addr;
+	unsigned int remaining;
+	image_desc_t *desc;
+
+	/* Get the image descriptor. */
+	desc = bl1_plat_get_image_desc(image_id);
+	if (desc == NULL) {
+		WARN("BL1-FWU: Invalid image ID %u\n", image_id);
+		return -EPERM;
+	}
+
+	/*
+	 * The request must originate from a non-secure caller and target a
+	 * secure image. Any other scenario is invalid.
+	 */
+	if (GET_SECURITY_STATE(flags) == SECURE) {
+		WARN("BL1-FWU: Copy not allowed from secure world.\n");
+		return -EPERM;
+	}
+	if (GET_SECURITY_STATE(desc->ep_info.h.attr) == NON_SECURE) {
+		WARN("BL1-FWU: Copy not allowed for non-secure images.\n");
+		return -EPERM;
+	}
+
+	/* Check whether the FWU state machine is in the correct state. */
+	if ((desc->state != IMAGE_STATE_RESET) &&
+	    (desc->state != IMAGE_STATE_COPYING)) {
+		WARN("BL1-FWU: Copy not allowed at this point of the FWU"
+			" process.\n");
+		return -EPERM;
+	}
+
+	if ((image_src == 0U) || (block_size == 0U) ||
+	    check_uptr_overflow(image_src, block_size - 1)) {
+		WARN("BL1-FWU: Copy not allowed due to invalid image source"
+			" or block size\n");
+		return -ENOMEM;
+	}
+
+	if (desc->state == IMAGE_STATE_COPYING) {
+		/*
+		 * There must have been at least 1 copy operation for this image
+		 * previously.
+		 */
+		assert(desc->copied_size != 0U);
+		/*
+		 * The image size must have been recorded in the 1st copy
+		 * operation.
+		 */
+		image_size = desc->image_info.image_size;
+		assert(image_size != 0);
+		assert(desc->copied_size < image_size);
+
+		INFO("BL1-FWU: Continuing image copy in blocks\n");
+	} else { /* desc->state == IMAGE_STATE_RESET */
+		INFO("BL1-FWU: Initial call to copy an image\n");
+
+		/*
+		 * image_size is relevant only for the 1st copy request, it is
+		 * then ignored for subsequent calls for this image.
+		 */
+		if (image_size == 0) {
+			WARN("BL1-FWU: Copy not allowed due to invalid image"
+				" size\n");
+			return -ENOMEM;
+		}
+
+		/* Check that the image size to load is within limit */
+		if (image_size > desc->image_info.image_max_size) {
+			WARN("BL1-FWU: Image size out of bounds\n");
+			return -ENOMEM;
+		}
+
+		/* Save the given image size. */
+		desc->image_info.image_size = image_size;
+
+		/* Make sure the image doesn't overlap other images. */
+		if (bl1_fwu_image_check_overlaps(image_id) != 0) {
+			desc->image_info.image_size = 0;
+			WARN("BL1-FWU: This image overlaps another one\n");
+			return -EPERM;
+		}
+
+		/*
+		 * copied_size must be explicitly initialized here because the
+		 * FWU code doesn't necessarily do it when it resets the state
+		 * machine.
+		 */
+		desc->copied_size = 0;
+	}
+
+	/*
+	 * If the given block size is more than the total image size
+	 * then clip the former to the latter.
+	 */
+	remaining = image_size - desc->copied_size;
+	if (block_size > remaining) {
+		WARN("BL1-FWU: Block size is too big, clipping it.\n");
+		block_size = remaining;
+	}
+
+	/* Make sure the source image is mapped in memory. */
+	if (bl1_plat_mem_check(image_src, block_size, flags) != 0) {
+		WARN("BL1-FWU: Source image is not mapped.\n");
+		return -ENOMEM;
+	}
+
+	if (bl1_fwu_add_loaded_id(image_id) != 0) {
+		WARN("BL1-FWU: Too many images loaded at the same time.\n");
+		return -ENOMEM;
+	}
+
+	/* Allow the platform to handle pre-image load before copying */
+	if (desc->state == IMAGE_STATE_RESET) {
+		if (bl1_plat_handle_pre_image_load(image_id) != 0) {
+			ERROR("BL1-FWU: Failure in pre-image load of image id %d\n",
+					image_id);
+			return -EPERM;
+		}
+	}
+
+	/* Everything looks sane. Go ahead and copy the block of data. */
+	dest_addr = desc->image_info.image_base + desc->copied_size;
+	(void)memcpy((void *) dest_addr, (const void *) image_src, block_size);
+	flush_dcache_range(dest_addr, block_size);
+
+	desc->copied_size += block_size;
+	desc->state = (block_size == remaining) ?
+		IMAGE_STATE_COPIED : IMAGE_STATE_COPYING;
+
+	INFO("BL1-FWU: Copy operation successful.\n");
+	return 0;
+}
+
+/*******************************************************************************
+ * This function is responsible for authenticating Normal/Secure images.
+ ******************************************************************************/
+static int bl1_fwu_image_auth(unsigned int image_id,
+			uintptr_t image_src,
+			unsigned int image_size,
+			unsigned int flags)
+{
+	int result;
+	uintptr_t base_addr;
+	unsigned int total_size;
+	image_desc_t *desc;
+
+	/* Get the image descriptor. */
+	desc = bl1_plat_get_image_desc(image_id);
+	if (desc ==  NULL)
+		return -EPERM;
+
+	if (GET_SECURITY_STATE(flags) == SECURE) {
+		if (desc->state != IMAGE_STATE_RESET) {
+			WARN("BL1-FWU: Authentication from secure world "
+				"while in invalid state\n");
+			return -EPERM;
+		}
+	} else {
+		if (GET_SECURITY_STATE(desc->ep_info.h.attr) == SECURE) {
+			if (desc->state != IMAGE_STATE_COPIED) {
+				WARN("BL1-FWU: Authentication of secure image "
+					"from non-secure world while not in copied state\n");
+				return -EPERM;
+			}
+		} else {
+			if (desc->state != IMAGE_STATE_RESET) {
+				WARN("BL1-FWU: Authentication of non-secure image "
+					"from non-secure world while in invalid state\n");
+				return -EPERM;
+			}
+		}
+	}
+
+	if (desc->state == IMAGE_STATE_COPIED) {
+		/*
+		 * Image is in COPIED state.
+		 * Use the stored address and size.
+		 */
+		base_addr = desc->image_info.image_base;
+		total_size = desc->image_info.image_size;
+	} else {
+		if ((image_src == 0U) || (image_size == 0U) ||
+		    check_uptr_overflow(image_src, image_size - 1)) {
+			WARN("BL1-FWU: Auth not allowed due to invalid"
+				" image source/size\n");
+			return -ENOMEM;
+		}
+
+		/*
+		 * Image is in RESET state.
+		 * Check the parameters and authenticate the source image in place.
+		 */
+		if (bl1_plat_mem_check(image_src, image_size,
+					desc->ep_info.h.attr) != 0) {
+			WARN("BL1-FWU: Authentication arguments source/size not mapped\n");
+			return -ENOMEM;
+		}
+
+		if (bl1_fwu_add_loaded_id(image_id) != 0) {
+			WARN("BL1-FWU: Too many images loaded at the same time.\n");
+			return -ENOMEM;
+		}
+
+		base_addr = image_src;
+		total_size = image_size;
+
+		/* Update the image size in the descriptor. */
+		desc->image_info.image_size = total_size;
+	}
+
+	/*
+	 * Authenticate the image.
+	 */
+	INFO("BL1-FWU: Authenticating image_id:%d\n", image_id);
+	result = auth_mod_verify_img(image_id, (void *)base_addr, total_size);
+	if (result != 0) {
+		WARN("BL1-FWU: Authentication Failed err=%d\n", result);
+
+		/*
+		 * Authentication has failed.
+		 * Clear the memory if the image was copied.
+		 * This is to prevent an attack where this contains
+		 * some malicious code that can somehow be executed later.
+		 */
+		if (desc->state == IMAGE_STATE_COPIED) {
+			/* Clear the memory.*/
+			zero_normalmem((void *)base_addr, total_size);
+			flush_dcache_range(base_addr, total_size);
+
+			/* Indicate that image can be copied again*/
+			desc->state = IMAGE_STATE_RESET;
+		}
+
+		/*
+		 * Even if this fails it's ok because the ID isn't in the array.
+		 * The image cannot be in RESET state here, it is checked at the
+		 * beginning of the function.
+		 */
+		(void)bl1_fwu_remove_loaded_id(image_id);
+		return -EAUTH;
+	}
+
+	/* Indicate that image is in authenticated state. */
+	desc->state = IMAGE_STATE_AUTHENTICATED;
+
+	/* Allow the platform to handle post-image load */
+	result = bl1_plat_handle_post_image_load(image_id);
+	if (result != 0) {
+		ERROR("BL1-FWU: Failure %d in post-image load of image id %d\n",
+				result, image_id);
+		/*
+		 * Panic here as the platform handling of post-image load is
+		 * not correct.
+		 */
+		plat_error_handler(result);
+	}
+
+	/*
+	 * Flush image_info to memory so that other
+	 * secure world images can see changes.
+	 */
+	flush_dcache_range((uintptr_t)&desc->image_info,
+		sizeof(image_info_t));
+
+	INFO("BL1-FWU: Authentication was successful\n");
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function is responsible for executing Secure images.
+ ******************************************************************************/
+static int bl1_fwu_image_execute(unsigned int image_id,
+			void **handle,
+			unsigned int flags)
+{
+	/* Get the image descriptor. */
+	image_desc_t *desc = bl1_plat_get_image_desc(image_id);
+
+	/*
+	 * Execution is NOT allowed if:
+	 * image_id is invalid OR
+	 * Caller is from Secure world OR
+	 * Image is Non-Secure OR
+	 * Image is Non-Executable OR
+	 * Image is NOT in AUTHENTICATED state.
+	 */
+	if ((desc == NULL) ||
+	    (GET_SECURITY_STATE(flags) == SECURE) ||
+	    (GET_SECURITY_STATE(desc->ep_info.h.attr) == NON_SECURE) ||
+	    (EP_GET_EXE(desc->ep_info.h.attr) == NON_EXECUTABLE) ||
+	    (desc->state != IMAGE_STATE_AUTHENTICATED)) {
+		WARN("BL1-FWU: Execution not allowed due to invalid state/args\n");
+		return -EPERM;
+	}
+
+	INFO("BL1-FWU: Executing Secure image\n");
+
+#ifdef __aarch64__
+	/* Save NS-EL1 system registers. */
+	cm_el1_sysregs_context_save(NON_SECURE);
+#endif
+
+	/* Prepare the image for execution. */
+	bl1_prepare_next_image(image_id);
+
+	/* Update the secure image id. */
+	sec_exec_image_id = image_id;
+
+#ifdef __aarch64__
+	*handle = cm_get_context(SECURE);
+#else
+	*handle = smc_get_ctx(SECURE);
+#endif
+	return 0;
+}
+
+/*******************************************************************************
+ * This function is responsible for resuming execution in the other security
+ * world
+ ******************************************************************************/
+static register_t bl1_fwu_image_resume(register_t image_param,
+			void **handle,
+			unsigned int flags)
+{
+	image_desc_t *desc;
+	unsigned int resume_sec_state;
+	unsigned int caller_sec_state = GET_SECURITY_STATE(flags);
+
+	/* Get the image descriptor for last executed secure image id. */
+	desc = bl1_plat_get_image_desc(sec_exec_image_id);
+	if (caller_sec_state == NON_SECURE) {
+		if (desc == NULL) {
+			WARN("BL1-FWU: Resume not allowed due to no available"
+				"secure image\n");
+			return -EPERM;
+		}
+	} else {
+		/* desc must be valid for secure world callers */
+		assert(desc != NULL);
+	}
+
+	assert(GET_SECURITY_STATE(desc->ep_info.h.attr) == SECURE);
+	assert(EP_GET_EXE(desc->ep_info.h.attr) == EXECUTABLE);
+
+	if (caller_sec_state == SECURE) {
+		assert(desc->state == IMAGE_STATE_EXECUTED);
+
+		/* Update the flags. */
+		desc->state = IMAGE_STATE_INTERRUPTED;
+		resume_sec_state = NON_SECURE;
+	} else {
+		assert(desc->state == IMAGE_STATE_INTERRUPTED);
+
+		/* Update the flags. */
+		desc->state = IMAGE_STATE_EXECUTED;
+		resume_sec_state = SECURE;
+	}
+
+	INFO("BL1-FWU: Resuming %s world context\n",
+		(resume_sec_state == SECURE) ? "secure" : "normal");
+
+#ifdef __aarch64__
+	/* Save the EL1 system registers of calling world. */
+	cm_el1_sysregs_context_save(caller_sec_state);
+
+	/* Restore the EL1 system registers of resuming world. */
+	cm_el1_sysregs_context_restore(resume_sec_state);
+
+	/* Update the next context. */
+	cm_set_next_eret_context(resume_sec_state);
+
+	*handle = cm_get_context(resume_sec_state);
+#else
+	/* Update the next context. */
+	cm_set_next_context(cm_get_context(resume_sec_state));
+
+	/* Prepare the smc context for the next BL image. */
+	smc_set_next_ctx(resume_sec_state);
+
+	*handle = smc_get_ctx(resume_sec_state);
+#endif
+	return image_param;
+}
+
+/*******************************************************************************
+ * This function is responsible for resuming normal world context.
+ ******************************************************************************/
+static int bl1_fwu_sec_image_done(void **handle, unsigned int flags)
+{
+	image_desc_t *desc;
+
+	/* Make sure caller is from the secure world */
+	if (GET_SECURITY_STATE(flags) == NON_SECURE) {
+		WARN("BL1-FWU: Image done not allowed from normal world\n");
+		return -EPERM;
+	}
+
+	/* Get the image descriptor for last executed secure image id */
+	desc = bl1_plat_get_image_desc(sec_exec_image_id);
+
+	/* desc must correspond to a valid secure executing image */
+	assert(desc != NULL);
+	assert(GET_SECURITY_STATE(desc->ep_info.h.attr) == SECURE);
+	assert(EP_GET_EXE(desc->ep_info.h.attr) == EXECUTABLE);
+	assert(desc->state == IMAGE_STATE_EXECUTED);
+
+#if ENABLE_ASSERTIONS
+	int rc = bl1_fwu_remove_loaded_id(sec_exec_image_id);
+	assert(rc == 0);
+#else
+	bl1_fwu_remove_loaded_id(sec_exec_image_id);
+#endif
+
+	/* Update the flags. */
+	desc->state = IMAGE_STATE_RESET;
+	sec_exec_image_id = INVALID_IMAGE_ID;
+
+	INFO("BL1-FWU: Resuming Normal world context\n");
+#ifdef __aarch64__
+	/*
+	 * Secure world is done so no need to save the context.
+	 * Just restore the Non-Secure context.
+	 */
+	cm_el1_sysregs_context_restore(NON_SECURE);
+
+	/* Update the next context. */
+	cm_set_next_eret_context(NON_SECURE);
+
+	*handle = cm_get_context(NON_SECURE);
+#else
+	/* Update the next context. */
+	cm_set_next_context(cm_get_context(NON_SECURE));
+
+	/* Prepare the smc context for the next BL image. */
+	smc_set_next_ctx(NON_SECURE);
+
+	*handle = smc_get_ctx(NON_SECURE);
+#endif
+	return 0;
+}
+
+/*******************************************************************************
+ * This function provides the opportunity for users to perform any
+ * platform specific handling after the Firmware update is done.
+ ******************************************************************************/
+__dead2 static void bl1_fwu_done(void *client_cookie, void *reserved)
+{
+	NOTICE("BL1-FWU: *******FWU Process Completed*******\n");
+
+	/*
+	 * Call platform done function.
+	 */
+	bl1_plat_fwu_done(client_cookie, reserved);
+	assert(false);
+}
+
+/*******************************************************************************
+ * This function resets an image to IMAGE_STATE_RESET. It fails if the image is
+ * being executed.
+ ******************************************************************************/
+static int bl1_fwu_image_reset(unsigned int image_id, unsigned int flags)
+{
+	image_desc_t *desc = bl1_plat_get_image_desc(image_id);
+
+	if ((desc == NULL) || (GET_SECURITY_STATE(flags) == SECURE)) {
+		WARN("BL1-FWU: Reset not allowed due to invalid args\n");
+		return -EPERM;
+	}
+
+	switch (desc->state) {
+
+	case IMAGE_STATE_RESET:
+		/* Nothing to do. */
+		break;
+
+	case IMAGE_STATE_INTERRUPTED:
+	case IMAGE_STATE_AUTHENTICATED:
+	case IMAGE_STATE_COPIED:
+	case IMAGE_STATE_COPYING:
+
+		if (bl1_fwu_remove_loaded_id(image_id) != 0) {
+			WARN("BL1-FWU: Image reset couldn't find the image ID\n");
+			return -EPERM;
+		}
+
+		if (desc->copied_size != 0U) {
+			/* Clear the memory if the image is copied */
+			assert(GET_SECURITY_STATE(desc->ep_info.h.attr)
+				== SECURE);
+
+			zero_normalmem((void *)desc->image_info.image_base,
+					desc->copied_size);
+			flush_dcache_range(desc->image_info.image_base,
+					desc->copied_size);
+		}
+
+		/* Reset status variables */
+		desc->copied_size = 0;
+		desc->image_info.image_size = 0;
+		desc->state = IMAGE_STATE_RESET;
+
+		/* Clear authentication state */
+		auth_img_flags[image_id] = 0;
+
+		break;
+
+	case IMAGE_STATE_EXECUTED:
+	default:
+		assert(false); /* Unreachable */
+		break;
+	}
+
+	return 0;
+}
diff --git a/bl1/bl1_main.c b/bl1/bl1_main.c
new file mode 100644
index 0000000..6fe5511
--- /dev/null
+++ b/bl1/bl1_main.c
@@ -0,0 +1,302 @@
+/*
+ * Copyright (c) 2013-2023, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <bl1/bl1.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 <lib/bootmarker_capture.h>
+#include <lib/cpus/errata.h>
+#include <lib/pmf/pmf.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+#include <smccc_helpers.h>
+#include <tools_share/uuid.h>
+
+#include "bl1_private.h"
+
+static void bl1_load_bl2(void);
+
+#if ENABLE_PAUTH
+uint64_t bl1_apiakey[2];
+#endif
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	PMF_REGISTER_SERVICE(bl_svc, PMF_RT_INSTR_SVC_ID,
+		BL_TOTAL_IDS, PMF_DUMP_ENABLE)
+#endif
+
+/*******************************************************************************
+ * Helper utility to calculate the BL2 memory layout taking into consideration
+ * the BL1 RW data assuming that it is at the top of the memory layout.
+ ******************************************************************************/
+void bl1_calc_bl2_mem_layout(const meminfo_t *bl1_mem_layout,
+			meminfo_t *bl2_mem_layout)
+{
+	assert(bl1_mem_layout != NULL);
+	assert(bl2_mem_layout != NULL);
+
+	/*
+	 * Remove BL1 RW data from the scope of memory visible to BL2.
+	 * This is assuming BL1 RW data is at the top of bl1_mem_layout.
+	 */
+	assert(BL1_RW_BASE > bl1_mem_layout->total_base);
+	bl2_mem_layout->total_base = bl1_mem_layout->total_base;
+	bl2_mem_layout->total_size = BL1_RW_BASE - bl1_mem_layout->total_base;
+
+	flush_dcache_range((uintptr_t)bl2_mem_layout, sizeof(meminfo_t));
+}
+
+/*******************************************************************************
+ * Setup function for BL1.
+ ******************************************************************************/
+void bl1_setup(void)
+{
+	/* Perform early platform-specific setup */
+	bl1_early_platform_setup();
+
+	/* Perform late platform-specific setup */
+	bl1_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 */
+}
+
+/*******************************************************************************
+ * Function to perform late architectural and platform specific initialization.
+ * It also queries the platform to load and run next BL image. Only called
+ * by the primary cpu after a cold boot.
+ ******************************************************************************/
+void bl1_main(void)
+{
+	unsigned int image_id;
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	PMF_CAPTURE_TIMESTAMP(bl_svc, BL1_ENTRY, PMF_CACHE_MAINT);
+#endif
+
+	/* Announce our arrival */
+	NOTICE(FIRMWARE_WELCOME_STR);
+	NOTICE("BL1: %s\n", version_string);
+	NOTICE("BL1: %s\n", build_message);
+
+	INFO("BL1: RAM %p - %p\n", (void *)BL1_RAM_BASE, (void *)BL1_RAM_LIMIT);
+
+	print_errata_status();
+
+#if ENABLE_ASSERTIONS
+	u_register_t val;
+	/*
+	 * Ensure that MMU/Caches and coherency are turned on
+	 */
+#ifdef __aarch64__
+	val = read_sctlr_el3();
+#else
+	val = read_sctlr();
+#endif
+	assert((val & SCTLR_M_BIT) != 0);
+	assert((val & SCTLR_C_BIT) != 0);
+	assert((val & SCTLR_I_BIT) != 0);
+	/*
+	 * Check that Cache Writeback Granule (CWG) in CTR_EL0 matches the
+	 * provided platform value
+	 */
+	val = (read_ctr_el0() >> CTR_CWG_SHIFT) & CTR_CWG_MASK;
+	/*
+	 * If CWG is zero, then no CWG information is available but we can
+	 * at least check the platform value is less than the architectural
+	 * maximum.
+	 */
+	if (val != 0)
+		assert(CACHE_WRITEBACK_GRANULE == SIZE_FROM_LOG2_WORDS(val));
+	else
+		assert(CACHE_WRITEBACK_GRANULE <= MAX_CACHE_LINE_SIZE);
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Perform remaining generic architectural setup from EL3 */
+	bl1_arch_setup();
+
+	crypto_mod_init();
+
+	/* Initialize authentication module */
+	auth_mod_init();
+
+	/* Initialize the measured boot */
+	bl1_plat_mboot_init();
+
+	/* Perform platform setup in BL1. */
+	bl1_platform_setup();
+
+#if ENABLE_PAUTH
+	/* Store APIAKey_EL1 key */
+	bl1_apiakey[0] = read_apiakeylo_el1();
+	bl1_apiakey[1] = read_apiakeyhi_el1();
+#endif /* ENABLE_PAUTH */
+
+	/* Get the image id of next image to load and run. */
+	image_id = bl1_plat_get_next_image_id();
+
+	/*
+	 * We currently interpret any image id other than
+	 * BL2_IMAGE_ID as the start of firmware update.
+	 */
+	if (image_id == BL2_IMAGE_ID)
+		bl1_load_bl2();
+	else
+		NOTICE("BL1-FWU: *******FWU Process Started*******\n");
+
+	/* Teardown the measured boot driver */
+	bl1_plat_mboot_finish();
+
+	bl1_prepare_next_image(image_id);
+
+#if ENABLE_RUNTIME_INSTRUMENTATION
+	PMF_CAPTURE_TIMESTAMP(bl_svc, BL1_EXIT, PMF_CACHE_MAINT);
+#endif
+
+	console_flush();
+}
+
+/*******************************************************************************
+ * This function locates and loads the BL2 raw binary image in the trusted SRAM.
+ * Called by the primary cpu after a cold boot.
+ * TODO: Add support for alternative image load mechanism e.g using virtio/elf
+ * loader etc.
+ ******************************************************************************/
+static void bl1_load_bl2(void)
+{
+	image_desc_t *desc;
+	image_info_t *info;
+	int err;
+
+	/* Get the image descriptor */
+	desc = bl1_plat_get_image_desc(BL2_IMAGE_ID);
+	assert(desc != NULL);
+
+	/* Get the image info */
+	info = &desc->image_info;
+	INFO("BL1: Loading BL2\n");
+
+	err = bl1_plat_handle_pre_image_load(BL2_IMAGE_ID);
+	if (err != 0) {
+		ERROR("Failure in pre image load handling of BL2 (%d)\n", err);
+		plat_error_handler(err);
+	}
+
+	err = load_auth_image(BL2_IMAGE_ID, info);
+	if (err != 0) {
+		ERROR("Failed to load BL2 firmware.\n");
+		plat_error_handler(err);
+	}
+
+	/* Allow platform to handle image information. */
+	err = bl1_plat_handle_post_image_load(BL2_IMAGE_ID);
+	if (err != 0) {
+		ERROR("Failure in post image load handling of BL2 (%d)\n", err);
+		plat_error_handler(err);
+	}
+
+	NOTICE("BL1: Booting BL2\n");
+}
+
+/*******************************************************************************
+ * Function called just before handing over to the next BL to inform the user
+ * about the boot progress. In debug mode, also print details about the BL
+ * image's execution context.
+ ******************************************************************************/
+void bl1_print_next_bl_ep_info(const entry_point_info_t *bl_ep_info)
+{
+#ifdef __aarch64__
+	NOTICE("BL1: Booting BL31\n");
+#else
+	NOTICE("BL1: Booting BL32\n");
+#endif /* __aarch64__ */
+	print_entry_point_info(bl_ep_info);
+}
+
+#if SPIN_ON_BL1_EXIT
+void print_debug_loop_message(void)
+{
+	NOTICE("BL1: Debug loop, spinning forever\n");
+	NOTICE("BL1: Please connect the debugger to continue\n");
+}
+#endif
+
+/*******************************************************************************
+ * Top level handler for servicing BL1 SMCs.
+ ******************************************************************************/
+u_register_t bl1_smc_handler(unsigned int smc_fid,
+	u_register_t x1,
+	u_register_t x2,
+	u_register_t x3,
+	u_register_t x4,
+	void *cookie,
+	void *handle,
+	unsigned int flags)
+{
+	/* BL1 Service UUID */
+	DEFINE_SVC_UUID2(bl1_svc_uid,
+		U(0xd46739fd), 0xcb72, 0x9a4d, 0xb5, 0x75,
+		0x67, 0x15, 0xd6, 0xf4, 0xbb, 0x4a);
+
+
+#if TRUSTED_BOARD_BOOT
+	/*
+	 * Dispatch FWU calls to FWU SMC handler and return its return
+	 * value
+	 */
+	if (is_fwu_fid(smc_fid)) {
+		return bl1_fwu_smc_handler(smc_fid, x1, x2, x3, x4, cookie,
+			handle, flags);
+	}
+#endif
+
+	switch (smc_fid) {
+	case BL1_SMC_CALL_COUNT:
+		SMC_RET1(handle, BL1_NUM_SMC_CALLS);
+
+	case BL1_SMC_UID:
+		SMC_UUID_RET(handle, bl1_svc_uid);
+
+	case BL1_SMC_VERSION:
+		SMC_RET1(handle, BL1_SMC_MAJOR_VER | BL1_SMC_MINOR_VER);
+
+	default:
+		WARN("Unimplemented BL1 SMC Call: 0x%x\n", smc_fid);
+		SMC_RET1(handle, SMC_UNK);
+	}
+}
+
+/*******************************************************************************
+ * BL1 SMC wrapper.  This function is only used in AArch32 mode to ensure ABI
+ * compliance when invoking bl1_smc_handler.
+ ******************************************************************************/
+u_register_t bl1_smc_wrapper(uint32_t smc_fid,
+	void *cookie,
+	void *handle,
+	unsigned int flags)
+{
+	u_register_t x1, x2, x3, x4;
+
+	assert(handle != NULL);
+
+	get_smc_params_from_ctx(handle, x1, x2, x3, x4);
+	return bl1_smc_handler(smc_fid, x1, x2, x3, x4, cookie, handle, flags);
+}
diff --git a/bl1/bl1_private.h b/bl1/bl1_private.h
new file mode 100644
index 0000000..61fb5be
--- /dev/null
+++ b/bl1/bl1_private.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BL1_PRIVATE_H
+#define BL1_PRIVATE_H
+
+#include <stdint.h>
+
+#include <common/bl_common.h>
+
+extern entry_point_info_t *bl2_ep_info;
+
+/******************************************
+ * Function prototypes
+ *****************************************/
+void bl1_arch_setup(void);
+
+void bl1_prepare_next_image(unsigned int image_id);
+void bl1_run_bl2_in_root(void);
+
+u_register_t bl1_fwu_smc_handler(unsigned int smc_fid,
+		u_register_t x1,
+		u_register_t x2,
+		u_register_t x3,
+		u_register_t x4,
+		void *cookie,
+		void *handle,
+		unsigned int flags);
+
+#endif /* BL1_PRIVATE_H */
diff --git a/bl1/tbbr/tbbr_img_desc.c b/bl1/tbbr/tbbr_img_desc.c
new file mode 100644
index 0000000..4836712
--- /dev/null
+++ b/bl1/tbbr/tbbr_img_desc.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <bl1/tbbr/tbbr_img_desc.h>
+#include <common/bl_common.h>
+
+image_desc_t bl1_tbbr_image_descs[] = {
+    {
+	    .image_id = FWU_CERT_ID,
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+		    VERSION_1, image_info_t, 0),
+	    .image_info.image_base = BL2_BASE,
+	    .image_info.image_max_size = BL2_LIMIT - BL2_BASE,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+		    VERSION_1, entry_point_info_t, SECURE),
+    },
+#if NS_BL1U_BASE
+    {
+	    .image_id = NS_BL1U_IMAGE_ID,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+		    VERSION_1, entry_point_info_t, NON_SECURE | EXECUTABLE),
+	    .ep_info.pc = NS_BL1U_BASE,
+    },
+#endif
+#if SCP_BL2U_BASE
+    {
+	    .image_id = SCP_BL2U_IMAGE_ID,
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+		    VERSION_1, image_info_t, 0),
+	    .image_info.image_base = SCP_BL2U_BASE,
+	    .image_info.image_max_size = SCP_BL2U_LIMIT - SCP_BL2U_BASE,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+		    VERSION_1, entry_point_info_t, SECURE),
+    },
+#endif
+#if BL2U_BASE
+    {
+	    .image_id = BL2U_IMAGE_ID,
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+		    VERSION_1, image_info_t, 0),
+	    .image_info.image_base = BL2U_BASE,
+	    .image_info.image_max_size = BL2U_LIMIT - BL2U_BASE,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+		    VERSION_1, entry_point_info_t, SECURE | EXECUTABLE),
+	    .ep_info.pc = BL2U_BASE,
+    },
+#endif
+#if NS_BL2U_BASE
+    {
+	    .image_id = NS_BL2U_IMAGE_ID,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+		    VERSION_1, entry_point_info_t, NON_SECURE),
+    },
+#endif
+	    BL2_IMAGE_DESC,
+
+    {
+	    .image_id = INVALID_IMAGE_ID,
+    }
+};