arm/arm64: add smp_boot_secondary

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@redhat.com>
Signed-off-by: Marcelo Tosatti <mtosatti@redhat.com>

arm/cstart.S

@@ -6,16 +6,35 @@
  * 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
 
 .globl start
 start:
+	/*
+	 * set stack, making room at top of stack for cpu0's
+	 * exception stacks. Must start wtih stackptr, not
+	 * stacktop, so the thread size masking (shifts) work.
+	 */
+	ldr	sp, =stackptr
+	lsr	sp, #THREAD_SHIFT
+	lsl	sp, #THREAD_SHIFT
+	add	sp, #THREAD_START_SP
+
+	/*
+	 * save sp before pushing anything on the stack
+	 * lr makes a good temp register right now
+	 */
+	mov	lr, sp
+
 	/*
 	 * bootloader params are in r0-r2
 	 * See the kernel doc Documentation/arm/Booting
@@ -27,11 +46,12 @@ start:
 	 * put the dtb in r0. This allows setup to be consistent
 	 * with arm64.
 	 */
-	ldr	sp, =stackptr
 	mov	r0, r2
 	push	{r0-r1}
 
 	/* set up vector table and mode stacks */
+	mov	r0, lr			@ lr is stack top (see above),
+					@ which is the exception stacks base
 	bl	exceptions_init
 
 	/* complete setup */
@@ -62,13 +82,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 +95,30 @@ exceptions_init:
 
 .text
 
+.global secondary_entry
+secondary_entry:
+	/* enable the MMU */
+	mov	r1, #0
+	ldr	r0, =mmu_idmap
+	ldr	r0, [r0]
+	bl	asm_mmu_enable
+
+	/*
+	 * Set the stack, and set up vector table
+	 * and exception stacks. Exception stacks
+	 * space starts at stack top and grows up.
+	 */
+	ldr	r1, =secondary_data
+	ldr	r0, [r1]
+	mov	sp, r0
+	bl	exceptions_init
+
+	/* 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 +211,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]

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]
+	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

arm/flat.lds

@@ -5,9 +5,6 @@ SECTIONS
     . = ALIGN(64K);
     etext = .;
     .data : {
-        exception_stacks = .;
-        . += 64K;
-        exception_stacks_end = .;
         *(.data)
     }
     . = ALIGN(16);

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

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_ */

lib/arm/asm/smp.h

+#ifndef _ASMARM_SMP_H_
+#define _ASMARM_SMP_H_
+/*
+ * Copyright (C) 2015, Red Hat Inc, Andrew Jones <drjones@redhat.com>
+ *
+ * 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;		/* must be first member of struct */
+	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_ */

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_ */

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);

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)

lib/arm/smp.c

+/*
+ * Secondary cpu support
+ *
+ * Copyright (C) 2015, Red Hat Inc, Andrew Jones <drjones@redhat.com>
+ *
+ * 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();
+}

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_ */

lib/arm64/asm/smp.h

+#include "../../arm/asm/smp.h"