diff options
-rw-r--r-- | core/arch/arm/include/arm.h | 3 | ||||
-rw-r--r-- | core/arch/arm/include/kernel/boot.h | 2 | ||||
-rw-r--r-- | core/arch/arm/plat-vexpress/fvp_spmc_pm.c | 82 |
3 files changed, 87 insertions, 0 deletions
diff --git a/core/arch/arm/include/arm.h b/core/arch/arm/include/arm.h index 36ca7ec0..0186a79e 100644 --- a/core/arch/arm/include/arm.h +++ b/core/arch/arm/include/arm.h @@ -45,6 +45,9 @@ #define MPIDR_CLUSTER_SHIFT MPIDR_AFF1_SHIFT #define MPIDR_CLUSTER_MASK MPIDR_AFF1_MASK +#define MPIDR_AARCH32_AFF_MASK (MPIDR_AFF0_MASK | MPIDR_AFF1_MASK | \ + MPIDR_AFF2_MASK) + /* CLIDR definitions */ #define CLIDR_LOUIS_SHIFT 21 #define CLIDR_LOC_SHIFT 24 diff --git a/core/arch/arm/include/kernel/boot.h b/core/arch/arm/include/kernel/boot.h index 5b06507f..53713c3f 100644 --- a/core/arch/arm/include/kernel/boot.h +++ b/core/arch/arm/include/kernel/boot.h @@ -88,4 +88,6 @@ void *get_external_dt(void); unsigned long get_aslr_seed(void *fdt); +void ffa_secondary_cpu_boot_req(vaddr_t secondary_ep, uint64_t cookie); + #endif /* __KERNEL_BOOT_H */ diff --git a/core/arch/arm/plat-vexpress/fvp_spmc_pm.c b/core/arch/arm/plat-vexpress/fvp_spmc_pm.c new file mode 100644 index 00000000..2f16a968 --- /dev/null +++ b/core/arch/arm/plat-vexpress/fvp_spmc_pm.c @@ -0,0 +1,82 @@ +// SPDX-License-Identifier: BSD-2-Clause +/* + * Copyright (c) 2019, Arm Limited + */ + +#include <arm.h> +#include <ffa.h> +#include <initcall.h> +#include <keep.h> +#include <kernel/boot.h> +#include <kernel/interrupt.h> +#include <kernel/misc.h> +#include <kernel/panic.h> +#include <mm/core_memprot.h> +#include <mm/core_mmu.h> +#include <platform_config.h> +#include <sm/psci.h> +#include <stdint.h> +#include <string.h> +#include <trace.h> + +/* + * Lookup table of core and cluster affinities on the FVP. In the absence of a + * DT that provides the same information, this table is used to initialise + * OP-TEE on secondary cores. + */ +static const uint64_t core_clus_aff_array[] = { + 0x0000, /* Cluster 0 Cpu 0 */ + 0x0001, /* Cluster 0 Cpu 1 */ + 0x0002, /* Cluster 0 Cpu 2 */ + 0x0003, /* Cluster 0 Cpu 3 */ + 0x0100, /* Cluster 1 Cpu 0 */ + 0x0101, /* Cluster 1 Cpu 1 */ + 0x0102, /* Cluster 1 Cpu 2 */ + 0x0103, /* Cluster 1 Cpu 3 */ +}; + +static uint32_t get_cpu_on_fid(void) +{ +#ifdef ARM64 + return PSCI_CPU_ON_SMC64; +#endif +#ifdef ARM32 + return PSCI_CPU_ON; +#endif +} + +void ffa_secondary_cpu_boot_req(vaddr_t secondary_ep, uint64_t cookie) +{ + unsigned long mpidr = read_mpidr(); + unsigned int aff_shift = 0; + unsigned long a1 = 0; + unsigned int cnt = 0; + + if (mpidr & MPIDR_MT_MASK) + aff_shift = MPIDR_CLUSTER_SHIFT; + + for (cnt = 0; cnt < ARRAY_SIZE(core_clus_aff_array); cnt++) { + int32_t ret = 0; + + /* Clear out the affinity fields until level 2 */ + a1 = mpidr & ~(unsigned long)MPIDR_AARCH32_AFF_MASK; + + /* Create an mpidr from core_clus_aff_array */ + a1 |= core_clus_aff_array[cnt] << aff_shift; + + /* Ignore current cpu */ + if (a1 == mpidr) + continue; + + DMSG("PSCI_CPU_ON op on mpidr 0x%lx", a1); + + /* Invoke the PSCI_CPU_ON function */ + ret = thread_smc(get_cpu_on_fid(), a1, secondary_ep, cookie); + + if (ret != PSCI_RET_SUCCESS) + EMSG("PSCI_CPU_ON op on mpidr 0x%lx failed %"PRId32, + a1, ret); + else + DMSG("PSCI_CPU_ON op on mpidr 0x%lx done", a1); + } +} |