feat(lfa): add relocatable code for BL31 live activation

This patch adds the relocatable code functions that are copied
out of the main BL31 image during a live activation and updates
the holding pen code to have a relocatable variant.

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

5 files changed, 186 insertions, 10 deletions

diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 3bde8fd09..b8b4dc85b 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -164,6 +164,15 @@ IMPORT_SYM(uintptr_t, __COHERENT_RAM_START__,	BL_COHERENT_RAM_BASE);
 IMPORT_SYM(uintptr_t, __COHERENT_RAM_END__,	BL_COHERENT_RAM_END);
 #endif
 
+#if ENABLE_LFA_BL31
+/* These symbols are needed so the relocatable code can be loaded. */
+IMPORT_SYM(uintptr_t, __LFA_RELOCATABLE_CODE_START__,	LFA_RELOCATABLE_CODE_START);
+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);
+#endif
+
 /*******************************************************************************
  * Structure used for telling the next BL how much of a particular type of
  * memory is available for its use and how much is already used.
diff --git a/include/services/lfa_holding_pen.h b/include/services/lfa_holding_pen.h
index 9420747ee..00473e898 100644
--- a/include/services/lfa_holding_pen.h
+++ b/include/services/lfa_holding_pen.h
@@ -16,4 +16,15 @@ bool lfa_holding_start(void);
 enum lfa_retc lfa_holding_wait(void);
 void lfa_holding_release(enum lfa_retc status);
 
+typedef uint64_t holding_lock_t;
+extern holding_lock_t lfa_r_holding_lock_var;
+extern uint64_t lfa_r_ep_addresses[PLATFORM_CORE_COUNT];
+extern uint64_t lfa_r_context_ids[PLATFORM_CORE_COUNT];
+
+void lfa_load_relocatable(void);
+void lfa_r_holding_lock(holding_lock_t *lock);
+void lfa_r_holding_unlock(holding_lock_t *lock);
+void lfa_r_holding_wait(holding_lock_t *lock);
+void lfa_r_holding_wait_warm_reset(holding_lock_t *lock);
+
 #endif
diff --git a/services/std_svc/lfa/lfa.mk b/services/std_svc/lfa/lfa.mk
index 53af69fd7..e9e78b71d 100644
--- a/services/std_svc/lfa/lfa.mk
+++ b/services/std_svc/lfa/lfa.mk
@@ -12,3 +12,7 @@ LFA_SOURCES	+=	$(addprefix services/std_svc/lfa/, \
 ifeq (${ENABLE_RMM}, 1)
 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
+endif
diff --git a/services/std_svc/lfa/lfa_holding_pen.c b/services/std_svc/lfa/lfa_holding_pen.c
index 9aa1e8c30..39eac41a8 100644
--- a/services/std_svc/lfa/lfa_holding_pen.c
+++ b/services/std_svc/lfa/lfa_holding_pen.c
@@ -7,10 +7,12 @@
 
 #include <string.h>
 
+#include <common/bl_common.h>
 #include <common/debug.h>
 #include <lib/psci/psci_lib.h>
 #include <lib/spinlock.h>
 #include <lib/utils_def.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 #include <services/lfa_holding_pen.h>
 
@@ -18,7 +20,7 @@
 
 static spinlock_t holding_lock;
 static spinlock_t activation_lock;
-static uint32_t activation_count;
+static volatile uint32_t activation_count;
 static enum lfa_retc activation_status;
 
 /**
@@ -44,28 +46,31 @@ static enum lfa_retc activation_status;
  */
 bool lfa_holding_start(void)
 {
-	bool status;
-	unsigned int no_of_cpus;
+	unsigned int no_of_cpus = psci_num_cpus_running_on_safe(plat_my_core_pos());
+	bool last_cpu;
 
+	/*
+	 * This lock ensures only one CPU uses the activation_count
+	 * variable at a time.
+	 */
 	spin_lock(&activation_lock);
 
+	/* First CPU to arrive locks the holding pen. */
 	if (activation_count == 0U) {
-		/* First CPU locks holding lock */
 		spin_lock(&holding_lock);
 	}
 
 	activation_count += 1U;
+	last_cpu = (activation_count == no_of_cpus);
 
-	no_of_cpus = psci_num_cpus_running_on_safe(plat_my_core_pos());
-	status = (activation_count == no_of_cpus);
-	if (!status) {
-		VERBOSE("Hold, %d CPU left\n",
-			 no_of_cpus - activation_count);
+	if (!last_cpu) {
+		VERBOSE("Hold, %d CPU left\n", no_of_cpus - activation_count);
 	}
 
+	/* Release the lock once count is updated. */
 	spin_unlock(&activation_lock);
 
-	return status;
+	return last_cpu;
 }
 
 /**
@@ -103,3 +108,42 @@ void lfa_holding_release(enum lfa_retc status)
 	activation_status = status;
 	spin_unlock(&holding_lock);
 }
+
+#if ENABLE_LFA_BL31
+void lfa_load_relocatable(void)
+{
+	int ret;
+
+	/*
+	 * The relocatable code area is outside of the normal BL31 image, so
+	 * before we can use it we have to make it writeable then copy the
+	 * relocatable code into it. Then once the copy is complete we must
+	 * mark it read only and executable again.
+	 */
+	ret = xlat_change_mem_attributes(LFA_RELOCATABLE_CODE_START,
+					 (LFA_RELOCATABLE_DATA_START - LFA_RELOCATABLE_CODE_START),
+					 MT_MEMORY | MT_RW | MT_EXECUTE_NEVER | EL3_PAS);
+	if (ret != 0) {
+		ERROR("LFA: Could not update memory attributes: %d\n", ret);
+		panic();
+	}
+
+	memcpy((void *)LFA_RELOCATABLE_CODE_START, (void *)LFA_RELOCATABLE_LMA,
+	       (uintptr_t)(LFA_RELOCATABLE_DATA_START - LFA_RELOCATABLE_CODE_START));
+
+	ret = xlat_change_mem_attributes(LFA_RELOCATABLE_CODE_START,
+					 (LFA_RELOCATABLE_DATA_START - LFA_RELOCATABLE_CODE_START),
+					 MT_MEMORY | MT_RO | MT_EXECUTE | EL3_PAS);
+	if (ret != 0) {
+		ERROR("LFA: Could not update memory attributes: %d\n", ret);
+		panic();
+	}
+
+	/*
+	 * The relocatable data section contains unknown information at this
+	 * point so zero it out before we use it.
+	 */
+	memset((void *)LFA_RELOCATABLE_DATA_START, 0,
+	       (LFA_RELOCATABLE_DATA_END - LFA_RELOCATABLE_DATA_START));
+}
+#endif /* ENABLE_LFA_BL31 */
diff --git a/services/std_svc/lfa/lfa_relocatable.S b/services/std_svc/lfa/lfa_relocatable.S
new file mode 100644
index 000000000..7436584bb
--- /dev/null
+++ b/services/std_svc/lfa/lfa_relocatable.S
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2026, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include <platform_def.h>
+
+/* Relocatable code section */
+
+	.globl  lfa_r_holding_lock
+	.globl  lfa_r_holding_wait
+	.globl  lfa_r_holding_unlock
+	.globl  lfa_r_holding_wait_warm_reset
+
+/*
+ * The 'rfunc' macro is a modified version of the 'func' macro in
+ * "include/common/asm_macros_common.S". It differs in that it does not change
+ * the section to ".text.asm". See the original version for comments.
+ */
+	.macro rfunc _name, _align=2
+	.cfi_sections .debug_frame
+	.section .lfa_relocatable_code.\_name, "ax"
+	.type \_name, %function
+	.cfi_startproc
+	.align \_align
+	\_name:
+#if ENABLE_BTI
+	 bti	jc
+#endif
+	.endm
+
+/*
+ * Enable holding lock.
+ *
+ * void lfa_r_holding_lock(spinlock_t *lock);
+ */
+rfunc lfa_r_holding_lock
+	mov	w1, #1
+	stlr	w1, [x0]
+	ret
+endfunc lfa_r_holding_lock
+
+/*
+ * Release hold.
+ *
+ * Use store-release to unconditionally clear the spinlock variable.
+ * Store operation generates an event to all cores waiting in WFE
+ * when address is monitored by the global monitor.
+ *
+ * void lfa_r_holding_unlock(spinlock_t *lock);
+ */
+rfunc lfa_r_holding_unlock
+	mov	w1, #0
+	stlr	w1, [x0]
+	ret
+endfunc lfa_r_holding_unlock
+
+/*
+ * Check lock using load-exclusive instruction.
+ *
+ * void lfa_r_holding_wait(spinlock_t *lock);
+ */
+rfunc lfa_r_holding_wait
+	sevl
+l1:	wfe
+	ldaxr	w1, [x0]
+	cbnz	w1, l1
+	ret
+endfunc lfa_r_holding_wait
+
+/*
+ * Check lock using load-exclusive instruction.
+ * Issue RMR_EL3 for warm reset if lock is unlocked
+ *
+ * void lfa_r_holding_wait(spinlock_t *lock);
+ */
+rfunc lfa_r_holding_wait_warm_reset
+	sevl
+l2:
+	wfe
+	ldaxr	w1, [x0]
+	cbnz	w1, l2
+l3:
+	wfi
+	b l3
+endfunc lfa_r_holding_wait_warm_reset
+
+/* Relocatable data section */
+
+	.globl  lfa_r_holding_lock_var
+	.globl  lfa_r_ep_addresses
+	.globl  lfa_r_context_ids
+
+.align 8
+.section .lfa_relocatable_data.lfa_r_holding_lock_var, "aw"
+lfa_r_holding_lock_var: .quad 0x0
+
+.align 8
+.section .lfa_relocatable_data.lfa_r_ep_addresses, "aw"
+lfa_r_ep_addresses:
+	.fill	PLATFORM_CORE_COUNT, 8, 0x0
+
+.align 8
+.section .lfa_relocatable_data.lfa_r_context_ids, "aw"
+lfa_r_context_ids:
+	.fill	PLATFORM_CORE_COUNT, 8, 0x0