FF-A: Register SPMC secondary core entry point.
When the primary physical core boots, the SPMC registers
the entry point (FFA_SECONDARY_EP_REGISTER at physical
FF-A instance) used for secondary core cold boot to the
SPMD. All Secure Partitions are then initialized on their
primary Execution Context. When the NWd (Hypervisor or OS
Kernel) boots, it calls PSCI_CPU_ON (into EL3) for waking
up physical cores. The PSCI layer calls into SPMD PM hooks,
then the SPMD calls into the SPMC by an exception return to
the registered secondary entry point. The target secondary
core then reaches vcpu_main. As follow-up to 56a62c96, PSCI
service calls from within the SWd are prevented.
Change-Id: I2bd42ea54e1a7feebff20e878345ec196ff352e9
Signed-off-by: Max Shvetsov <maksims.svecovs@arm.com>
Signed-off-by: Olivier Deprez <olivier.deprez@arm.com>
diff --git a/src/arch/aarch64/hypervisor/hypervisor_entry.S b/src/arch/aarch64/hypervisor/hypervisor_entry.S
index e85510a..be7c0a7 100644
--- a/src/arch/aarch64/hypervisor/hypervisor_entry.S
+++ b/src/arch/aarch64/hypervisor/hypervisor_entry.S
@@ -9,6 +9,13 @@
#include "hf/arch/offsets.h"
#include "msr.h"
+.macro get_core_affinity reg1 reg2
+ mrs \reg1, mpidr_el1
+ ubfx \reg2, \reg1, 0, 24
+ ubfx \reg1, \reg1, 32, 8
+ orr \reg1, \reg2, \reg1, lsl #32
+.endm
+
/**
* Called only on first boot after the image has been relocated and BSS zeroed.
*
@@ -25,10 +32,7 @@
add x28, x28, :lo12:cpus
/* Set the ID of this CPU from the affinity bits of mpidr. */
- mrs x30, mpidr_el1
- ubfx x29, x30, 0, 24
- ubfx x30, x30, 32, 8
- orr x30, x29, x30, lsl #32
+ get_core_affinity x30, x29
str x30, [x28, CPU_ID]
mov x0, x28
@@ -59,6 +63,44 @@
.section .text.entry, "ax"
.global cpu_entry
cpu_entry:
+#if SECURE_WORLD == 1
+
+ /* Get number of cpus gathered from DT. */
+ adrp x3, cpu_count
+ add x3, x3, :lo12:cpu_count
+ ldr w3, [x3]
+
+ /* Prevent number of CPUs to be higher than supported by platform. */
+ cmp w3, #MAX_CPUS
+ bhi .
+
+ /* x0 points to first cpu in cpus array. */
+ adrp x0, cpus
+ add x0, x0, :lo12:cpus
+
+ /* Get current core affinity. */
+ get_core_affinity x1, x2
+
+ /* Dead stop here if no more cpu. */
+0: cbz w3, 0b
+
+ sub w3, w3, #1
+
+ /* Get cpu id pointed to by x0 in cpu array. */
+ ldr x2, [x0, CPU_ID]
+
+ /* Exit if current core id matches cpu id. */
+ cmp x1, x2
+ beq 1f
+
+ /* Point to next cpu in cpus array and loop. */
+ add x0, x0, #CPU_SIZE
+ b 0b
+
+1: /* x0 points to current cpu in cpus array */
+
+#endif
+
bl mm_enable
bl prepare_for_c
diff --git a/src/arch/aarch64/hypervisor/offsets.c b/src/arch/aarch64/hypervisor/offsets.c
index 6b9275d..2273678 100644
--- a/src/arch/aarch64/hypervisor/offsets.c
+++ b/src/arch/aarch64/hypervisor/offsets.c
@@ -10,6 +10,8 @@
#include "hf/offset_size_header.h"
#include "hf/vm.h"
+DEFINE_SIZEOF(CPU_SIZE, struct cpu)
+
DEFINE_OFFSETOF(CPU_ID, struct cpu, id)
DEFINE_OFFSETOF(CPU_STACK_BOTTOM, struct cpu, stack_bottom)
DEFINE_OFFSETOF(VCPU_VM, struct vcpu, vm)
diff --git a/src/arch/aarch64/plat/psci/spmc.c b/src/arch/aarch64/plat/psci/spmc.c
index fbee952..b92e48f 100644
--- a/src/arch/aarch64/plat/psci/spmc.c
+++ b/src/arch/aarch64/plat/psci/spmc.c
@@ -11,6 +11,8 @@
#include "psci.h"
+void cpu_entry(struct cpu *c);
+
/**
* Returns zero in context of the SPMC as it does not rely
* on the EL3 PSCI framework.
@@ -26,6 +28,16 @@
*/
void plat_psci_init(void)
{
+ struct ffa_value res;
+
+ /* Register SPMC secondary cold boot entry point */
+ res = smc_ffa_call(
+ (struct ffa_value){.func = FFA_SECONDARY_EP_REGISTER_64,
+ .arg1 = (uintreg_t)&cpu_entry});
+
+ if (res.func != FFA_SUCCESS_64) {
+ panic("FFA_SECONDARY_EP_REGISTER_64 failed");
+ }
}
void plat_psci_cpu_suspend(uint32_t power_state)
@@ -35,6 +47,11 @@
void plat_psci_cpu_resume(struct cpu *c, ipaddr_t entry_point)
{
- (void)c;
- (void)entry_point;
+ if (cpu_on(c, entry_point, 0UL)) {
+ /*
+ * This is the boot time PSCI cold reset path (svc_cpu_on_finish
+ * handler relayed by SPMD) on secondary cores.
+ */
+ dlog_verbose("%s: cpu mpidr 0x%x ON\n", __func__, c->id);
+ }
}
diff --git a/src/cpu.c b/src/cpu.c
index 11eb0a3..42c2428 100644
--- a/src/cpu.c
+++ b/src/cpu.c
@@ -10,6 +10,8 @@
#include <stdalign.h>
+#include "hf/arch/cache.h"
+
#include "hf/api.h"
#include "hf/check.h"
#include "hf/dlog.h"
@@ -70,7 +72,7 @@
},
};
-static uint32_t cpu_count = 1;
+uint32_t cpu_count = 1;
void cpu_module_init(const cpu_id_t *cpu_ids, size_t count)
{
@@ -111,6 +113,15 @@
dlog_warning("Boot CPU's ID not found in config.\n");
cpus[0].id = boot_cpu_id;
}
+
+ /*
+ * Clean the cache for the cpus array such that secondary cores
+ * hitting the entry point can read the cpus array consistently
+ * with MMU off (hence data cache off).
+ */
+ arch_cache_clean_range(va_from_ptr(cpus), sizeof(cpus));
+
+ arch_cache_clean_range(va_from_ptr(&cpu_count), sizeof(cpu_count));
}
size_t cpu_index(struct cpu *c)