[PATCH 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>
---
 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




[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