From e231f3a5f46f3897368521e0b61fec8cad91b35e Mon Sep 17 00:00:00 2001 From: Usama Arif Date: Thu, 19 Sep 2019 10:54:16 +0100 Subject: [PATCH 1/2] a5ds: Hold the secondary cpus in pen rather than panic For the secondary CPUs, hold the cpu in wfe rather then panic. This will be needed when multicore support is added to a5ds as the smc call will write to the hold base and signal an event to power on the secondary CPUs. Change-Id: I0ffc2059e9ef894c21375ca5c94def859bfa6599 Signed-off-by: Usama Arif --- plat/arm/board/a5ds/aarch32/a5ds_helpers.S | 82 ++++++++++++++++++++-- plat/arm/board/a5ds/include/platform_def.h | 8 +++ 2 files changed, 83 insertions(+), 7 deletions(-) diff --git a/plat/arm/board/a5ds/aarch32/a5ds_helpers.S b/plat/arm/board/a5ds/aarch32/a5ds_helpers.S index 23a22d9c5..ed7ad9c86 100644 --- a/plat/arm/board/a5ds/aarch32/a5ds_helpers.S +++ b/plat/arm/board/a5ds/aarch32/a5ds_helpers.S @@ -12,17 +12,36 @@ .globl plat_get_my_entrypoint .globl plat_is_my_cpu_primary - /* -------------------------------------------------------------------- + /* ----------------------------------------------------- * void plat_secondary_cold_boot_setup (void); * - * For AArch32, cold-booting secondary CPUs is not yet - * implemented and they panic. - * -------------------------------------------------------------------- + * This function performs any platform specific actions + * needed for a secondary cpu after a cold reset e.g + * mark the cpu's presence, mechanism to place it in a + * holding pen etc. + * ----------------------------------------------------- */ func plat_secondary_cold_boot_setup -cb_panic: - wfi - b cb_panic + /* Calculate address of our hold entry */ + bl plat_my_core_pos + lsl r0, r0, #A5DS_HOLD_ENTRY_SHIFT + mov_imm r2, A5DS_HOLD_BASE + /* Clear the value stored in the hold address for the specific core */ + mov_imm r3, A5DS_HOLD_STATE_WAIT + str r3, [r2, r0] + dmb ish + + /* Wait until we have a go */ +poll_mailbox: + ldr r1, [r2, r0] + cmp r1, #A5DS_HOLD_STATE_WAIT + beq 1f + mov_imm r0, A5DS_TRUSTED_MAILBOX_BASE + ldr r1, [r0] + bx r1 +1: + wfe + b poll_mailbox endfunc plat_secondary_cold_boot_setup /* --------------------------------------------------------------------- @@ -56,3 +75,52 @@ func plat_is_my_cpu_primary movne r0, #0 bx lr endfunc plat_is_my_cpu_primary + + /* --------------------------------------------------------------------- + * Loads MPIDR in r0 and calls plat_arm_calc_core_pos + * --------------------------------------------------------------------- + */ +func plat_my_core_pos + ldcopr r0, MPIDR + b plat_arm_calc_core_pos + +endfunc plat_my_core_pos + + /* --------------------------------------------------------------------- + * unsigned int plat_arm_calc_core_pos(u_register_t mpidr) + * + * Function to calculate the core position on A5DS. + * + * (ClusterId * A5DS_MAX_CPUS_PER_CLUSTER * A5DS_MAX_PE_PER_CPU) + + * (CPUId * A5DS_MAX_PE_PER_CPU) + + * ThreadId + * + * which can be simplified as: + * + * ((ClusterId * A5DS_MAX_CPUS_PER_CLUSTER + CPUId) * A5DS_MAX_PE_PER_CPU) + * + ThreadId + * --------------------------------------------------------------------- + */ +func plat_arm_calc_core_pos + mov r3, r0 + + /* + * Check for MT bit in MPIDR. If not set, shift MPIDR to left to make it + * look as if in a multi-threaded implementation + */ + tst r0, #MPIDR_MT_MASK + lsleq r3, r0, #MPIDR_AFFINITY_BITS + + /* Extract individual affinity fields from MPIDR */ + ubfx r0, r3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS + ubfx r1, r3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS + ubfx r2, r3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS + + /* Compute linear position */ + mov r3, #A5DS_MAX_CPUS_PER_CLUSTER + mla r1, r2, r3, r1 + mov r3, #A5DS_MAX_PE_PER_CPU + mla r0, r1, r3, r0 + + bx lr +endfunc plat_arm_calc_core_pos diff --git a/plat/arm/board/a5ds/include/platform_def.h b/plat/arm/board/a5ds/include/platform_def.h index db65c3778..13c19343f 100644 --- a/plat/arm/board/a5ds/include/platform_def.h +++ b/plat/arm/board/a5ds/include/platform_def.h @@ -325,6 +325,14 @@ /* Mailbox base address */ #define A5DS_TRUSTED_MAILBOX_BASE A5DS_SHARED_RAM_BASE +#define A5DS_TRUSTED_MAILBOX_SIZE (8 + A5DS_HOLD_SIZE) +#define A5DS_HOLD_BASE (A5DS_TRUSTED_MAILBOX_BASE + 8) +#define A5DS_HOLD_SIZE (PLATFORM_CORE_COUNT * \ + A5DS_HOLD_ENTRY_SIZE) +#define A5DS_HOLD_ENTRY_SHIFT 3 +#define A5DS_HOLD_ENTRY_SIZE (1 << A5DS_HOLD_ENTRY_SHIFT) +#define A5DS_HOLD_STATE_WAIT 0 +#define A5DS_HOLD_STATE_GO 1 /* * GIC related constants to cater for GICv2 From ec885bacb247e9a88c0e21406bdf42821eb340c7 Mon Sep 17 00:00:00 2001 From: Usama Arif Date: Thu, 19 Sep 2019 11:07:24 +0100 Subject: [PATCH 2/2] a5ds: add multicore support Enable cores 1-3 using psci. On receiving the smc call from kernel, core 0 will bring the secondary cores out pen and signal an event for the cores. Currently on switching the cores is enabled i.e. it is not possible to suspend, switch cores off, etc. Change-Id: I6087e1d2ec650e1d587fd543efc1b08cbb50ae5f Signed-off-by: Usama Arif --- fdts/a5ds.dts | 26 +++++++++++++++ plat/arm/board/a5ds/a5ds_pm.c | 37 ++++++++++++++++++++++ plat/arm/board/a5ds/include/platform_def.h | 10 +++--- 3 files changed, 68 insertions(+), 5 deletions(-) diff --git a/fdts/a5ds.dts b/fdts/a5ds.dts index 8bc4adf8a..91212e8a0 100644 --- a/fdts/a5ds.dts +++ b/fdts/a5ds.dts @@ -12,14 +12,40 @@ interrupt-parent = <&gic>; #address-cells = <1>; #size-cells = <1>; + + psci { + compatible = "arm,psci-1.0", "arm,psci-0.2", "arm,psci"; + method = "smc"; + cpu_on = <0x84000003>; + }; + cpus { #address-cells = <1>; #size-cells = <0>; cpu@0 { device_type = "cpu"; compatible = "arm,cortex-a5"; + enable-method = "psci"; reg = <0>; }; + cpu@1 { + device_type = "cpu"; + compatible = "arm,cortex-a5"; + enable-method = "psci"; + reg = <1>; + }; + cpu@2 { + device_type = "cpu"; + compatible = "arm,cortex-a5"; + enable-method = "psci"; + reg = <2>; + }; + cpu@3 { + device_type = "cpu"; + compatible = "arm,cortex-a5"; + enable-method = "psci"; + reg = <3>; + }; }; memory@80000000 { diff --git a/plat/arm/board/a5ds/a5ds_pm.c b/plat/arm/board/a5ds/a5ds_pm.c index 5fd443b12..98de77d10 100644 --- a/plat/arm/board/a5ds/a5ds_pm.c +++ b/plat/arm/board/a5ds/a5ds_pm.c @@ -6,6 +6,38 @@ #include #include +#include +#include + +/******************************************************************************* + * Platform handler called when a power domain is about to be turned on. The + * mpidr determines the CPU to be turned on. + ******************************************************************************/ +static int a5ds_pwr_domain_on(u_register_t mpidr) +{ + unsigned int pos = plat_core_pos_by_mpidr(mpidr); + uint64_t *hold_base = (uint64_t *)A5DS_HOLD_BASE; + + hold_base[pos] = A5DS_HOLD_STATE_GO; + dsbish(); + sev(); + + return PSCI_E_SUCCESS; +} + +/******************************************************************************* + * Platform handler called when a power domain has just been powered on after + * being turned off earlier. The target_state encodes the low power state that + * each level has woken up from. + ******************************************************************************/ +void a5ds_pwr_domain_on_finish(const psci_power_state_t *target_state) +{ + /* TODO: This setup is needed only after a cold boot*/ + gicv2_pcpu_distif_init(); + + /* Enable the gic cpu interface */ + gicv2_cpuif_enable(); +} /******************************************************************************* * Export the platform handlers via a5ds_psci_pm_ops. The ARM Standard @@ -14,11 +46,16 @@ plat_psci_ops_t a5ds_psci_pm_ops = { /* dummy struct */ .validate_ns_entrypoint = NULL, + .pwr_domain_on = a5ds_pwr_domain_on, + .pwr_domain_on_finish = a5ds_pwr_domain_on_finish }; int __init plat_setup_psci_ops(uintptr_t sec_entrypoint, const plat_psci_ops_t **psci_ops) { + uintptr_t *mailbox = (void *)A5DS_TRUSTED_MAILBOX_BASE; + *mailbox = sec_entrypoint; + *psci_ops = &a5ds_psci_pm_ops; return 0; diff --git a/plat/arm/board/a5ds/include/platform_def.h b/plat/arm/board/a5ds/include/platform_def.h index 13c19343f..e9e4b9aef 100644 --- a/plat/arm/board/a5ds/include/platform_def.h +++ b/plat/arm/board/a5ds/include/platform_def.h @@ -97,9 +97,9 @@ /* Default number of threads per CPU on A5DS */ #define A5DS_MAX_PE_PER_CPU 1 -#define A5DS_CORE_COUNT 1 +#define A5DS_CORE_COUNT 4 -#define A5DS_PRIMARY_CPU 0x0 +#define A5DS_PRIMARY_CPU 0x0 #define FLASH1_BASE UL(0x8000000) #define FLASH1_SIZE UL(0x2800000) @@ -229,11 +229,11 @@ #define BL32_LIMIT (ARM_BL_RAM_BASE + ARM_BL_RAM_SIZE) /* Required platform porting definitions */ -#define PLATFORM_CORE_COUNT 1 -#define PLAT_NUM_PWR_DOMAINS (A5DS_CLUSTER_COUNT + \ +#define PLATFORM_CORE_COUNT A5DS_CORE_COUNT +#define PLAT_NUM_PWR_DOMAINS (A5DS_CLUSTER_COUNT + \ PLATFORM_CORE_COUNT) + 1 -#define PLAT_MAX_PWR_LVL 2 +#define PLAT_MAX_PWR_LVL 2 /* * Other platform porting definitions are provided by included headers