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

[Andrew Jones <drjones@redhat.com>]

On Mon, Feb 02, 2015 at 11:23:28AM +0100, Andrew Jones wrote:
> 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>
> ---
> 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.

Drat. Just tested this v2 on hardware and see I introduced a bug with
it. See below

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

Moving the stack setup from just above the call to secondary_cinit
to here leads to sp := 0 on hardware.  I should only load secondary_data
after the mmu is setup.  Actually, I didn't notice that the exception
stacks base was getting set to zero in v1, as I hadn't tested it. The
fix for the stack will now fix them too though. I have a v3 ready to
send, which has been tested on hardware, and with exceptions tested on
secondaries. Sending...

> +	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@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;
> +	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@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();
> +}
> 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
>