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