feat(lfa): update LFA service to support BL31 live activation

This patch adds the bulk of the BL31 live activation service
framework including the warm reset handler.

Signed-off-by: John Powell <john.powell@arm.com>
Change-Id: Id9cbeeddbc023ee2540b88008e6c2502d816ee24

8 files changed, 631 insertions, 42 deletions

diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index 387d2066e..52255f263 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -16,6 +16,10 @@
 	.globl	bl31_entrypoint
 	.globl	bl31_warm_entrypoint
 
+#if ENABLE_LFA_BL31
+	.globl	bl31_lfa_entrypoint
+#endif
+
 	/* -----------------------------------------------------
 	 * bl31_entrypoint() is the cold boot entrypoint,
 	 * executed only by the primary cpu.
@@ -169,34 +173,7 @@ func bl31_warm_entrypoint
 		_exception_vectors=runtime_exceptions		\
 		_pie_fixup_size=0
 
-/*
- * We're about to enable MMU and participate in PSCI state coordination.
- *
- * The PSCI implementation invokes platform routines that enable CPUs to
- * participate in coherency. On a system where CPUs are not
- * cache-coherent without appropriate platform specific programming,
- * having caches enabled until such time might lead to coherency issues
- * (resulting from stale data getting speculatively fetched, among
- * others). Therefore we keep data caches disabled even after enabling
- * the MMU for such platforms.
- *
- * On systems with hardware-assisted coherency, or on single cluster
- * platforms, such platform specific programming is not required to
- * enter coherency (as CPUs already are); and there's no reason to have
- * caches disabled either.
- *
- * IMPORTANT: after invoking bl31_plat_enable_mmu(), the stack may end up
- * corrupted. Thus, when using this function, we must operate under the
- * assumption that we've no stack to use. Therefore, DO NOT place this
- * in another C function call or, generally, any place that would break
- * the aforementioned assumption.
- */
-#if HW_ASSISTED_COHERENCY || WARMBOOT_ENABLE_DCACHE_EARLY
-	mov     x0, xzr
-#else
-	mov     x0, #DISABLE_DCACHE
-#endif
-	bl      bl31_plat_enable_mmu
+	el3_enable_mmu
 
 	bl	bl31_warmboot
 
@@ -220,3 +197,35 @@ func bl31_warm_entrypoint
 #endif
 	b	el3_exit
 endfunc bl31_warm_entrypoint
+
+#if ENABLE_LFA_BL31
+	/*
+	 *---------------------------------------------------------------------
+	 * This CPU has been warm reset during a BL31 live firmware activation
+	 * --------------------------------------------------------------------
+	 */
+func bl31_lfa_entrypoint
+	/*
+	 * When a BL31 LFA update requests a warm reboot this is the entry
+	 * point used. We are trying to do as little as possible and just get
+	 * the new BL31 up and running as quickly as possible while restoring
+	 * the state of the system.
+	 */
+	el3_entrypoint_common					\
+		_init_sctlr=PROGRAMMABLE_RESET_ADDRESS		\
+		_warm_boot_mailbox=0				\
+		_secondary_cold_boot=0				\
+		_init_memory=0					\
+		_init_c_runtime=0				\
+		_exception_vectors=runtime_exceptions		\
+		_pie_fixup_size=0
+
+	el3_enable_mmu
+
+	/* Jump to C LFA warm reset entrypoint. */
+	bl	lfa_warm_reset_entrypoint
+
+	b	el3_exit
+endfunc bl31_lfa_entrypoint
+
+#endif /* ENABLE_LFA_BL31 */
diff --git a/include/arch/aarch64/el3_common_macros.S b/include/arch/aarch64/el3_common_macros.S
index 2181b731a..119cf5246 100644
--- a/include/arch/aarch64/el3_common_macros.S
+++ b/include/arch/aarch64/el3_common_macros.S
@@ -13,6 +13,7 @@
 #include <context.h>
 #include <lib/el3_runtime/cpu_data.h>
 #include <lib/per_cpu/per_cpu_macros.S>
+#include <lib/xlat_tables/xlat_mmu_helpers.h>
 
 	/*
 	 * Helper macro to initialise EL3 registers we care about.
@@ -576,4 +577,35 @@ feat_sctlr2_not_supported\@:
 		isb
 	.endm
 
+	.macro el3_enable_mmu
+	/*
+	 * We're about to enable MMU and participate in PSCI state coordination.
+	 *
+	 * The PSCI implementation invokes platform routines that enable CPUs to
+	 * participate in coherency. On a system where CPUs are not
+	 * cache-coherent without appropriate platform specific programming,
+	 * having caches enabled until such time might lead to coherency issues
+	 * (resulting from stale data getting speculatively fetched, among
+	 * others). Therefore we keep data caches disabled even after enabling
+	 * the MMU for such platforms.
+	 *
+	 * On systems with hardware-assisted coherency, or on single cluster
+	 * platforms, such platform specific programming is not required to
+	 * enter coherency (as CPUs already are); and there's no reason to have
+	 * caches disabled either.
+	 *
+	 * IMPORTANT: after invoking bl31_plat_enable_mmu(), the stack may end up
+	 * corrupted. Thus, when using this function, we must operate under the
+	 * assumption that we've no stack to use. Therefore, DO NOT place this
+	 * in another C function call or, generally, any place that would break
+	 * the aforementioned assumption.
+	 */
+#if HW_ASSISTED_COHERENCY || WARMBOOT_ENABLE_DCACHE_EARLY
+		mov     x0, xzr
+#else
+		mov     x0, #DISABLE_DCACHE
+#endif
+		bl      bl31_plat_enable_mmu
+	.endm
+
 #endif /* EL3_COMMON_MACROS_S */
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index b8b4dc85b..681a5885e 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2025, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2026, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -171,6 +171,10 @@ IMPORT_SYM(uintptr_t, __LFA_RELOCATABLE_CODE_END__,	LFA_RELOCATABLE_CODE_END);
 IMPORT_SYM(uintptr_t, __LFA_RELOCATABLE_LMA__,		LFA_RELOCATABLE_LMA);
 IMPORT_SYM(uintptr_t, __LFA_RELOCATABLE_DATA_START__,	LFA_RELOCATABLE_DATA_START);
 IMPORT_SYM(uintptr_t, __LFA_RELOCATABLE_DATA_END__,	LFA_RELOCATABLE_DATA_END);
+IMPORT_SYM(uintptr_t, __ERRATA_START__,			ERRATA_START);
+IMPORT_SYM(uintptr_t, __ERRATA_END__,			ERRATA_END);
+IMPORT_SYM(uintptr_t, __CPU_OPS_START__,		CPU_OPS_START);
+IMPORT_SYM(uintptr_t, __CPU_OPS_END__,			CPU_OPS_END);
 #endif
 
 /*******************************************************************************
diff --git a/include/plat/common/plat_lfa.h b/include/plat/common/plat_lfa.h
index ad733b75d..3cb23a4dc 100644
--- a/include/plat/common/plat_lfa.h
+++ b/include/plat/common/plat_lfa.h
@@ -10,6 +10,15 @@
 #include <services/lfa_component_desc.h>
 #include <tools_share/uuid.h>
 
+/*
+ * This data structure contains component info retrieved from the platform-
+ * implemented API plat_lfa_get_components.
+ *
+ * lfa_component_id    Component described by the data structure.
+ * uuid                UUID of this component.
+ * activator           Activator data structure used by this component.
+ * activation_pending  Flag indicating whether this component can be activated.
+ */
 typedef struct plat_lfa_component_info {
 	const uint32_t lfa_component_id;
 	const uuid_t uuid;
@@ -21,6 +30,9 @@ uint32_t plat_lfa_get_components(plat_lfa_component_info_t **components);
 bool is_plat_lfa_activation_pending(uint32_t lfa_component_id);
 int plat_lfa_cancel(uint32_t lfa_component_id);
 int plat_lfa_load_auth_image(uint32_t lfa_component_id);
+int plat_lfa_get_image_info(uint32_t lfa_component_id, uintptr_t *image_address,
+			    size_t *image_size);
 int plat_lfa_notify_activate(uint32_t lfa_component_id);
+uint64_t plat_lfa_mailbox_base(void);
 
 #endif /* PLAT_LFA_H */
diff --git a/make_helpers/constraints.mk b/make_helpers/constraints.mk
index a11603f33..5a828c0cd 100644
--- a/make_helpers/constraints.mk
+++ b/make_helpers/constraints.mk
@@ -62,8 +62,10 @@ endif
 ifeq (${CTX_INCLUDE_EL2_REGS}, 1)
 	ifeq (${SPD},none)
 		ifeq (${ENABLE_RMM},0)
-                        $(error CTX_INCLUDE_EL2_REGS is available only when SPD \
-                        or RMM is enabled)
+                        ifeq (${ENABLE_LFA_BL31},0)
+                                $(error CTX_INCLUDE_EL2_REGS is available only when SPD \
+                                RMM, or BL31 LFA is enabled)
+                        endif
 		endif
 	endif
 endif
diff --git a/services/std_svc/lfa/bl31_lfa.c b/services/std_svc/lfa/bl31_lfa.c
index 2a75065e1..cb6f42e0b 100644
--- a/services/std_svc/lfa/bl31_lfa.c
+++ b/services/std_svc/lfa/bl31_lfa.c
@@ -1,28 +1,258 @@
 /*
- * Copyright (c) 2025, Arm Limited. All rights reserved.
+ * Copyright (c) 2025-2026, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
+#include <errno.h>
+
+#include <common/build_message.h>
+#include <common/debug.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
 #include <services/bl31_lfa.h>
+#include <services/lfa_holding_pen.h>
 #include <services/lfa_svc.h>
 
-static int lfa_bl31_prime(struct lfa_component_status *activation)
+static const uintptr_t TEXT_START = (uintptr_t)__TEXT_START__;
+
+/* Static function prototypes. */
+static int lfa_bl31_check(uintptr_t base_va, size_t size, uint64_t **entry);
+static int lfa_r_bl31_copy(uintptr_t va, uint64_t *entry, void *src, size_t size, bool reset);
+
+static inline size_t lfa_bl31_errata_size(void)
+{
+	return ERRATA_END - ERRATA_START;
+}
+
+static inline size_t lfa_bl31_cpu_ops_size(void)
+{
+	return CPU_OPS_END - CPU_OPS_START;
+}
+
+static int lfa_bl31_patch_region(struct lfa_component_status *activation,
+				 uintptr_t dst_va, size_t size, bool reset)
+{
+	uint64_t *entry = NULL;
+	size_t src_offset;
+	uintptr_t src_va;
+	uintptr_t patch_start;
+	uintptr_t patch_end;
+	size_t patch_size;
+
+	patch_start = dst_va & ~(PAGE_SIZE - 1U);
+	patch_end = (dst_va + size + PAGE_SIZE - 1U) & ~(PAGE_SIZE - 1U);
+	patch_size = patch_end - patch_start;
+
+	if ((patch_start < TEXT_START) ||
+	    ((patch_size > activation->image_size) &&
+	    (activation->image_size != 0))) {
+		ERROR("BL31 LFA: invalid patch region dst=0x%lx size=0x%zx image_size=0x%lx\n",
+		      dst_va, size, activation->image_size);
+		return LFA_CRITICAL_ERROR;
+	}
+
+	src_offset = patch_start - TEXT_START;
+	if ((activation->image_size != 0) &&
+	    ((src_offset > activation->image_size) ||
+	    (patch_size > (activation->image_size - src_offset)))) {
+		ERROR("BL31 LFA: staged image too small for patch region 0x%lx size 0x%zx "
+		      "aligned_start 0x%lx aligned_size 0x%zx\n",
+		      dst_va, size, patch_start, patch_size);
+		return LFA_CRITICAL_ERROR;
+	}
+
+	src_va = activation->image_address + src_offset;
+
+	if (lfa_bl31_check(patch_start, patch_size, &entry) != 0) {
+		ERROR("BL31 LFA: xlat check failed for patch region dst=0x%lx size=0x%zx "
+		      "aligned_start 0x%lx aligned_size 0x%zx\n",
+		      dst_va, size, patch_start, patch_size);
+		return LFA_CRITICAL_ERROR;
+	}
+
+	lfa_r_bl31_copy(patch_start, entry, (void *)src_va, patch_size, reset);
+	return LFA_SUCCESS;
+}
+
+int32_t lfa_bl31_prime(struct lfa_component_status *activation)
+{
+	// TODO: Check that activation->image_size is not too large.
+	return LFA_SUCCESS;
+}
+
+/* Function that actually performs the activation of the firmware image */
+static int lfa_bl31_image_activation(struct lfa_component_status *activation,
+	uint64_t ep_address, uint64_t context_id)
+{
+	VERBOSE("BL31 LFA: Starting BL31 Image Activation (core %d)\n", plat_my_core_pos());
+	VERBOSE("  Address: 0x%lX\n", activation->image_address);
+	VERBOSE("  Size:    %ld bytes\n", activation->image_size);
+	VERBOSE("  Reset:   %s\n", activation->reset ? "yes" : "no");
+
+#if !ENABLE_RUNTIME_INSTRUMENTATION
+	/*
+	 * Used to avoid console corruption. Not used if measuring performance
+	 * since printouts are removed.
+	 */
+	console_flush();
+#endif
+
+	/* Prepare for warm reset and setup NS entrypoint for primary cores */
+	if (activation->reset) {
+		if (prepare_warm_reset(ep_address, context_id,
+				(uint64_t)&lfa_r_holding_lock_var) != LFA_SUCCESS) {
+			lfa_r_holding_unlock(&lfa_r_holding_lock_var);
+			return LFA_CRITICAL_ERROR;
+		}
+	}
+
+	if (lfa_bl31_patch_region(activation, CPU_OPS_START,
+				  lfa_bl31_cpu_ops_size(), false) != LFA_SUCCESS) {
+		return LFA_CRITICAL_ERROR;
+	}
+
+	/*
+	 * Patch the errata section last so reset, if requested, is triggered
+	 * only after cpu_ops has been updated to point at the new errata code.
+	 */
+	return lfa_bl31_patch_region(activation, ERRATA_START,
+				      lfa_bl31_errata_size(),
+				      activation->reset);
+}
+
+int32_t lfa_bl31_activate(struct lfa_component_status *activation,
+	uint64_t ep_address, uint64_t context_id)
 {
-	return LFA_WRONG_STATE;
+	uint32_t core_pos = plat_my_core_pos();
+	int ret = LFA_SUCCESS;
+
+	lfa_r_ep_addresses[core_pos] = ep_address;
+	lfa_r_context_ids[core_pos] = context_id;
+
+	/* Case when Firmware requires CPU Rendezvous for LFA Activation */
+	if (activation->cpu_rendezvous) {
+		/*
+		 * Only one core will return true from this function and it
+		 * controls the activation process.
+		 */
+		if (lfa_holding_start()) {
+			VERBOSE("BL31 LFA: CPU %d is leader core\n", core_pos);
+
+			/*
+			 * Before we release other cores from the holding pen
+			 * try to get the image info. That way if this fails the
+			 * system will still be in a recoverable state.
+			 */
+			ret = plat_lfa_get_image_info(activation->component_id,
+						      &activation->image_address,
+						      &activation->image_size);
+			if (ret) {
+				ERROR("BL31 LFA: Could not get image info!\n");
+				lfa_holding_release(LFA_CRITICAL_ERROR);
+				return LFA_CRITICAL_ERROR;
+			}
+
+			/*
+			 * Only one core will load the relocatable code module and
+			 * acquire the relocatable lock, then release the rest of the
+			 * cores which will then wait for us to release the relocatable
+			 * lock once the activation is complete.
+			 */
+			lfa_load_relocatable();
+			lfa_r_holding_lock(&lfa_r_holding_lock_var);
+			lfa_holding_release(LFA_SUCCESS);
+
+			/*
+			 * Perform image activation. Will not return if reset
+			 * requested.
+			 */
+			ret = lfa_bl31_image_activation(activation, ep_address, context_id);
+
+			/* Unlock holding pen for all other cores */
+			lfa_r_holding_unlock(&lfa_r_holding_lock_var);
+
+			if (ret) {
+				ERROR("BL31 LFA: failed image activation with error: %d\n", ret);
+				return ret;
+			}
+
+			INFO("BL31 LFA: Successful image activation!\n");
+		} else {
+			/* Wait until release from rendezvous holding pen. */
+			ret = lfa_holding_wait();
+			if (ret != LFA_SUCCESS) {
+				return ret;
+			}
+
+			if (activation->reset) {
+				/*
+				 * Prepare for warm reset and setup NS
+				 * entrypoint for secondary cores
+				 */
+				if (prepare_warm_reset(ep_address, context_id,
+						(uint64_t)&lfa_r_holding_lock_var) != LFA_SUCCESS) {
+					return LFA_CRITICAL_ERROR;
+				}
+
+				/*
+				 * Wait until activation of the image is
+				 * complete and warm reset occurs on main core
+				 * then reset.
+				 */
+				lfa_r_holding_wait_warm_reset(&lfa_r_holding_lock_var);
+			} else {
+				/*
+				 * Wait until main core is completed with
+				 * activation
+				 */
+				lfa_r_holding_wait(&lfa_r_holding_lock_var);
+			}
+		}
+	}
+
+	/*
+	 * No CPU rendezvous required. Perform LFA Activate with single core
+	 * NOTE: Assumption that only one core will call activate with
+	 * CPU_RENDEZVOUS=0
+	 */
+	else {
+		/*
+		 * Loading relocatable module normally happens in the holding
+		 * pen. Since there is no holding pen without CPU rendezvous we
+		 * are doing it here instead.
+		 */
+		lfa_load_relocatable();
+
+		/* Perform actual image activation */
+		ret = lfa_bl31_image_activation(activation, ep_address, context_id);
+		if (ret) {
+			ERROR("Failed image activation with error: %d\n", ret);
+			return ret;
+		}
+	}
+
+	NOTICE("BL31 Live Activation Without Reset %u\n", plat_my_core_pos());
+	NOTICE("  Version : %s\n", build_version_string);
+	NOTICE("  %s\n", build_message);
+
+	return ret;
 }
 
-static int lfa_bl31_activate(struct lfa_component_status *activation,
-		uint64_t ep_address,
-		uint64_t context_id)
+int32_t lfa_bl31_cancel(struct lfa_component_status *activation)
 {
-	return LFA_WRONG_STATE;
+	/* Let CPUs out of the holding pen if needed. */
+	lfa_holding_release(LFA_ACTIVATION_FAILED);
+
+	return LFA_SUCCESS;
 }
 
-static struct lfa_component_ops bl31_activator = {
+struct lfa_component_ops bl31_activator = {
 	.prime = lfa_bl31_prime,
 	.activate = lfa_bl31_activate,
-	.may_reset_cpu = false,
+	.cancel = lfa_bl31_cancel,
+	.may_reset_cpu = true,
 	.cpu_rendezvous_required = true,
 };
 
@@ -30,3 +260,128 @@ struct lfa_component_ops *get_bl31_activator(void)
 {
 	return &bl31_activator;
 }
+
+uint64_t *lfa_get_xlat_table_entry(uintptr_t virtual_addr, const xlat_ctx_t *ctx)
+{
+	uint64_t *table = ctx->base_table;
+	unsigned int entries = ctx->base_table_entries;
+	unsigned int start_level = GET_XLAT_TABLE_LEVEL_BASE((unsigned long long)ctx->va_max_address + 1ULL);
+
+	for (unsigned int level = start_level; level <= XLAT_TABLE_LEVEL_MAX; level++) {
+		uint64_t idx, desc, desc_type;
+
+		idx = XLAT_TABLE_IDX(virtual_addr, level);
+		if (idx >= entries) {
+			return NULL;
+		}
+
+		desc = table[idx];
+		desc_type = desc & DESC_MASK;
+
+		if (desc_type == INVALID_DESC) {
+			return NULL;
+		}
+
+		if (level == XLAT_TABLE_LEVEL_MAX) {
+			/*
+			 * Only page descriptors allowed at the final lookup
+			 * level.
+			 */
+			assert(desc_type == PAGE_DESC);
+			return &table[idx];
+		}
+
+		if (desc_type == BLOCK_DESC) {
+			return &table[idx];
+		}
+
+		assert(desc_type == TABLE_DESC);
+		table = (uint64_t *)(uintptr_t)(desc & TABLE_ADDR_MASK);
+		entries = XLAT_TABLE_ENTRIES;
+	}
+
+	return NULL;
+}
+
+int lfa_bl31_check(uintptr_t base_va, size_t size, uint64_t **entry)
+{
+	const xlat_ctx_t *ctx = get_xlat_tables();
+
+	assert(ctx != NULL);
+	assert(ctx->initialized);
+
+	if (!IS_PAGE_ALIGNED(base_va)) {
+		WARN("%s: Address 0x%lx is not aligned on a page boundary.\n", __func__, base_va);
+		return -EINVAL;
+	}
+
+	if (size == 0U) {
+		WARN("%s: Size is 0.\n", __func__);
+		return -EINVAL;
+	}
+
+	size_t pages_count = size / PAGE_SIZE;
+
+	VERBOSE("BL31 update will impact %zu pages starting from address 0x%lx...\n",
+		pages_count, base_va);
+
+	*entry = lfa_get_xlat_table_entry(base_va, ctx);
+
+	uint64_t *last = lfa_get_xlat_table_entry(base_va + (pages_count - 1) * PAGE_SIZE, ctx);
+
+	/* Check that all descriptors are in the same page. */
+	if ((((uint64_t)*entry) >> 12) != ((((uint64_t)last) >> 12))) {
+		ERROR("All page descriptors are not part of the same page.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+__attribute__((noinline, section(".lfa_relocatable_code")))
+int lfa_r_bl31_copy(uintptr_t base_va, uint64_t *entry, void *src, size_t size, bool reset)
+{
+	do {
+		uint64_t desc = *entry;
+		size_t count = MIN(PAGE_SIZE, size) / sizeof(uint64_t);
+
+		*entry = INVALID_DESC;
+		dsbishst();
+		tlbivae3is(TLBI_ADDR(base_va));
+		dsbish();
+		isb();
+		*entry = (desc & ~LOWER_ATTRS(AP_RO));
+		dsbish();
+
+		for (uint64_t i = 0; i < count; i++) {
+			((uint64_t *) base_va)[i] = ((uint64_t *) src)[i];
+		}
+		if (size <= PAGE_SIZE) {
+			for (uint64_t i = count * sizeof(uint64_t); i < size; i++) {
+				((uint8_t *) base_va)[i] = ((uint8_t *) src)[i];
+			}
+			size = 0;
+		} else {
+			size -= PAGE_SIZE;
+		}
+
+		*entry = INVALID_DESC;
+		dsbishst();
+		tlbivae3is(TLBI_ADDR(base_va));
+		dsbish();
+		isb();
+		*entry = desc;
+
+		base_va += PAGE_SIZE;
+		src += PAGE_SIZE;
+		entry++;
+	} while (size > 0);
+
+	dsbish();
+
+	if (reset) {
+		wfi();
+	}
+
+	return 0;
+}
diff --git a/services/std_svc/lfa/lfa.mk b/services/std_svc/lfa/lfa.mk
index e9e78b71d..d3d5cd864 100644
--- a/services/std_svc/lfa/lfa.mk
+++ b/services/std_svc/lfa/lfa.mk
@@ -6,7 +6,6 @@
 
 LFA_SOURCES	+=	$(addprefix services/std_svc/lfa/, \
 			  lfa_main.c \
-			  bl31_lfa.c \
 			  lfa_holding_pen.c)
 
 ifeq (${ENABLE_RMM}, 1)
@@ -14,5 +13,8 @@ LFA_SOURCES	+=	services/std_svc/rmmd/rmmd_rmm_lfa.c
 endif
 
 ifeq (${ENABLE_LFA_BL31}, 1)
-LFA_SOURCES	+=	services/std_svc/lfa/lfa_relocatable.S
+LFA_SOURCES	+=	$(addprefix services/std_svc/lfa/, \
+			  bl31_lfa.c \
+			  lfa_relocatable.S \
+			  lfa_reset.c)
 endif
diff --git a/services/std_svc/lfa/lfa_reset.c b/services/std_svc/lfa/lfa_reset.c
new file mode 100644
index 000000000..07db8cc97
--- /dev/null
+++ b/services/std_svc/lfa/lfa_reset.c
@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2026, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/build_message.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/psci/psci.h>
+#include <lib/spinlock.h>
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+#include <services/lfa_svc.h>
+
+/*
+ * These variables are used pre and post live activation so place them in the
+ * relocatable data section to ensure they don't get moved around and persist
+ * across live activation.
+ */
+static lfa_mailbox_message_t lfa_mailbox[PLATFORM_CORE_COUNT] __attribute__((section(".lfa_relocatable_data")));
+static const plat_psci_ops_t *lfa_psci_ops __attribute__((section(".lfa_relocatable_data")));
+static uintptr_t plat_wr_ep __attribute__((section(".lfa_relocatable_data")));
+static uint64_t reset_cpu_count __attribute__((section(".lfa_relocatable_data")));
+static spinlock_t cpucountlock __attribute__((section(".lfa_relocatable_data")));
+
+extern plat_psci_ops_t *psci_plat_pm_ops;
+extern int psci_validate_entry_point(entry_point_info_t *ep,
+			      uintptr_t entrypoint,
+			      u_register_t context_id);
+extern void bl31_lfa_entrypoint(void);
+extern void bl31_warm_entrypoint(void);
+
+static void add_reset_cpu(void)
+{
+	spin_lock(&cpucountlock);
+	reset_cpu_count++;
+	spin_unlock(&cpucountlock);
+}
+
+static uint64_t remove_reset_cpu(void)
+{
+	uint64_t cpu_count;
+
+	spin_lock(&cpucountlock);
+	if (reset_cpu_count > 0) {
+		reset_cpu_count--;
+	}
+	cpu_count = reset_cpu_count;
+	spin_unlock(&cpucountlock);
+
+	return cpu_count;
+}
+
+/*
+ * Prepare CPU warm reset. This functioned should be called before entering
+ * relocatable code for each CPU.
+ */
+uint64_t prepare_warm_reset(uintptr_t lfa_ns_ep, uint64_t context_id, uint64_t cpu_spinlock)
+{
+	const psci_power_state_t state_info = { {ARM_LOCAL_STATE_OFF} };
+	unsigned int core_pos = plat_my_core_pos();
+	lfa_mailbox_message_t mmesg;
+	int rc;
+	static spinlock_t setuplock;
+
+	spin_lock(&setuplock);
+	if (plat_wr_ep == 0 && plat_lfa_mailbox_base()) {
+		plat_wr_ep = *((uintptr_t *)plat_lfa_mailbox_base());
+	}
+
+	if (lfa_psci_ops == NULL) {
+		plat_setup_psci_ops((uintptr_t)bl31_lfa_entrypoint, &lfa_psci_ops);
+	}
+	spin_unlock(&setuplock);
+
+	rc = psci_validate_entry_point(&mmesg.ep, lfa_ns_ep, (u_register_t)context_id);
+	if (rc != PSCI_E_SUCCESS) {
+		ERROR("BL31 LFA: Invalid NS entrypoint! (error %d)\n", rc);
+		return LFA_CRITICAL_ERROR;
+	}
+
+	/* Stash the holding pen lock so we can release it after reboot. */
+	mmesg.spinlock_p = (uint64_t)cpu_spinlock;
+
+	/* Stash the platform warmboot entry point so we can restore it. */
+	mmesg.plat_wr_mailbox = plat_wr_ep;
+	memcpy((void *)&lfa_mailbox[core_pos], &mmesg, sizeof(lfa_mailbox_message_t));
+
+	/*
+	 * TODO: refactor PSCI suspend API to be usable here. The main issue is that
+	 * PSCI suspend will put the core down immediately, but the use case here is
+	 * slightly different since we have to do the firmware copy operation at the
+	 * very end right before putting the core down for reset.
+	 */
+	if (lfa_psci_ops->pwr_domain_suspend != NULL) {
+		/*
+		 * We can't use the proper PSCI API since it will put the core
+		 * down immediately, we still need to copy BL31.
+		 */
+#if USE_GIC_DRIVER
+		gic_cpuif_disable(core_pos);
+#endif
+		lfa_psci_ops->pwr_domain_suspend(&state_info);
+	} else {
+		ERROR("BL31: PSCI power domain suspend not supported!\n");
+		return LFA_CRITICAL_ERROR;
+	}
+
+	add_reset_cpu();
+
+	cm_el2_sysregs_context_save(NON_SECURE);
+
+	return LFA_SUCCESS;
+}
+
+/*
+ * Entrypoint function for CPU warm reset case.
+ */
+void __no_pauth lfa_warm_reset_entrypoint(void)
+{
+	static spinlock_t resetlock;
+	uint32_t counter_freq;
+	const psci_power_state_t state_info = { {ARM_LOCAL_STATE_OFF} };
+	lfa_mailbox_message_t mmesg;
+	unsigned int core_pos = plat_my_core_pos();
+
+#if ENABLE_FEAT_RME
+	/*
+	 * At warm boot GPT data structures have already been initialized in RAM
+	 * but the sysregs for this CPU need to be initialized. Note that the GPT
+	 * accesses are controlled attributes in GPCCR and do not depend on the
+	 * SCR_EL3.C bit.
+	 */
+	if (gpt_enable() != 0) {
+		panic();
+	}
+#endif
+
+	/* TODO see if we can use psci_warmboot_entrypoint here. */
+	psci_plat_pm_ops->pwr_domain_suspend_finish(&state_info);
+#if USE_GIC_DRIVER
+	gic_cpuif_enable(core_pos);
+#endif
+
+	/* Init registers that never change for the lifetime of TF-A */
+	cm_manage_extensions_el3(core_pos);
+
+	/* Re-init the cntfrq_el0 register */
+	counter_freq = plat_get_syscnt_freq2();
+	write_cntfrq_el0(counter_freq);
+
+	memcpy(&mmesg, (void *)&lfa_mailbox[core_pos], sizeof(lfa_mailbox_message_t));
+
+	spin_lock(&resetlock);
+	NOTICE("BL31 LFA Warm Reset on CPU %u\n", core_pos);
+	NOTICE("  Version : %s\n", build_version_string);
+	NOTICE("  %s\n", build_message);
+	console_flush();
+	spin_unlock(&resetlock);
+
+	/* The last CPU to reach here restores the warm entrypoint. */
+	if (remove_reset_cpu() == 0) {
+		plat_arm_program_trusted_mailbox(mmesg.plat_wr_mailbox);
+		plat_wr_ep = 0;
+		lfa_psci_ops = NULL;
+	}
+
+	spin_unlock((spinlock_t *)mmesg.spinlock_p);
+
+	cm_init_my_context(&mmesg.ep);
+
+	cm_prepare_el3_exit_ns();
+}