diff options
| -rw-r--r-- | bl31/aarch64/bl31_entrypoint.S | 65 | ||||
| -rw-r--r-- | include/arch/aarch64/el3_common_macros.S | 32 | ||||
| -rw-r--r-- | include/common/bl_common.h | 6 | ||||
| -rw-r--r-- | include/plat/common/plat_lfa.h | 12 | ||||
| -rw-r--r-- | make_helpers/constraints.mk | 6 | ||||
| -rw-r--r-- | services/std_svc/lfa/bl31_lfa.c | 373 | ||||
| -rw-r--r-- | services/std_svc/lfa/lfa.mk | 6 | ||||
| -rw-r--r-- | services/std_svc/lfa/lfa_reset.c | 173 |
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(); +} |