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>
---
v2:
- Remove secondary_data.exception_stacks. Originally I didn't have
the exception stacks at the top of the stack, so this member was
useful. After choosing to just use the top of the stack for the
base, the member has become unnecessary, and can been removed.
- Fix bug for exceptions taken in svc mode on a secondary processor.
It was using cpu0's exception stacks region instead of its own.
arm/cstart.S | 41 +++++++++++++++++++++++++++++-----
arm/cstart64.S | 25 +++++++++++++++++++++
config/config-arm-common.mak | 1 +
lib/arm/asm-offsets.c | 2 ++
lib/arm/asm/psci.h | 2 ++
lib/arm/asm/smp.h | 49 ++++++++++++++++++++++++++++++++++++++++
lib/arm/asm/thread_info.h | 26 ++++++++++++++++++----
lib/arm/psci.c | 19 ++++++++++++++++
lib/arm/setup.c | 8 +++++--
lib/arm/smp.c | 53 ++++++++++++++++++++++++++++++++++++++++++++
lib/arm64/asm-offsets.c | 2 ++
lib/arm64/asm/psci.h | 2 ++
lib/arm64/asm/smp.h | 1 +
13 files changed, 219 insertions(+), 12 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..5b0b8e62cec65 100644
--- a/arm/cstart.S
+++ b/arm/cstart.S
@@ -6,10 +6,13 @@
* This work is licensed under the terms of the GNU LGPL, version 2.
*/
#define __ASSEMBLY__
+#include <asm/thread_info.h>
#include <asm/asm-offsets.h>
#include <asm/ptrace.h>
#include <asm/cp15.h>
+#define THREAD_START_SP ((THREAD_SIZE - S_FRAME_SIZE * 8) & ~7)
+
.arm
.section .init
@@ -32,6 +35,7 @@ start:
push {r0-r1}
/* set up vector table and mode stacks */
+ ldr r0, =exception_stacks
bl exceptions_init
/* complete setup */
@@ -62,13 +66,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 +79,30 @@ exceptions_init:
.text
+.global secondary_entry
+secondary_entry:
+ /*
+ * Set the stack, and set up vector table
+ * and exception stacks. Exception stacks
+ * space starts at stack top and grows up.
+ */
+ ldr r4, =secondary_data
+ ldr r0, [r4, #SECONDARY_DATA_STACK]
+ mov sp, r0
+ bl exceptions_init
+
+ /* enable the MMU */
+ mov r1, #0
+ ldr r0, =mmu_idmap
+ ldr r0, [r0]
+ bl asm_mmu_enable
+
+ /* finish init in C code */
+ bl secondary_cinit
+
+ /* r0 is now the entry function, run it */
+ mov pc, r0
+
.globl halt
halt:
1: wfi
@@ -168,7 +195,9 @@ vector_svc:
* and spsr_<exception> (parent CPSR)
*/
push { r1 }
- ldr r1, =exception_stacks
+ lsr r1, sp, #THREAD_SHIFT
+ lsl r1, #THREAD_SHIFT
+ add r1, #THREAD_START_SP
str r0, [r1, #S_R0]
pop { r0 }
str r0, [r1, #S_R1]
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..ad2a4873dfc0d 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,6 @@ 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);
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..0526016ea0f40
--- /dev/null
+++ b/lib/arm/asm/smp.h
@@ -0,0 +1,49 @@
+#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>
+
+#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;
+};
+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..95058bff9d857 100644
--- a/lib/arm/asm/thread_info.h
+++ b/lib/arm/asm/thread_info.h
@@ -7,16 +7,33 @@
*
* This work is licensed under the terms of the GNU LGPL, version 2.
*/
-#include <asm/processor.h>
#include <asm/page.h>
-#define __MIN_THREAD_SIZE 16384
-#if PAGE_SIZE > __MIN_THREAD_SIZE
+#define MIN_THREAD_SHIFT 14 /* THREAD_SIZE == 16K */
+#if PAGE_SHIFT > MIN_THREAD_SHIFT
+#define THREAD_SHIFT PAGE_SHIFT
#define THREAD_SIZE PAGE_SIZE
+#define THREAD_MASK PAGE_MASK
#else
-#define THREAD_SIZE __MIN_THREAD_SIZE
+#define THREAD_SHIFT MIN_THREAD_SHIFT
+#define THREAD_SIZE (_AC(1,UL) << THREAD_SHIFT)
+#define THREAD_MASK (~(THREAD_SIZE-1))
#endif
+
+#ifndef __ASSEMBLY__
+#include <asm/processor.h>
+
+#ifdef __arm__
+#include <asm/ptrace.h>
+/*
+ * 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 - (sizeof(struct pt_regs) * 8)) & ~7)
+#else
#define THREAD_START_SP (THREAD_SIZE - 16)
+#endif
#define TIF_USER_MODE (1U << 0)
@@ -46,4 +63,5 @@ static inline struct thread_info *current_thread_info(void)
extern void thread_info_init(struct thread_info *ti, unsigned int flags);
+#endif /* !__ASSEMBLY__ */
#endif /* _ASMARM_THREAD_INFO_H_ */
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..f389ba6598faa
--- /dev/null
+++ b/lib/arm/smp.c
@@ -0,0 +1,53 @@
+/*
+ * 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;
+ 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
