[PATCH v2 17/18] arm/arm64: add smp_boot_secondary

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



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




[Index of Archives]     [KVM ARM]     [KVM ia64]     [KVM ppc]     [Virtualization Tools]     [Spice Development]     [Libvirt]     [Libvirt Users]     [Linux USB Devel]     [Linux Audio Users]     [Yosemite Questions]     [Linux Kernel]     [Linux SCSI]     [XFree86]
  Powered by Linux