From 7ee4db6e4720ebb45a7c20da06c06091ee95c298 Mon Sep 17 00:00:00 2001 From: Oliver Swede Date: Mon, 2 Dec 2019 13:21:52 +0000 Subject: [PATCH] plat/arm/board/arm_fpga: Add PSCI implementation for FPGA images This adds a basic PSCI implementation allow secondary CPUs to be released from an initial state and continue through to the warm boot entrypoint. Each secondary CPU is kept in a holding pen, whereby it polls the value representing its hold state, by reading this from an array that acts as a table for all the PEs. The hold states are initially set to 0 for all cores to indicate that the executing core should continue polling. To prevent the secondary CPUs from interfering with the platform's initialization, they are only updated by the primary CPU once the cold boot sequence has completed and fpga_pwr_domain_on(mpidr) is called. The polling target CPU will then read 1 (which indicates that it should branch to the warm reset entrypoint) and then jump to that address rather than continue polling. In addition to the initial polling behaviour of the secondary CPUs before their warm boot reset sequence, they are also placed in a low-power wfe() state at the end of each poll; accordingly, the PSCI fpga_pwr_domain_on(mpidr) function also signals an event to all cores (after updating the target CPU's hold entry) to wake them from this state, allowing any secondary CPUs that are still polling to check their hold state again. This method is in accordance with both the PSCI and Linux kernel recommendations, as the lessened overhead reduces the energy consumption associated with the busy-loop. The table of hold entries is implemented by a global array as shared SRAM (which is used by other platforms in similar implementations) is not available on the FPGA images. Signed-off-by: Oliver Swede Change-Id: I65cfd1892f8be1dfcb285f0e1e94e7a9870cdf5a --- .../arm/board/arm_fpga/aarch64/fpga_helpers.S | 46 +++++++++-- plat/arm/board/arm_fpga/fpga_pm.c | 78 ++++++++++++++++++- plat/arm/board/arm_fpga/fpga_topology.c | 54 ++++++++++++- .../arm/board/arm_fpga/include/platform_def.h | 4 + 4 files changed, 170 insertions(+), 12 deletions(-) diff --git a/plat/arm/board/arm_fpga/aarch64/fpga_helpers.S b/plat/arm/board/arm_fpga/aarch64/fpga_helpers.S index 57e5320bb..f350455d5 100644 --- a/plat/arm/board/arm_fpga/aarch64/fpga_helpers.S +++ b/plat/arm/board/arm_fpga/aarch64/fpga_helpers.S @@ -20,8 +20,8 @@ .globl plat_crash_console_flush /* ----------------------------------------------------------------------- - * unsigned long plat_get_my_entrypoint (void); - * TODO: determine if warm boot should be supported for FPGA images + * Indicate a cold boot for every CPU - warm boot is unsupported for the + * holding pen PSCI implementation. * ----------------------------------------------------------------------- */ func plat_get_my_entrypoint @@ -31,11 +31,26 @@ endfunc plat_get_my_entrypoint /* ----------------------------------------------------------------------- * void plat_secondary_cold_boot_setup (void); - * TODO: add placeholder PSCI implementation for FPGA images * ----------------------------------------------------------------------- */ func plat_secondary_cold_boot_setup - ret + /* + * Poll the CPU's hold entry until it indicates to jump + * to the entrypoint address. + */ + bl plat_my_core_pos + lsl x0, x0, #PLAT_FPGA_HOLD_ENTRY_SHIFT + ldr x1, =hold_base + ldr x2, =fpga_sec_entrypoint +poll_hold_entry: + ldr x3, [x1, x0] + cmp x3, #PLAT_FPGA_HOLD_STATE_GO + b.ne 1f + ldr x3, [x2] + br x3 +1: + wfe + b poll_hold_entry endfunc plat_secondary_cold_boot_setup /* ----------------------------------------------------------------------- @@ -64,11 +79,30 @@ endfunc plat_my_core_pos /* ----------------------------------------------------------------------- * unsigned int plat_fpga_calc_core_pos(u_register_t mpidr) - * TODO: add calculation of the core position for FPGA image CPUs * ----------------------------------------------------------------------- */ func plat_fpga_calc_core_pos - mov x0, #0 + /* + * Check for MT bit in MPIDR, which may be either value for images + * running on the FPGA. + * + * If not set, shift MPIDR to left to make it look as if in a + * multi-threaded implementation. + */ + tst x0, #MPIDR_MT_MASK + lsl x3, x0, #MPIDR_AFFINITY_BITS + csel x3, x3, x0, eq + + /* Extract individual affinity fields from MPIDR */ + ubfx x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS + ubfx x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS + ubfx x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS + + /* Compute linear position */ + mov x4, #FPGA_MAX_CPUS_PER_CLUSTER + madd x1, x2, x4, x1 + mov x5, #FPGA_MAX_PE_PER_CPU + madd x0, x1, x5, x0 ret endfunc plat_fpga_calc_core_pos diff --git a/plat/arm/board/arm_fpga/fpga_pm.c b/plat/arm/board/arm_fpga/fpga_pm.c index dc95028da..a734e1d8c 100644 --- a/plat/arm/board/arm_fpga/fpga_pm.c +++ b/plat/arm/board/arm_fpga/fpga_pm.c @@ -4,12 +4,86 @@ * SPDX-License-Identifier: BSD-3-Clause */ -#include +#include -/* TODO: add PSCI implementation */ +#include +#include +#include +#include + +/* + * This is a basic PSCI implementation that allows secondary CPUs to be + * released from their initial state and continue to the warm boot entrypoint. + * + * The secondary CPUs are placed in a holding pen and released by calls + * to fpga_pwr_domain_on(mpidr), which updates the hold entry for the CPU + * specified by the mpidr argument - the (polling) target CPU will then branch + * to the BL31 warm boot sequence at the entrypoint address. + * + * Additionally, the secondary CPUs are kept in a low-power wfe() state + * (placed there at the end of each poll) and woken when necessary through + * calls to sev() in fpga_pwr_domain_on(mpidr), once the hold state for the + * relevant CPU has been updated. + * + * Hotplug is currently implemented using a wfi-loop, which removes the + * dependencies on any power controllers or other mechanism that is specific + * to the running system as specified by the FPGA image. + */ + +uint64_t hold_base[PLATFORM_CORE_COUNT]; +uintptr_t fpga_sec_entrypoint; + +/* + * Calls to the CPU specified by the mpidr will set its hold entry to a value + * indicating that it should stop polling and branch off to the warm entrypoint. + */ +static int fpga_pwr_domain_on(u_register_t mpidr) +{ + unsigned int pos = plat_core_pos_by_mpidr(mpidr); + unsigned long current_mpidr = read_mpidr_el1(); + + if (mpidr == current_mpidr) { + return PSCI_E_ALREADY_ON; + } + hold_base[pos] = PLAT_FPGA_HOLD_STATE_GO; + flush_dcache_range((uintptr_t)&hold_base[pos], sizeof(uint64_t)); + sev(); /* Wake any CPUs from wfe */ + + return PSCI_E_SUCCESS; +} + +static void fpga_pwr_domain_off(const psci_power_state_t *target_state) +{ + while (1) { + wfi(); + } +} + +static void fpga_cpu_standby(plat_local_state_t cpu_state) +{ + /* + * Enter standby state + * dsb is good practice before using wfi to enter low power states + */ + u_register_t scr = read_scr_el3(); + write_scr_el3(scr|SCR_IRQ_BIT); + dsb(); + wfi(); + write_scr_el3(scr); +} + +plat_psci_ops_t plat_fpga_psci_pm_ops = { + .pwr_domain_on = fpga_pwr_domain_on, + .pwr_domain_off = fpga_pwr_domain_off, + .cpu_standby = fpga_cpu_standby +}; int plat_setup_psci_ops(uintptr_t sec_entrypoint, const plat_psci_ops_t **psci_ops) { + fpga_sec_entrypoint = sec_entrypoint; + flush_dcache_range((uint64_t)&fpga_sec_entrypoint, + sizeof(fpga_sec_entrypoint)); + *psci_ops = &plat_fpga_psci_pm_ops; return 0; } diff --git a/plat/arm/board/arm_fpga/fpga_topology.c b/plat/arm/board/arm_fpga/fpga_topology.c index 5458376b5..a705429ac 100644 --- a/plat/arm/board/arm_fpga/fpga_topology.c +++ b/plat/arm/board/arm_fpga/fpga_topology.c @@ -9,17 +9,63 @@ #include "fpga_private.h" #include +static unsigned char fpga_power_domain_tree_desc[FPGA_MAX_CLUSTER_COUNT + 2]; + const unsigned char *plat_get_power_domain_tree_desc(void) { - /* TODO: add description of power domain topology and PSCI implementation */ - return NULL; + int i; + /* + * The highest level is the system level. The next level is constituted + * by clusters and then cores in clusters. + * + * This description of the power domain topology is aligned with the CPU + * indices returned by the plat_core_pos_by_mpidr() and plat_my_core_pos() + * APIs. + */ + fpga_power_domain_tree_desc[0] = 1; + fpga_power_domain_tree_desc[1] = FPGA_MAX_CLUSTER_COUNT; + + for (i = 0; i < FPGA_MAX_CLUSTER_COUNT; i++) { + fpga_power_domain_tree_desc[i + 2] = FPGA_MAX_CPUS_PER_CLUSTER; + } + + return fpga_power_domain_tree_desc; } int plat_core_pos_by_mpidr(u_register_t mpidr) { + unsigned int cluster_id, cpu_id, thread_id; + + mpidr &= MPIDR_AFFINITY_MASK; + if (mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)) { + return -1; + } + + if (mpidr & MPIDR_MT_MASK) { + thread_id = MPIDR_AFFLVL0_VAL(mpidr); + } else { + thread_id = 0; + } + + cpu_id = MPIDR_AFFLVL1_VAL(mpidr); + cluster_id = MPIDR_AFFLVL2_VAL(mpidr); + + if (cluster_id >= FPGA_MAX_CLUSTER_COUNT) { + return -1; + } else if (cpu_id >= FPGA_MAX_CPUS_PER_CLUSTER) { + return -1; + } else if (thread_id >= FPGA_MAX_PE_PER_CPU) { + return -1; + } + /* - * TODO: calculate core position in a way that accounts for CPUs that - * potentially implement multithreading + * The image running on the FPGA may or may not implement multithreading, + * and it shouldn't be assumed this is consistent across all CPUs. + * This ensures that any passed mpidr values reflect the status of the + * primary CPU's MT bit. */ + mpidr |= (read_mpidr_el1() & MPIDR_MT_MASK); + + /* Calculate the correct core, catering for multi-threaded images */ return (int) plat_fpga_calc_core_pos(mpidr); } diff --git a/plat/arm/board/arm_fpga/include/platform_def.h b/plat/arm/board/arm_fpga/include/platform_def.h index 313892023..bf3245e97 100644 --- a/plat/arm/board/arm_fpga/include/platform_def.h +++ b/plat/arm/board/arm_fpga/include/platform_def.h @@ -33,6 +33,10 @@ #define PLAT_MAX_PWR_LVL MPIDR_AFFLVL2 +#define PLAT_FPGA_HOLD_ENTRY_SHIFT 3 +#define PLAT_FPGA_HOLD_STATE_WAIT 0 +#define PLAT_FPGA_HOLD_STATE_GO 1 + #define PLAT_FPGA_CONSOLE_BAUDRATE 38400 #endif