Add a common entry point, present/online cpu masks, and
smp_boot_secondary() to support booting secondary cpus.
Adds a bit more PSCI API that we need too. We also
adjust THREAD_START_SP for arm to make some room for
exception stacks.
Signed-off-by: Andrew Jones <drjones@xxxxxxxxxx>
---
arm/cstart.S | 32 +++++++++++++++++++++----
arm/cstart64.S | 25 +++++++++++++++++++
config/config-arm-common.mak | 1 +
lib/arm/asm-offsets.c | 3 +++
lib/arm/asm/psci.h | 2 ++
lib/arm/asm/smp.h | 56 +++++++++++++++++++++++++++++++++++++++++++
lib/arm/asm/thread_info.h | 11 +++++++++
lib/arm/psci.c | 19 +++++++++++++++
lib/arm/setup.c | 8 +++++--
lib/arm/smp.c | 57 ++++++++++++++++++++++++++++++++++++++++++++
lib/arm64/asm-offsets.c | 2 ++
lib/arm64/asm/psci.h | 2 ++
lib/arm64/asm/smp.h | 1 +
13 files changed, 212 insertions(+), 7 deletions(-)
create mode 100644 lib/arm/asm/smp.h
create mode 100644 lib/arm/smp.c
create mode 100644 lib/arm64/asm/smp.h
diff --git a/arm/cstart.S b/arm/cstart.S
index 08a0b3ecc61f6..574802670ee17 100644
--- a/arm/cstart.S
+++ b/arm/cstart.S
@@ -32,6 +32,7 @@ start:
push {r0-r1}
/* set up vector table and mode stacks */
+ ldr r0, =exception_stacks
bl exceptions_init
/* complete setup */
@@ -62,13 +63,12 @@ exceptions_init:
mcr p15, 0, r2, c12, c0, 0 @ write VBAR
mrs r2, cpsr
- ldr r1, =exception_stacks
/* first frame reserved for svc mode */
- set_mode_stack UND_MODE, r1
- set_mode_stack ABT_MODE, r1
- set_mode_stack IRQ_MODE, r1
- set_mode_stack FIQ_MODE, r1
+ set_mode_stack UND_MODE, r0
+ set_mode_stack ABT_MODE, r0
+ set_mode_stack IRQ_MODE, r0
+ set_mode_stack FIQ_MODE, r0
msr cpsr_cxsf, r2 @ back to svc mode
isb
@@ -76,6 +76,28 @@ exceptions_init:
.text
+.global secondary_entry
+secondary_entry:
+ /* set up vector table and mode stacks */
+ ldr r4, =secondary_data
+ ldr r0, [r4, #SECONDARY_DATA_ESTACKS]
+ bl exceptions_init
+
+ /* enable the MMU */
+ mov r1, #0
+ ldr r0, =mmu_idmap
+ ldr r0, [r0]
+ bl asm_mmu_enable
+
+ /* set the stack */
+ ldr sp, [r4, #SECONDARY_DATA_STACK]
+
+ /* finish init in C code */
+ bl secondary_cinit
+
+ /* r0 is now the entry function, run it */
+ mov pc, r0
+
.globl halt
halt:
1: wfi
diff --git a/arm/cstart64.S b/arm/cstart64.S
index 58e4040cfb40f..b4d7f1939793b 100644
--- a/arm/cstart64.S
+++ b/arm/cstart64.S
@@ -55,6 +55,31 @@ exceptions_init:
.text
+.globl secondary_entry
+secondary_entry:
+ /* Enable FP/ASIMD */
+ mov x0, #(3 << 20)
+ msr cpacr_el1, x0
+
+ /* set up exception handling */
+ bl exceptions_init
+
+ /* enable the MMU */
+ adr x0, mmu_idmap
+ ldr x0, [x0]
+ bl asm_mmu_enable
+
+ /* set the stack */
+ adr x1, secondary_data
+ ldr x0, [x1, #SECONDARY_DATA_STACK]
+ mov sp, x0
+
+ /* finish init in C code */
+ bl secondary_cinit
+
+ /* x0 is now the entry function, run it */
+ br x0
+
.globl halt
halt:
1: wfi
diff --git a/config/config-arm-common.mak b/config/config-arm-common.mak
index 13f5338a35a02..314261ef60cf7 100644
--- a/config/config-arm-common.mak
+++ b/config/config-arm-common.mak
@@ -36,6 +36,7 @@ cflatobjs += lib/arm/setup.o
cflatobjs += lib/arm/mmu.o
cflatobjs += lib/arm/bitops.o
cflatobjs += lib/arm/psci.o
+cflatobjs += lib/arm/smp.o
libeabi = lib/arm/libeabi.a
eabiobjs = lib/arm/eabi_compat.o
diff --git a/lib/arm/asm-offsets.c b/lib/arm/asm-offsets.c
index 1ee9da070f609..b0932c62703b8 100644
--- a/lib/arm/asm-offsets.c
+++ b/lib/arm/asm-offsets.c
@@ -8,6 +8,7 @@
#include <libcflat.h>
#include <kbuild.h>
#include <asm/ptrace.h>
+#include <asm/smp.h>
int main(void)
{
@@ -30,5 +31,7 @@ int main(void)
OFFSET(S_PSR, pt_regs, ARM_cpsr);
OFFSET(S_OLD_R0, pt_regs, ARM_ORIG_r0);
DEFINE(S_FRAME_SIZE, sizeof(struct pt_regs));
+ OFFSET(SECONDARY_DATA_STACK, secondary_data, stack);
+ OFFSET(SECONDARY_DATA_ESTACKS, secondary_data, exception_stacks);
return 0;
}
diff --git a/lib/arm/asm/psci.h b/lib/arm/asm/psci.h
index e2e66b47de480..c5fe78184b5ac 100644
--- a/lib/arm/asm/psci.h
+++ b/lib/arm/asm/psci.h
@@ -9,5 +9,7 @@
extern int psci_invoke(u32 function_id, u32 arg0, u32 arg1, u32 arg2);
extern int psci_cpu_on(unsigned long cpuid, unsigned long entry_point);
extern void psci_sys_reset(void);
+extern int cpu_psci_cpu_boot(unsigned int cpu);
+extern void cpu_psci_cpu_die(unsigned int cpu);
#endif /* _ASMARM_PSCI_H_ */
diff --git a/lib/arm/asm/smp.h b/lib/arm/asm/smp.h
new file mode 100644
index 0000000000000..46c05a3f67b96
--- /dev/null
+++ b/lib/arm/asm/smp.h
@@ -0,0 +1,56 @@
+#ifndef _ASMARM_SMP_H_
+#define _ASMARM_SMP_H_
+/*
+ * Copyright (C) 2015, Red Hat Inc, Andrew Jones <drjones@xxxxxxxxxx>
+ *
+ * This work is licensed under the terms of the GNU LGPL, version 2.
+ */
+#include <asm/thread_info.h>
+#include <asm/cpumask.h>
+#ifdef __arm__
+#include <asm/ptrace.h>
+#endif
+
+#define smp_processor_id() (current_thread_info()->cpu)
+
+extern void halt(void);
+
+extern cpumask_t cpu_present_mask;
+extern cpumask_t cpu_online_mask;
+#define cpu_present(cpu) cpumask_test_cpu(cpu, &cpu_present_mask)
+#define cpu_online(cpu) cpumask_test_cpu(cpu, &cpu_online_mask)
+#define for_each_present_cpu(cpu) for_each_cpu(cpu, &cpu_present_mask)
+#define for_each_online_cpu(cpu) for_each_cpu(cpu, &cpu_online_mask)
+
+static inline void set_cpu_present(int cpu, bool present)
+{
+ if (present)
+ cpumask_set_cpu(cpu, &cpu_present_mask);
+ else
+ cpumask_clear_cpu(cpu, &cpu_present_mask);
+}
+
+static inline void set_cpu_online(int cpu, bool online)
+{
+ if (online)
+ cpumask_set_cpu(cpu, &cpu_online_mask);
+ else
+ cpumask_clear_cpu(cpu, &cpu_online_mask);
+}
+
+typedef void (*secondary_entry_fn)(void);
+
+/* secondary_data is reused for each cpu, so only boot one at a time */
+struct secondary_data {
+ void *stack;
+ secondary_entry_fn entry;
+#ifdef __arm__
+#define SECONDARY_EXCEPTION_STACKS_SIZE (sizeof(struct pt_regs) * 8)
+ void *exception_stacks;
+#endif
+};
+extern struct secondary_data secondary_data;
+
+extern void smp_boot_secondary(int cpu, secondary_entry_fn entry);
+
+#endif /* _ASMARM_SMP_H_ */
diff --git a/lib/arm/asm/thread_info.h b/lib/arm/asm/thread_info.h
index 5f7104f7c234f..c7ec8bc91dec0 100644
--- a/lib/arm/asm/thread_info.h
+++ b/lib/arm/asm/thread_info.h
@@ -9,6 +9,7 @@
*/
#include <asm/processor.h>
#include <asm/page.h>
+#include <asm/smp.h>
#define __MIN_THREAD_SIZE 16384
#if PAGE_SIZE > __MIN_THREAD_SIZE
@@ -16,7 +17,17 @@
#else
#define THREAD_SIZE __MIN_THREAD_SIZE
#endif
+
+#ifdef __arm__
+/*
+ * arm needs room left at the top for the exception stacks,
+ * and the stack needs to be 8-byte aligned
+ */
+#define THREAD_START_SP \
+ ((THREAD_SIZE - SECONDARY_EXCEPTION_STACKS_SIZE) & ~7)
+#else
#define THREAD_START_SP (THREAD_SIZE - 16)
+#endif
#define TIF_USER_MODE (1U << 0)
diff --git a/lib/arm/psci.c b/lib/arm/psci.c
index 027c4f66f1815..aca88851171f5 100644
--- a/lib/arm/psci.c
+++ b/lib/arm/psci.c
@@ -7,6 +7,8 @@
* This work is licensed under the terms of the GNU LGPL, version 2.
*/
#include <asm/psci.h>
+#include <asm/setup.h>
+#include <asm/page.h>
#define T PSCI_INVOKE_ARG_TYPE
__attribute__((noinline))
@@ -24,6 +26,23 @@ int psci_cpu_on(unsigned long cpuid, unsigned long entry_point)
return psci_invoke(PSCI_FN_CPU_ON, cpuid, entry_point, 0);
}
+extern void secondary_entry(void);
+int cpu_psci_cpu_boot(unsigned int cpu)
+{
+ int err = psci_cpu_on(cpus[cpu], __pa(secondary_entry));
+ if (err)
+ printf("failed to boot CPU%d (%d)\n", cpu, err);
+ return err;
+}
+
+#define PSCI_POWER_STATE_TYPE_POWER_DOWN (1U << 16)
+void cpu_psci_cpu_die(unsigned int cpu)
+{
+ int err = psci_invoke(PSCI_0_2_FN_CPU_OFF,
+ PSCI_POWER_STATE_TYPE_POWER_DOWN, 0, 0);
+ printf("unable to power off CPU%d (%d)\n", cpu, err);
+}
+
void psci_sys_reset(void)
{
psci_invoke(PSCI_0_2_FN_SYSTEM_RESET, 0, 0, 0);
diff --git a/lib/arm/setup.c b/lib/arm/setup.c
index b30c8696f6539..02e81a689a8a6 100644
--- a/lib/arm/setup.c
+++ b/lib/arm/setup.c
@@ -18,6 +18,7 @@
#include <asm/setup.h>
#include <asm/page.h>
#include <asm/mmu.h>
+#include <asm/smp.h>
extern unsigned long stacktop;
extern void io_init(void);
@@ -30,14 +31,17 @@ phys_addr_t __phys_offset, __phys_end;
static void cpu_set(int fdtnode __unused, u32 regval, void *info __unused)
{
- assert(nr_cpus < NR_CPUS);
- cpus[nr_cpus++] = regval;
+ int cpu = nr_cpus++;
+ assert(cpu < NR_CPUS);
+ cpus[cpu] = regval;
+ set_cpu_present(cpu, true);
}
static void cpu_init(void)
{
nr_cpus = 0;
assert(dt_for_each_cpu_node(cpu_set, NULL) == 0);
+ set_cpu_online(0, true);
}
static void mem_init(phys_addr_t freemem_start)
diff --git a/lib/arm/smp.c b/lib/arm/smp.c
new file mode 100644
index 0000000000000..967ac50e19e8f
--- /dev/null
+++ b/lib/arm/smp.c
@@ -0,0 +1,57 @@
+/*
+ * Secondary cpu support
+ *
+ * Copyright (C) 2015, Red Hat Inc, Andrew Jones <drjones@xxxxxxxxxx>
+ *
+ * This work is licensed under the terms of the GNU LGPL, version 2.
+ */
+#include <libcflat.h>
+#include <alloc.h>
+#include <asm/thread_info.h>
+#include <asm/cpumask.h>
+#include <asm/mmu.h>
+#include <asm/psci.h>
+#include <asm/smp.h>
+
+cpumask_t cpu_present_mask;
+cpumask_t cpu_online_mask;
+struct secondary_data secondary_data;
+
+secondary_entry_fn secondary_cinit(void)
+{
+ struct thread_info *ti = current_thread_info();
+ secondary_entry_fn entry;
+
+ thread_info_init(ti, 0);
+ mmu_set_enabled();
+
+ /*
+ * Save secondary_data.entry locally to avoid opening a race
+ * window between marking ourselves online and calling it.
+ */
+ entry = secondary_data.entry;
+ set_cpu_online(ti->cpu, true);
+ sev();
+
+ /*
+ * Return to the assembly stub, allowing entry to be called
+ * from there with an empty stack.
+ */
+ return entry;
+}
+
+void smp_boot_secondary(int cpu, secondary_entry_fn entry)
+{
+ void *stack_base = memalign(THREAD_SIZE, THREAD_SIZE);
+
+ secondary_data.stack = stack_base + THREAD_START_SP;
+ secondary_data.entry = entry;
+#ifdef __arm__
+ /* arm uses the top of the stack for exception stacks */
+ secondary_data.exception_stacks = secondary_data.stack;
+#endif
+ assert(cpu_psci_cpu_boot(cpu) == 0);
+
+ while (!cpu_online(cpu))
+ wfe();
+}
diff --git a/lib/arm64/asm-offsets.c b/lib/arm64/asm-offsets.c
index d7d33f4d917ab..c1193f255581c 100644
--- a/lib/arm64/asm-offsets.c
+++ b/lib/arm64/asm-offsets.c
@@ -8,6 +8,7 @@
#include <libcflat.h>
#include <kbuild.h>
#include <asm/ptrace.h>
+#include <asm/smp.h>
int main(void)
{
@@ -26,5 +27,6 @@ int main(void)
OFFSET(S_ORIG_X0, pt_regs, orig_x0);
OFFSET(S_SYSCALLNO, pt_regs, syscallno);
DEFINE(S_FRAME_SIZE, sizeof(struct pt_regs));
+ OFFSET(SECONDARY_DATA_STACK, secondary_data, stack);
return 0;
}
diff --git a/lib/arm64/asm/psci.h b/lib/arm64/asm/psci.h
index c481be4bd6bab..940d61d34c05d 100644
--- a/lib/arm64/asm/psci.h
+++ b/lib/arm64/asm/psci.h
@@ -9,5 +9,7 @@
extern int psci_invoke(u64 function_id, u64 arg0, u64 arg1, u64 arg2);
extern int psci_cpu_on(unsigned long cpuid, unsigned long entry_point);
extern void psci_sys_reset(void);
+extern int cpu_psci_cpu_boot(unsigned int cpu);
+extern void cpu_psci_cpu_die(unsigned int cpu);
#endif /* _ASMARM64_PSCI_H_ */
diff --git a/lib/arm64/asm/smp.h b/lib/arm64/asm/smp.h
new file mode 100644
index 0000000000000..e6cdaf4859939
--- /dev/null
+++ b/lib/arm64/asm/smp.h
@@ -0,0 +1 @@
+#include "../../arm/asm/smp.h"
--
1.9.3
--
To unsubscribe from this list: send the line "unsubscribe kvm" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
