[v3,17/18] arm/arm64: add smp_boot_secondary

Message ID	1422880199-12093-1-git-send-email-drjones@redhat.com (mailing list archive)
State	New, archived
Headers	show Return-Path: <kvm-owner@kernel.org> From: Andrew Jones <drjones@redhat.com> To: kvmarm@lists.cs.columbia.edu, kvm@vger.kernel.org Cc: christoffer.dall@linaro.org, pbonzini@redhat.com Subject: [PATCH v3 17/18] arm/arm64: add smp_boot_secondary Date: Mon, 2 Feb 2015 13:29:59 +0100 Message-Id: <1422880199-12093-1-git-send-email-drjones@redhat.com> In-Reply-To: <1422815686-24591-18-git-send-email-drjones@redhat.com> References: <1422815686-24591-18-git-send-email-drjones@redhat.com> Sender: kvm-owner@vger.kernel.org Precedence: bulk

Message ID

1422880199-12093-1-git-send-email-drjones@redhat.com (mailing list archive)

State

New, archived

Headers

From: Andrew Jones <drjones@redhat.com>
To: kvmarm@lists.cs.columbia.edu, kvm@vger.kernel.org
Cc: christoffer.dall@linaro.org, pbonzini@redhat.com
Subject: [PATCH v3 17/18] arm/arm64: add smp_boot_secondary
Date: Mon,  2 Feb 2015 13:29:59 +0100
Message-Id: <1422880199-12093-1-git-send-email-drjones@redhat.com>
In-Reply-To: <1422815686-24591-18-git-send-email-drjones@redhat.com>
References: <1422815686-24591-18-git-send-email-drjones@redhat.com>
Sender: kvm-owner@vger.kernel.org
Precedence: bulk

Commit Message

Andrew Jones Feb. 2, 2015, 12:29 p.m. UTC

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>
---
v3:
    - fix bug in cstart.S:secondary_entry, enable mmu before loading
      secondary_data to set the stack and exception stacks base
    - remove unnecessary asm-offsets:SECONDARY_DATA_STACK, just require
      stack to always be the first member of the struct
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        |  1 +
 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      |  1 +
 lib/arm64/asm/psci.h         |  2 ++
 lib/arm64/asm/smp.h          |  1 +
 13 files changed, 217 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

Comments

Andrew Jones Feb. 2, 2015, 2:35 p.m. UTC | #1

On Mon, Feb 02, 2015 at 01:29:59PM +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>
> ---
> v3:
>     - fix bug in cstart.S:secondary_entry, enable mmu before loading
>       secondary_data to set the stack and exception stacks base
>     - remove unnecessary asm-offsets:SECONDARY_DATA_STACK, just require
>       stack to always be the first member of the struct
> 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.

I'm back. And you guessed it, I need a v4. What an embarrassment this
patch is... I actually introduced two bugs with v2, while fixing one.
One step forward, two steps back. I didn't see a problem while testing
this time, but it popped into my head shortly after hitting enter on
git-send-email. (git-send-email is a great trigger for my brain, that's
when all the thinking starts. Too bad it doesn't work with --dry-run.)

So, v2 fixed secondary processors using cpu0's exception stacks, but
broke cpu0's use in the process. My plan had been to leave cpu0 on
the linker setup exception_stacks, but now that the handler looks at
the top of the stack for exception stacks for all cpus, we need to setup
cpu0's exception stacks the same way, and we can remove the one from the
linker script. v4 coming... Hopefully when I hit send my mind will just
go blank this time - like it generally is. I've added a v4 branch to
github as well.

https://github.com/rhdrjones/kvm-unit-tests/tree/arm/smp-v4


> 
>  arm/cstart.S                 | 41 +++++++++++++++++++++++++++++-----
>  arm/cstart64.S               | 25 +++++++++++++++++++++
>  config/config-arm-common.mak |  1 +
>  lib/arm/asm-offsets.c        |  1 +
>  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      |  1 +
>  lib/arm64/asm/psci.h         |  2 ++
>  lib/arm64/asm/smp.h          |  1 +
>  13 files changed, 217 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..3839549742fee 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:
> +	/* 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	r4, =secondary_data
> +	ldr	r0, [r4]
> +	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 +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..cdda13c17af9e 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]
> +	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..e389d928b3bf7 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)
>  {
> 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..4cb86b6ce3420
> --- /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;		/* 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_ */
> 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..980bf1fc8445b 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)
>  {
> 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@vger.kernel.org
More majordomo info at  http://vger.kernel.org/majordomo-info.html

diff --git a/arm/cstart.S b/arm/cstart.S
index 08a0b3ecc61f6..3839549742fee 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:
+	/* 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	r4, =secondary_data
+	ldr	r0, [r4]
+	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 +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..cdda13c17af9e 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]
+	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..e389d928b3bf7 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)
 {
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..4cb86b6ce3420
--- /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;		/* 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_ */
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..980bf1fc8445b 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)
 {
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"

[v3,17/18] arm/arm64: add smp_boot_secondary

Commit Message

Comments

Patch