[kvm-unit-tests PATCH 00/18] arm/arm64: add smp support

[kvm-unit-tests PATCH 00/18] arm/arm64: add smp support

[Andrew Jones]

This series extends the kvm-unit-tests/arm[64] framework to support smp.
A break down of the patches is as follows

01-02: prepare general framework for smp use
03-06: arm/arm64 fixups not 100% related to this series,
       but need to post some time...
07-09: add thread_info (for per-thread data) and suck some global
       data into it
10-11: add cpumask support (for per-cpu data) and suck some more
       global data in
   12: add arm64 simple spinlock implementation
13-14: add some PSCI support
15-16: further prep for smp_boot_secondary
   17: finally add smp_boot_secondary
   18: as usual, add a selftest to make sure it all works

These patches are also available here:
https://github.com/rhdrjones/kvm-unit-tests/tree/arm/smp

Thanks in advance for reviews!


Andrew Jones (18):
  x86: expose spin_lock/unlock to lib code
  lib/report: guard access to counters
  arm: fixups: add barriers, actually set MAIR
  arm64: fixup: use id_aa64mmfr0_el1 to set tcr
  arm/arm64: processor.[ch] cleanups
  arm/arm64: get rid of get_sp()
  arm/arm64: introduce thread_info
  arm/arm64: add per thread user_mode flag
  arm/arm64: maintain per thread exception handlers
  arm/arm64: add simple cpumask API
  arm/arm64: make mmu_on per cpu
  arm64: implement spinlocks
  arm/arm64: import include/uapi/linux/psci.h
  arm/arm64: add some PSCI API
  arm/arm64: add cpu_relax() and friends
  arm: clarify comment about exception stack use
  arm/arm64: add smp_boot_secondary
  arm/arm64: Add smp selftest

 arm/cstart.S                 |  47 +++++++++++++----
 arm/cstart64.S               |  48 +++++++++++-------
 arm/flat.lds                 |   6 +++
 arm/selftest.c               |  70 +++++++++++++++++++++++--
 arm/unittests.cfg            |  11 +++-
 config/config-arm-common.mak |   3 ++
 config/config-arm64.mak      |   1 +
 lib/arm/asm-offsets.c        |   3 ++
 lib/arm/asm/barrier.h        |   5 ++
 lib/arm/asm/bitops.h         |  53 +++++++++++++++++++
 lib/arm/asm/cpumask.h        | 118 +++++++++++++++++++++++++++++++++++++++++++
 lib/arm/asm/mmu-api.h        |   1 +
 lib/arm/asm/processor.h      |  12 ++++-
 lib/arm/asm/psci.h           |  15 ++++++
 lib/arm/asm/smp.h            |  56 ++++++++++++++++++++
 lib/arm/asm/thread_info.h    |  60 ++++++++++++++++++++++
 lib/arm/asm/uapi-psci.h      |  73 ++++++++++++++++++++++++++
 lib/arm/bitops.c             |  81 +++++++++++++++++++++++++++++
 lib/arm/mmu.c                |  15 ++++--
 lib/arm/processor.c          |  44 +++++++++++-----
 lib/arm/psci.c               |  49 ++++++++++++++++++
 lib/arm/setup.c              |  11 +++-
 lib/arm/smp.c                |  57 +++++++++++++++++++++
 lib/arm64/asm-offsets.c      |   2 +
 lib/arm64/asm/barrier.h      |   5 ++
 lib/arm64/asm/bitops.h       |  51 +++++++++++++++++++
 lib/arm64/asm/cpumask.h      |   1 +
 lib/arm64/asm/processor.h    |  18 ++++++-
 lib/arm64/asm/psci.h         |  15 ++++++
 lib/arm64/asm/smp.h          |   1 +
 lib/arm64/asm/spinlock.h     |   8 +--
 lib/arm64/asm/thread_info.h  |   1 +
 lib/arm64/asm/uapi-psci.h    |   1 +
 lib/arm64/processor.c        |  81 +++++++++++++++++++----------
 lib/arm64/spinlock.c         |  43 ++++++++++++++++
 lib/report.c                 |  16 ++++++
 lib/x86/asm/spinlock.h       |  11 ++++
 lib/x86/smp.h                |   7 +--
 38 files changed, 1008 insertions(+), 92 deletions(-)
 create mode 100644 lib/arm/asm/bitops.h
 create mode 100644 lib/arm/asm/cpumask.h
 create mode 100644 lib/arm/asm/psci.h
 create mode 100644 lib/arm/asm/smp.h
 create mode 100644 lib/arm/asm/thread_info.h
 create mode 100644 lib/arm/asm/uapi-psci.h
 create mode 100644 lib/arm/bitops.c
 create mode 100644 lib/arm/psci.c
 create mode 100644 lib/arm/smp.c
 create mode 100644 lib/arm64/asm/bitops.h
 create mode 100644 lib/arm64/asm/cpumask.h
 create mode 100644 lib/arm64/asm/psci.h
 create mode 100644 lib/arm64/asm/smp.h
 create mode 100644 lib/arm64/asm/thread_info.h
 create mode 100644 lib/arm64/asm/uapi-psci.h
 create mode 100644 lib/arm64/spinlock.c
 create mode 100644 lib/x86/asm/spinlock.h

-- 
1.9.3

[PATCH 01/18] x86: expose spin_lock/unlock to lib code

[Andrew Jones]

Move the spin_lock/unlock declarations to lib/x86/asm/spinlock.h,
allowing lib code, e.g. lib/report.c, to use spinlocks.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 lib/x86/asm/spinlock.h | 11 +++++++++++
 lib/x86/smp.h          |  7 +------
 2 files changed, 12 insertions(+), 6 deletions(-)
 create mode 100644 lib/x86/asm/spinlock.h

diff --git a/lib/x86/asm/spinlock.h b/lib/x86/asm/spinlock.h
new file mode 100644
index 0000000000000..4b0cb331c048b
--- /dev/null
+++ b/lib/x86/asm/spinlock.h
@@ -0,0 +1,11 @@
+#ifndef __ASM_SPINLOCK_H
+#define __ASM_SPINLOCK_H
+
+struct spinlock {
+    int v;
+};
+
+void spin_lock(struct spinlock *lock);
+void spin_unlock(struct spinlock *lock);
+
+#endif
diff --git a/lib/x86/smp.h b/lib/x86/smp.h
index df5fdba9b9288..566018f49ba31 100644
--- a/lib/x86/smp.h
+++ b/lib/x86/smp.h
@@ -1,21 +1,16 @@
 #ifndef __SMP_H
 #define __SMP_H
+#include <asm/spinlock.h>
 
 #define mb() 	asm volatile("mfence":::"memory")
 #define rmb()	asm volatile("lfence":::"memory")
 #define wmb()	asm volatile("sfence" ::: "memory")
 
-struct spinlock {
-    int v;
-};
-
 void smp_init(void);
 
 int cpu_count(void);
 int smp_id(void);
 void on_cpu(int cpu, void (*function)(void *data), void *data);
 void on_cpu_async(int cpu, void (*function)(void *data), void *data);
-void spin_lock(struct spinlock *lock);
-void spin_unlock(struct spinlock *lock);
 
 #endif
-- 
1.9.3

[PATCH 02/18] lib/report: guard access to counters

[Andrew Jones]

Use a lock to avoid exposing the counters, and any other
global data, to potential races.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 lib/report.c | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/lib/report.c b/lib/report.c
index dc30250c676d3..35e664108a921 100644
--- a/lib/report.c
+++ b/lib/report.c
@@ -11,20 +11,26 @@
  */
 
 #include "libcflat.h"
+#include "asm/spinlock.h"
 
 static unsigned int tests, failures, xfailures;
 static char prefixes[256];
+static struct spinlock lock;
 
 void report_prefix_push(const char *prefix)
 {
+	spin_lock(&lock);
 	strcat(prefixes, prefix);
 	strcat(prefixes, ": ");
+	spin_unlock(&lock);
 }
 
 void report_prefix_pop(void)
 {
 	char *p, *q;
 
+	spin_lock(&lock);
+
 	if (!*prefixes)
 		return;
 
@@ -33,6 +39,8 @@ void report_prefix_pop(void)
 			p = q, q = strstr(p, ": ") + 2)
 		;
 	*p = '\0';
+
+	spin_unlock(&lock);
 }
 
 void va_report_xfail(const char *msg_fmt, bool xfail, bool cond, va_list va)
@@ -41,6 +49,8 @@ void va_report_xfail(const char *msg_fmt, bool xfail, bool cond, va_list va)
 	char *fail = xfail ? "XFAIL" : "FAIL";
 	char buf[2000];
 
+	spin_lock(&lock);
+
 	tests++;
 	printf("%s: ", cond ? pass : fail);
 	puts(prefixes);
@@ -53,6 +63,8 @@ void va_report_xfail(const char *msg_fmt, bool xfail, bool cond, va_list va)
 		xfailures++;
 	else if (!cond)
 		failures++;
+
+	spin_unlock(&lock);
 }
 
 void report(const char *msg_fmt, bool pass, ...)
@@ -73,10 +85,14 @@ void report_xfail(const char *msg_fmt, bool xfail, bool pass, ...)
 
 int report_summary(void)
 {
+	spin_lock(&lock);
+
 	printf("\nSUMMARY: %d tests, %d unexpected failures", tests, failures);
 	if (xfailures)
 		printf(", %d expected failures\n", xfailures);
 	else
 		printf("\n");
 	return failures > 0 ? 1 : 0;
+
+	spin_unlock(&lock);
 }
-- 
1.9.3

[PATCH 03/18] arm: fixups: add barriers, actually set MAIR

[Andrew Jones]

Sprinkle in some more isbs after context-changing operations,
as the ARM ARM states we should. I haven't seen any problems
without them, but we should do it right. Also, *actually* set
the MAIR in asm_mmu_enable. We were reading, not writing...
Luckily this was just spotted while adding the isbs, rather
than leading to a nightmare debug session some day...

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 arm/cstart.S        | 8 ++++++--
 lib/arm/processor.c | 1 +
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/arm/cstart.S b/arm/cstart.S
index da496e9eae7e0..39fac8f1e1bd8 100644
--- a/arm/cstart.S
+++ b/arm/cstart.S
@@ -50,6 +50,7 @@ start:
 .macro set_mode_stack mode, stack
 	add	\stack, #S_FRAME_SIZE
 	msr	cpsr_c, #(\mode | PSR_I_BIT | PSR_F_BIT)
+	isb
 	mov	sp, \stack
 .endm
 
@@ -70,6 +71,7 @@ exceptions_init:
 	set_mode_stack	FIQ_MODE, r1
 
 	msr	cpsr_cxsf, r2		@ back to svc mode
+	isb
 	mov	pc, lr
 
 .text
@@ -96,12 +98,13 @@ asm_mmu_enable:
 
 	/* MAIR */
 	ldr	r2, =PRRR
-	mrc	p15, 0, r2, c10, c2, 0
+	mcr	p15, 0, r2, c10, c2, 0
 	ldr	r2, =NMRR
-	mrc	p15, 0, r2, c10, c2, 1
+	mcr	p15, 0, r2, c10, c2, 1
 
 	/* TTBR0 */
 	mcrr	p15, 0, r0, r1, c2
+	isb
 
 	/* SCTLR */
 	mrc	p15, 0, r2, c1, c0, 0
@@ -109,6 +112,7 @@ asm_mmu_enable:
 	orr	r2, #CR_I
 	orr	r2, #CR_M
 	mcr	p15, 0, r2, c1, c0, 0
+	isb
 
 	mov     pc, lr
 
diff --git a/lib/arm/processor.c b/lib/arm/processor.c
index da4163664a835..f8bd94cbb8bc6 100644
--- a/lib/arm/processor.c
+++ b/lib/arm/processor.c
@@ -115,6 +115,7 @@ void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr)
 		"bic	r0, #" xstr(MODE_MASK) "\n"
 		"orr	r0, #" xstr(USR_MODE) "\n"
 		"msr	cpsr_c, r0\n"
+		"isb\n"
 		"mov	r0, %0\n"
 		"mov	sp, %1\n"
 		"mov	pc, %2\n"
-- 
1.9.3

[PATCH 04/18] arm64: fixup: use id_aa64mmfr0_el1 to set tcr

[Andrew Jones]

We shouldn't assume we can set tcr_el1.ips to 42 bits. Set
it based on what we read from id_aa64mmfr0_el1. Didn't see
a problem, but might as well be correct.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 arm/cstart64.S | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/arm/cstart64.S b/arm/cstart64.S
index 5151f4c77d745..9047e7ef14646 100644
--- a/arm/cstart64.S
+++ b/arm/cstart64.S
@@ -92,7 +92,7 @@ asm_mmu_enable:
 		     TCR_TG0_64K | TCR_TG1_64K |	\
 		     TCR_IRGN_WBWA | TCR_ORGN_WBWA |	\
 		     TCR_SHARED
-	mov	x2, #3			// 011 is 42 bits
+	mrs	x2, id_aa64mmfr0_el1
 	bfi	x1, x2, #32, #3
 	msr	tcr_el1, x1
 
-- 
1.9.3

[PATCH 05/18] arm/arm64: processor.[ch] cleanups

[Andrew Jones]

Add 'const' to a few global arrays that are constant. Also,
no need for default_vector_handler to be static. unittests
may want to reset vector handlers to it.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 lib/arm/processor.c       | 2 +-
 lib/arm64/asm/processor.h | 2 ++
 lib/arm64/processor.c     | 8 ++++----
 3 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/lib/arm/processor.c b/lib/arm/processor.c
index f8bd94cbb8bc6..5ad999c6b378b 100644
--- a/lib/arm/processor.c
+++ b/lib/arm/processor.c
@@ -20,7 +20,7 @@ static const char *processor_modes[] = {
 	"UK12_32", "UK13_32", "UK14_32", "SYS_32"
 };
 
-static char *vector_names[] = {
+static const char *vector_names[] = {
 	"rst", "und", "svc", "pabt", "dabt", "addrexcptn", "irq", "fiq"
 };
 
diff --git a/lib/arm64/asm/processor.h b/lib/arm64/asm/processor.h
index f73ffb5e4bc95..a33f70afb3a3e 100644
--- a/lib/arm64/asm/processor.h
+++ b/lib/arm64/asm/processor.h
@@ -47,6 +47,8 @@ typedef void (*exception_fn)(struct pt_regs *regs, unsigned int esr);
 extern void install_vector_handler(enum vector v, vector_fn fn);
 extern void install_exception_handler(enum vector v, unsigned int ec,
 				      exception_fn fn);
+extern void default_vector_handler(enum vector v, struct pt_regs *regs,
+				   unsigned int esr);
 
 extern void show_regs(struct pt_regs *regs);
 extern void *get_sp(void);
diff --git a/lib/arm64/processor.c b/lib/arm64/processor.c
index 7dc0b2e026134..efc78353d7ec0 100644
--- a/lib/arm64/processor.c
+++ b/lib/arm64/processor.c
@@ -10,7 +10,7 @@
 #include <asm/processor.h>
 #include <asm/esr.h>
 
-static char *vector_names[] = {
+static const char *vector_names[] = {
 	"el1t_sync",
 	"el1t_irq",
 	"el1t_fiq",
@@ -29,7 +29,7 @@ static char *vector_names[] = {
 	"el0_error_32",
 };
 
-static char *ec_names[EC_MAX] = {
+static const char *ec_names[EC_MAX] = {
 	[ESR_EL1_EC_UNKNOWN]		= "UNKNOWN",
 	[ESR_EL1_EC_WFI]		= "WFI",
 	[ESR_EL1_EC_CP15_32]		= "CP15_32",
@@ -142,8 +142,8 @@ void install_exception_handler(enum vector v, unsigned int ec, exception_fn fn)
 		exception_handlers[v][ec] = fn;
 }
 
-static void default_vector_handler(enum vector v, struct pt_regs *regs,
-				   unsigned int esr)
+void default_vector_handler(enum vector v, struct pt_regs *regs,
+			    unsigned int esr)
 {
 	unsigned int ec = esr >> ESR_EL1_EC_SHIFT;
 
-- 
1.9.3

[PATCH 06/18] arm/arm64: get rid of get_sp()

[Andrew Jones]

get_sp() only worked by accident, because gcc inlined calls
to it. It should have always been explicitly inlined. It was
also only added for debugging, and not in any use now.
Furthermore, while we will have need for a "get_sp", we'll
add it back with a new name, current_stack_pointer, in order
to be consistent with Linux.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 lib/arm/asm/processor.h   | 1 -
 lib/arm/processor.c       | 6 ------
 lib/arm64/asm/processor.h | 1 -
 lib/arm64/processor.c     | 6 ------
 4 files changed, 14 deletions(-)

diff --git a/lib/arm/asm/processor.h b/lib/arm/asm/processor.h
index a56f8d1fc9797..9c37db66640e8 100644
--- a/lib/arm/asm/processor.h
+++ b/lib/arm/asm/processor.h
@@ -23,7 +23,6 @@ typedef void (*exception_fn)(struct pt_regs *);
 extern void install_exception_handler(enum vector v, exception_fn fn);
 
 extern void show_regs(struct pt_regs *regs);
-extern void *get_sp(void);
 
 static inline unsigned long current_cpsr(void)
 {
diff --git a/lib/arm/processor.c b/lib/arm/processor.c
index 5ad999c6b378b..d2fd597fcd139 100644
--- a/lib/arm/processor.c
+++ b/lib/arm/processor.c
@@ -64,12 +64,6 @@ void show_regs(struct pt_regs *regs)
 	}
 }
 
-void *get_sp(void)
-{
-	register unsigned long sp asm("sp");
-	return (void *)sp;
-}
-
 static exception_fn exception_handlers[EXCPTN_MAX];
 
 void install_exception_handler(enum vector v, exception_fn fn)
diff --git a/lib/arm64/asm/processor.h b/lib/arm64/asm/processor.h
index a33f70afb3a3e..d287f55b8dac6 100644
--- a/lib/arm64/asm/processor.h
+++ b/lib/arm64/asm/processor.h
@@ -51,7 +51,6 @@ extern void default_vector_handler(enum vector v, struct pt_regs *regs,
 				   unsigned int esr);
 
 extern void show_regs(struct pt_regs *regs);
-extern void *get_sp(void);
 extern bool get_far(unsigned int esr, unsigned long *far);
 
 static inline unsigned long current_level(void)
diff --git a/lib/arm64/processor.c b/lib/arm64/processor.c
index efc78353d7ec0..7f61b3fff281f 100644
--- a/lib/arm64/processor.c
+++ b/lib/arm64/processor.c
@@ -78,12 +78,6 @@ void show_regs(struct pt_regs *regs)
 	printf("\n");
 }
 
-void *get_sp(void)
-{
-	register unsigned long sp asm("sp");
-	return (void *)sp;
-}
-
 bool get_far(unsigned int esr, unsigned long *far)
 {
 	unsigned int ec = esr >> ESR_EL1_EC_SHIFT;
-- 
1.9.3

[PATCH 07/18] arm/arm64: introduce thread_info

[Andrew Jones]

For smp we need a way to maintain thread local state. The bottom
of the thread stack is a good place, and is where Linux puts it.
So we just steal the concept of the thread_info structure that
lives at the bottom of the stack in Linux, and introduce it to
kvm-unit-tests/arm[64]. For starters we just have cpu index for
state, and that's implicitly initialized to zero for CPU0 already.
So, as we don't have secondary cpus yet, there's not much to do.

Additionally, sneak a small fixup in to the initial stack setup
for arm64. We were assuming that spsel is EL1 after reset, which
has been true so far, but let's not assume.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 arm/cstart.S                |  2 +-
 arm/cstart64.S              |  5 ++++-
 arm/flat.lds                |  6 ++++++
 arm/selftest.c              |  8 ++++----
 lib/arm/asm/thread_info.h   | 27 +++++++++++++++++++++++++++
 lib/arm64/asm/thread_info.h |  1 +
 6 files changed, 43 insertions(+), 6 deletions(-)
 create mode 100644 lib/arm/asm/thread_info.h
 create mode 100644 lib/arm64/asm/thread_info.h

diff --git a/arm/cstart.S b/arm/cstart.S
index 39fac8f1e1bd8..fd02aaab268d7 100644
--- a/arm/cstart.S
+++ b/arm/cstart.S
@@ -27,7 +27,7 @@ start:
 	 * put the dtb in r0. This allows setup to be consistent
 	 * with arm64.
 	 */
-	ldr	sp, =stacktop
+	ldr	sp, =stackptr
 	mov	r0, r2
 	push	{r0-r1}
 
diff --git a/arm/cstart64.S b/arm/cstart64.S
index 9047e7ef14646..2fe15eb1d3972 100644
--- a/arm/cstart64.S
+++ b/arm/cstart64.S
@@ -21,7 +21,10 @@ start:
 	 * The physical address of the dtb is in x0, x1-x3 are reserved
 	 * See the kernel doc Documentation/arm64/booting.txt
 	 */
-	adr     x4, stacktop
+	mov	x4, #1
+	msr	spsel, x4
+	isb
+	adr     x4, stackptr
 	mov	sp, x4
 	stp	x0, x1, [sp, #-16]!
 
diff --git a/arm/flat.lds b/arm/flat.lds
index a8849ee0939a8..df80d3678e556 100644
--- a/arm/flat.lds
+++ b/arm/flat.lds
@@ -18,6 +18,12 @@ SECTIONS
     edata = .;
     . += 64K;
     . = ALIGN(64K);
+    /*
+     * stack depth is ~16K, see THREAD_SIZE
+     * sp must be 16 byte aligned for arm64, and 8 byte aligned for arm
+     * sp must always be strictly less than the true stacktop
+     */
+    stackptr = . - 16;
     stacktop = .;
 }
 
diff --git a/arm/selftest.c b/arm/selftest.c
index de816f8142c54..05ca7efe95f83 100644
--- a/arm/selftest.c
+++ b/arm/selftest.c
@@ -11,7 +11,7 @@
 #include <asm/ptrace.h>
 #include <asm/asm-offsets.h>
 #include <asm/processor.h>
-#include <asm/page.h>
+#include <asm/thread_info.h>
 
 static void assert_args(int num_args, int needed_args)
 {
@@ -313,9 +313,9 @@ int main(int argc, char **argv)
 
 	} else if (strcmp(argv[0], "vectors-user") == 0) {
 
-		void *sp = memalign(PAGE_SIZE, PAGE_SIZE);
-		memset(sp, 0, PAGE_SIZE);
-		start_usr(check_vectors, NULL, (unsigned long)sp + PAGE_SIZE);
+		void *sp = memalign(THREAD_SIZE, THREAD_SIZE);
+		start_usr(check_vectors, NULL,
+				(unsigned long)sp + THREAD_START_SP);
 	}
 
 	return report_summary();
diff --git a/lib/arm/asm/thread_info.h b/lib/arm/asm/thread_info.h
new file mode 100644
index 0000000000000..ea86f142a7d93
--- /dev/null
+++ b/lib/arm/asm/thread_info.h
@@ -0,0 +1,27 @@
+#ifndef _ASMARM_THREAD_INFO_H_
+#define _ASMARM_THREAD_INFO_H_
+/*
+ * Adapted from arch/arm64/include/asm/thread_info.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.
+ */
+
+#define THREAD_SIZE		16384
+#define THREAD_START_SP		(THREAD_SIZE - 16)
+
+struct thread_info {
+	int cpu;
+	char ext[0];		/* allow unit tests to add extended info */
+};
+
+register unsigned long current_stack_pointer asm("sp");
+
+static inline struct thread_info *current_thread_info(void)
+{
+	return (struct thread_info *)
+		(current_stack_pointer & ~(THREAD_SIZE - 1));
+}
+
+#endif /* _ASMARM_THREAD_INFO_H_ */
diff --git a/lib/arm64/asm/thread_info.h b/lib/arm64/asm/thread_info.h
new file mode 100644
index 0000000000000..b01fa8f34b3bf
--- /dev/null
+++ b/lib/arm64/asm/thread_info.h
@@ -0,0 +1 @@
+#include "../../arm/asm/thread_info.h"
-- 
1.9.3

[PATCH 08/18] arm/arm64: add per thread user_mode flag

[Andrew Jones]

While current_mode() == USR_MODE works on armv7 from PL0 to check
if we're in user mode, current_mode() would require reading a
privileged register on armv8. To work around this, on arm64 we
introduced a 'user_mode' variable. This variable needs to be per
thread now. Rather than starting to pollute thread_info with a
bunch of bools, create a flags field and a TIF_USER_MODE flag to
replace it. Use it on armv7 too for consistency. Also, now that
we need to create a thread_info initializer, add mpidr utilities
for setting thread_info->cpu.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 arm/cstart64.S            | 16 +---------------
 arm/selftest.c            |  2 +-
 lib/arm/asm/processor.h   | 11 +++++++++++
 lib/arm/asm/thread_info.h | 13 +++++++++++--
 lib/arm/processor.c       | 14 ++++++++++++++
 lib/arm/setup.c           |  3 +++
 lib/arm64/asm/processor.h | 14 +++++++++++++-
 lib/arm64/processor.c     | 15 +++++++++++++--
 8 files changed, 67 insertions(+), 21 deletions(-)

diff --git a/arm/cstart64.S b/arm/cstart64.S
index 2fe15eb1d3972..58e4040cfb40f 100644
--- a/arm/cstart64.S
+++ b/arm/cstart64.S
@@ -156,13 +156,7 @@ asm_mmu_enable:
 	mrs	x2, spsr_el1
 	stp	x1, x2, [sp, #S_PC]
 
-	and	x2, x2, #PSR_MODE_MASK
-	cmp	x2, #PSR_MODE_EL0t
-	b.ne	1f
-	adr	x2, user_mode
-	str	xzr, [x2]		/* we're in kernel mode now */
-
-1:	mov	x0, \vec
+	mov	x0, \vec
 	mov	x1, sp
 	mrs	x2, esr_el1
 	bl	do_handle_exception
@@ -171,14 +165,6 @@ asm_mmu_enable:
 	msr	spsr_el1, x2
 	msr	elr_el1, x1
 
-	and	x2, x2, #PSR_MODE_MASK
-	cmp	x2, #PSR_MODE_EL0t
-	b.ne	1f
-	adr	x2, user_mode
-	mov	x1, #1
-	str	x1, [x2]		/* we're going back to user mode */
-
-1:
 	.if \vec >= 8
 	ldr	x1, [sp, #S_SP]
 	msr	sp_el0, x1
diff --git a/arm/selftest.c b/arm/selftest.c
index 05ca7efe95f83..d77495747b08a 100644
--- a/arm/selftest.c
+++ b/arm/selftest.c
@@ -240,7 +240,7 @@ static enum vector check_vector_prep(void)
 {
 	unsigned long daif;
 
-	if (user_mode)
+	if (is_user())
 		return EL0_SYNC_64;
 
 	asm volatile("mrs %0, daif" : "=r" (daif) ::);
diff --git a/lib/arm/asm/processor.h b/lib/arm/asm/processor.h
index 9c37db66640e8..f25e7eee3666c 100644
--- a/lib/arm/asm/processor.h
+++ b/lib/arm/asm/processor.h
@@ -33,6 +33,17 @@ static inline unsigned long current_cpsr(void)
 
 #define current_mode() (current_cpsr() & MODE_MASK)
 
+static inline unsigned int get_mpidr(void)
+{
+	unsigned int mpidr;
+	asm volatile("mrc p15, 0, %0, c0, c0, 5" : "=r" (mpidr));
+	return mpidr;
+}
+
+/* Only support Aff0 for now, up to 4 cpus */
+#define mpidr_to_cpu(mpidr) ((int)((mpidr) & 0xff))
+
 extern void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr);
+extern bool is_user(void);
 
 #endif /* _ASMARM_PROCESSOR_H_ */
diff --git a/lib/arm/asm/thread_info.h b/lib/arm/asm/thread_info.h
index ea86f142a7d93..17997e21d1274 100644
--- a/lib/arm/asm/thread_info.h
+++ b/lib/arm/asm/thread_info.h
@@ -11,17 +11,26 @@
 #define THREAD_SIZE		16384
 #define THREAD_START_SP		(THREAD_SIZE - 16)
 
+#define TIF_USER_MODE		(1U << 0)
+
 struct thread_info {
 	int cpu;
+	unsigned int flags;
 	char ext[0];		/* allow unit tests to add extended info */
 };
 
+static inline struct thread_info *thread_info_sp(unsigned long sp)
+{
+	return (struct thread_info *)(sp & ~(THREAD_SIZE - 1));
+}
+
 register unsigned long current_stack_pointer asm("sp");
 
 static inline struct thread_info *current_thread_info(void)
 {
-	return (struct thread_info *)
-		(current_stack_pointer & ~(THREAD_SIZE - 1));
+	return thread_info_sp(current_stack_pointer);
 }
 
+extern void thread_info_init(struct thread_info *ti, unsigned int flags);
+
 #endif /* _ASMARM_THREAD_INFO_H_ */
diff --git a/lib/arm/processor.c b/lib/arm/processor.c
index d2fd597fcd139..8a514a29c063b 100644
--- a/lib/arm/processor.c
+++ b/lib/arm/processor.c
@@ -100,10 +100,19 @@ void do_handle_exception(enum vector v, struct pt_regs *regs)
 	abort();
 }
 
+void thread_info_init(struct thread_info *ti, unsigned int flags)
+{
+	memset(ti, 0, sizeof(struct thread_info));
+	ti->cpu = mpidr_to_cpu(get_mpidr());
+	ti->flags = flags;
+}
+
 void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr)
 {
 	sp_usr &= (~7UL); /* stack ptr needs 8-byte alignment */
 
+	thread_info_init(thread_info_sp(sp_usr), TIF_USER_MODE);
+
 	asm volatile(
 		"mrs	r0, cpsr\n"
 		"bic	r0, #" xstr(MODE_MASK) "\n"
@@ -115,3 +124,8 @@ void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr)
 		"mov	pc, %2\n"
 	:: "r" (arg), "r" (sp_usr), "r" (func) : "r0");
 }
+
+bool is_user(void)
+{
+	return current_thread_info()->flags & TIF_USER_MODE;
+}
diff --git a/lib/arm/setup.c b/lib/arm/setup.c
index 8f58802e958ac..b30c8696f6539 100644
--- a/lib/arm/setup.c
+++ b/lib/arm/setup.c
@@ -14,6 +14,7 @@
 #include <libfdt/libfdt.h>
 #include <devicetree.h>
 #include <alloc.h>
+#include <asm/thread_info.h>
 #include <asm/setup.h>
 #include <asm/page.h>
 #include <asm/mmu.h>
@@ -79,6 +80,8 @@ void setup(const void *fdt)
 	io_init();
 	cpu_init();
 
+	thread_info_init(current_thread_info(), 0);
+
 	assert(dt_get_bootargs(&bootargs) == 0);
 	setup_args(bootargs);
 }
diff --git a/lib/arm64/asm/processor.h b/lib/arm64/asm/processor.h
index d287f55b8dac6..c1326351d201f 100644
--- a/lib/arm64/asm/processor.h
+++ b/lib/arm64/asm/processor.h
@@ -60,8 +60,20 @@ static inline unsigned long current_level(void)
 	return el & 0xc;
 }
 
-extern bool user_mode;
+#define DEFINE_GET_SYSREG32(reg)				\
+static inline unsigned int get_##reg(void)			\
+{								\
+	unsigned int reg;					\
+	asm volatile("mrs %0, " #reg "_el1" : "=r" (reg));	\
+	return reg;						\
+}
+DEFINE_GET_SYSREG32(mpidr)
+
+/* Only support Aff0 for now, gicv2 only */
+#define mpidr_to_cpu(mpidr) ((int)((mpidr) & 0xff))
+
 extern void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr);
+extern bool is_user(void);
 
 #endif /* !__ASSEMBLY__ */
 #endif /* _ASMARM64_PROCESSOR_H_ */
diff --git a/lib/arm64/processor.c b/lib/arm64/processor.c
index 7f61b3fff281f..152767eecf062 100644
--- a/lib/arm64/processor.c
+++ b/lib/arm64/processor.c
@@ -168,12 +168,18 @@ void install_vector_handler(enum vector v, vector_fn fn)
 		vector_handlers[v] = fn;
 }
 
-bool user_mode;
+void thread_info_init(struct thread_info *ti, unsigned int flags)
+{
+	memset(ti, 0, sizeof(struct thread_info));
+	ti->cpu = mpidr_to_cpu(get_mpidr());
+	ti->flags = flags;
+}
+
 void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr)
 {
 	sp_usr &= (~15UL); /* stack ptr needs 16-byte alignment */
 
-	user_mode = true;
+	thread_info_init(thread_info_sp(sp_usr), TIF_USER_MODE);
 
 	asm volatile(
 		"mov	x0, %0\n"
@@ -184,3 +190,8 @@ void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr)
 		"eret\n"
 	:: "r" (arg), "r" (sp_usr), "r" (func) : "x0", "x3");
 }
+
+bool is_user(void)
+{
+	return current_thread_info()->flags & TIF_USER_MODE;
+}
-- 
1.9.3

[PATCH 09/18] arm/arm64: maintain per thread exception handlers

[Andrew Jones]

Add exception handlers to thread info. And, since we allow threads
running in user mode to install exception handlers too (a convenience
for unit test developers), check for handlers on the user mode stack
thread info too. But, unit test developers will likely also expect the
installation of exception handlers done in kernel mode, before switching
to user mode, to work. So, if there's no handler in the thread info
hanging off the user mode stack, then still check the kernel mode stack
thread info for one.

Use THREAD_SIZE == PAGE_SIZE, when PAGE_SIZE is larger than 16K.
This is for arm64, which uses 64K pages, because the exception
handler arrays are 8K together, making the stack too small with
THREAD_SIZE == 16K.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 arm/flat.lds              |  2 +-
 lib/arm/asm/thread_info.h | 17 ++++++++++++++--
 lib/arm/processor.c       | 21 ++++++++++++++-----
 lib/arm64/asm/processor.h |  1 +
 lib/arm64/processor.c     | 52 ++++++++++++++++++++++++++++++++++-------------
 5 files changed, 71 insertions(+), 22 deletions(-)

diff --git a/arm/flat.lds b/arm/flat.lds
index df80d3678e556..c01f01e7b8682 100644
--- a/arm/flat.lds
+++ b/arm/flat.lds
@@ -19,7 +19,7 @@ SECTIONS
     . += 64K;
     . = ALIGN(64K);
     /*
-     * stack depth is ~16K, see THREAD_SIZE
+     * stack depth is 16K for arm and PAGE_SIZE for arm64, see THREAD_SIZE
      * sp must be 16 byte aligned for arm64, and 8 byte aligned for arm
      * sp must always be strictly less than the true stacktop
      */
diff --git a/lib/arm/asm/thread_info.h b/lib/arm/asm/thread_info.h
index 17997e21d1274..5f7104f7c234f 100644
--- a/lib/arm/asm/thread_info.h
+++ b/lib/arm/asm/thread_info.h
@@ -7,8 +7,15 @@
  *
  * This work is licensed under the terms of the GNU LGPL, version 2.
  */
-
-#define THREAD_SIZE		16384
+#include <asm/processor.h>
+#include <asm/page.h>
+
+#define __MIN_THREAD_SIZE	16384
+#if PAGE_SIZE > __MIN_THREAD_SIZE
+#define THREAD_SIZE		PAGE_SIZE
+#else
+#define THREAD_SIZE		__MIN_THREAD_SIZE
+#endif
 #define THREAD_START_SP		(THREAD_SIZE - 16)
 
 #define TIF_USER_MODE		(1U << 0)
@@ -16,6 +23,12 @@
 struct thread_info {
 	int cpu;
 	unsigned int flags;
+#ifdef __arm__
+	exception_fn exception_handlers[EXCPTN_MAX];
+#else
+	vector_fn vector_handlers[VECTOR_MAX];
+	exception_fn exception_handlers[VECTOR_MAX][EC_MAX];
+#endif
 	char ext[0];		/* allow unit tests to add extended info */
 };
 
diff --git a/lib/arm/processor.c b/lib/arm/processor.c
index 8a514a29c063b..1cef46ab28647 100644
--- a/lib/arm/processor.c
+++ b/lib/arm/processor.c
@@ -8,6 +8,7 @@
 #include <libcflat.h>
 #include <asm/ptrace.h>
 #include <asm/processor.h>
+#include <asm/thread_info.h>
 
 static const char *processor_modes[] = {
 	"USER_26", "FIQ_26" , "IRQ_26" , "SVC_26" ,
@@ -64,18 +65,28 @@ void show_regs(struct pt_regs *regs)
 	}
 }
 
-static exception_fn exception_handlers[EXCPTN_MAX];
-
 void install_exception_handler(enum vector v, exception_fn fn)
 {
+	struct thread_info *ti = current_thread_info();
+
 	if (v < EXCPTN_MAX)
-		exception_handlers[v] = fn;
+		ti->exception_handlers[v] = fn;
 }
 
 void do_handle_exception(enum vector v, struct pt_regs *regs)
 {
-	if (v < EXCPTN_MAX && exception_handlers[v]) {
-		exception_handlers[v](regs);
+	struct thread_info *ti = thread_info_sp(regs->ARM_sp);
+
+	if (ti->flags & TIF_USER_MODE) {
+		if (v < EXCPTN_MAX && ti->exception_handlers[v]) {
+			ti->exception_handlers[v](regs);
+			return;
+		}
+		ti = current_thread_info();
+	}
+
+	if (v < EXCPTN_MAX && ti->exception_handlers[v]) {
+		ti->exception_handlers[v](regs);
 		return;
 	}
 
diff --git a/lib/arm64/asm/processor.h b/lib/arm64/asm/processor.h
index c1326351d201f..228a21c7f8856 100644
--- a/lib/arm64/asm/processor.h
+++ b/lib/arm64/asm/processor.h
@@ -49,6 +49,7 @@ extern void install_exception_handler(enum vector v, unsigned int ec,
 				      exception_fn fn);
 extern void default_vector_handler(enum vector v, struct pt_regs *regs,
 				   unsigned int esr);
+extern void vector_handlers_default_init(vector_fn *handlers);
 
 extern void show_regs(struct pt_regs *regs);
 extern bool get_far(unsigned int esr, unsigned long *far);
diff --git a/lib/arm64/processor.c b/lib/arm64/processor.c
index 152767eecf062..c240ce33c3d0c 100644
--- a/lib/arm64/processor.c
+++ b/lib/arm64/processor.c
@@ -9,6 +9,7 @@
 #include <asm/ptrace.h>
 #include <asm/processor.h>
 #include <asm/esr.h>
+#include <asm/thread_info.h>
 
 static const char *vector_names[] = {
 	"el1t_sync",
@@ -128,44 +129,66 @@ static void bad_exception(enum vector v, struct pt_regs *regs,
 	abort();
 }
 
-static exception_fn exception_handlers[VECTOR_MAX][EC_MAX];
-
 void install_exception_handler(enum vector v, unsigned int ec, exception_fn fn)
 {
+	struct thread_info *ti = current_thread_info();
+
 	if (v < VECTOR_MAX && ec < EC_MAX)
-		exception_handlers[v][ec] = fn;
+		ti->exception_handlers[v][ec] = fn;
 }
 
 void default_vector_handler(enum vector v, struct pt_regs *regs,
 			    unsigned int esr)
 {
+	struct thread_info *ti = thread_info_sp(regs->sp);
 	unsigned int ec = esr >> ESR_EL1_EC_SHIFT;
 
-	if (ec < EC_MAX && exception_handlers[v][ec])
-		exception_handlers[v][ec](regs, esr);
+	if (ti->flags & TIF_USER_MODE) {
+		if (ec < EC_MAX && ti->exception_handlers[v][ec]) {
+			ti->exception_handlers[v][ec](regs, esr);
+			return;
+		}
+		ti = current_thread_info();
+	}
+
+	if (ec < EC_MAX && ti->exception_handlers[v][ec])
+		ti->exception_handlers[v][ec](regs, esr);
 	else
 		bad_exception(v, regs, esr, false);
 }
 
-static vector_fn vector_handlers[VECTOR_MAX] = {
-	[EL1H_SYNC]	= default_vector_handler,
-	[EL1H_IRQ]	= default_vector_handler,
-	[EL0_SYNC_64]	= default_vector_handler,
-	[EL0_IRQ_64]	= default_vector_handler,
-};
+void vector_handlers_default_init(vector_fn *handlers)
+{
+	handlers[EL1H_SYNC]	= default_vector_handler;
+	handlers[EL1H_IRQ]	= default_vector_handler;
+	handlers[EL0_SYNC_64]	= default_vector_handler;
+	handlers[EL0_IRQ_64]	= default_vector_handler;
+}
 
 void do_handle_exception(enum vector v, struct pt_regs *regs, unsigned int esr)
 {
-	if (v < VECTOR_MAX && vector_handlers[v])
-		vector_handlers[v](v, regs, esr);
+	struct thread_info *ti = thread_info_sp(regs->sp);
+
+	if (ti->flags & TIF_USER_MODE) {
+		if (v < VECTOR_MAX && ti->vector_handlers[v]) {
+			ti->vector_handlers[v](v, regs, esr);
+			return;
+		}
+		ti = current_thread_info();
+	}
+
+	if (v < VECTOR_MAX && ti->vector_handlers[v])
+		ti->vector_handlers[v](v, regs, esr);
 	else
 		bad_exception(v, regs, esr, true);
 }
 
 void install_vector_handler(enum vector v, vector_fn fn)
 {
+	struct thread_info *ti = current_thread_info();
+
 	if (v < VECTOR_MAX)
-		vector_handlers[v] = fn;
+		ti->vector_handlers[v] = fn;
 }
 
 void thread_info_init(struct thread_info *ti, unsigned int flags)
@@ -173,6 +196,7 @@ void thread_info_init(struct thread_info *ti, unsigned int flags)
 	memset(ti, 0, sizeof(struct thread_info));
 	ti->cpu = mpidr_to_cpu(get_mpidr());
 	ti->flags = flags;
+	vector_handlers_default_init(ti->vector_handlers);
 }
 
 void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr)
-- 
1.9.3

[PATCH 10/18] arm/arm64: add simple cpumask API

[Andrew Jones]

On smp, cpumasks become quite useful. Add a simple implementation,
along with implementations of bitops it needs.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 config/config-arm-common.mak |   1 +
 lib/arm/asm/bitops.h         |  53 +++++++++++++++++++
 lib/arm/asm/cpumask.h        | 118 +++++++++++++++++++++++++++++++++++++++++++
 lib/arm/bitops.c             |  81 +++++++++++++++++++++++++++++
 lib/arm64/asm/bitops.h       |  51 +++++++++++++++++++
 lib/arm64/asm/cpumask.h      |   1 +
 6 files changed, 305 insertions(+)
 create mode 100644 lib/arm/asm/bitops.h
 create mode 100644 lib/arm/asm/cpumask.h
 create mode 100644 lib/arm/bitops.c
 create mode 100644 lib/arm64/asm/bitops.h
 create mode 100644 lib/arm64/asm/cpumask.h

diff --git a/config/config-arm-common.mak b/config/config-arm-common.mak
index b01e9ab836b2d..94eac8967e234 100644
--- a/config/config-arm-common.mak
+++ b/config/config-arm-common.mak
@@ -34,6 +34,7 @@ cflatobjs += lib/chr-testdev.o
 cflatobjs += lib/arm/io.o
 cflatobjs += lib/arm/setup.o
 cflatobjs += lib/arm/mmu.o
+cflatobjs += lib/arm/bitops.o
 
 libeabi = lib/arm/libeabi.a
 eabiobjs = lib/arm/eabi_compat.o
diff --git a/lib/arm/asm/bitops.h b/lib/arm/asm/bitops.h
new file mode 100644
index 0000000000000..8049634be0485
--- /dev/null
+++ b/lib/arm/asm/bitops.h
@@ -0,0 +1,53 @@
+#ifndef _ASMARM_BITOPS_H_
+#define _ASMARM_BITOPS_H_
+/*
+ * Adapated from
+ *   include/linux/bitops.h
+ *   arch/arm/lib/bitops.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.
+ */
+
+#define BITS_PER_LONG	32
+#define BIT(nr)		(1UL << (nr))
+#define BIT_MASK(nr)	(1UL << ((nr) % BITS_PER_LONG))
+#define BIT_WORD(nr)	((nr) / BITS_PER_LONG)
+
+#define ATOMIC_BITOP(insn, mask, word)				\
+({								\
+	unsigned long tmp1, tmp2;				\
+	asm volatile(						\
+	"1:	ldrex	%0, [%2]\n"				\
+		insn"	%0, %0, %3\n"				\
+	"	strex	%1, %0, [%2]\n"				\
+	"	cmp	%1, #0\n"				\
+	"	bne	1b\n"					\
+	: "=&r" (tmp1), "=&r" (tmp2)				\
+	: "r" (word), "r" (mask)				\
+	: "cc");						\
+})
+
+#define ATOMIC_TESTOP(insn, mask, word, old)			\
+({								\
+	unsigned long tmp1, tmp2;				\
+	asm volatile(						\
+	"1:	ldrex	%0, [%3]\n"				\
+	"	and	%1, %0, %4\n"				\
+		insn"	%0, %0, %4\n"				\
+	"	strex	%2, %0, [%3]\n"				\
+	"	cmp	%2, #0\n"				\
+	"	bne	1b\n"					\
+	: "=&r" (tmp1), "=&r" (old), "=&r" (tmp2)		\
+	: "r" (word), "r" (mask)				\
+	: "cc");						\
+})
+
+extern void set_bit(int nr, volatile unsigned long *addr);
+extern void clear_bit(int nr, volatile unsigned long *addr);
+extern int test_bit(int nr, const volatile unsigned long *addr);
+extern int test_and_set_bit(int nr, volatile unsigned long *addr);
+extern int test_and_clear_bit(int nr, volatile unsigned long *addr);
+
+#endif /* _ASMARM_BITOPS_H_ */
diff --git a/lib/arm/asm/cpumask.h b/lib/arm/asm/cpumask.h
new file mode 100644
index 0000000000000..85b8e4b51a403
--- /dev/null
+++ b/lib/arm/asm/cpumask.h
@@ -0,0 +1,118 @@
+#ifndef _ASMARM_CPUMASK_H_
+#define _ASMARM_CPUMASK_H_
+/*
+ * Simple cpumask implementation
+ *
+ * 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/setup.h>
+#include <asm/bitops.h>
+
+#define CPUMASK_NR_LONGS ((NR_CPUS + BITS_PER_LONG - 1) / BITS_PER_LONG)
+
+typedef struct cpumask {
+	unsigned long bits[CPUMASK_NR_LONGS];
+} cpumask_t;
+
+#define cpumask_bits(maskp) ((maskp)->bits)
+
+static inline void cpumask_set_cpu(int cpu, cpumask_t *mask)
+{
+	set_bit(cpu, cpumask_bits(mask));
+}
+
+static inline void cpumask_clear_cpu(int cpu, cpumask_t *mask)
+{
+	clear_bit(cpu, cpumask_bits(mask));
+}
+
+static inline int cpumask_test_cpu(int cpu, const cpumask_t *mask)
+{
+	return test_bit(cpu, cpumask_bits(mask));
+}
+
+static inline int cpumask_test_and_set_cpu(int cpu, cpumask_t *mask)
+{
+	return test_and_set_bit(cpu, cpumask_bits(mask));
+}
+
+static inline int cpumask_test_and_clear_cpu(int cpu, cpumask_t *mask)
+{
+	return test_and_clear_bit(cpu, cpumask_bits(mask));
+}
+
+static inline void cpumask_setall(cpumask_t *mask)
+{
+	int i;
+	for (i = 0; i < nr_cpus; i += BITS_PER_LONG)
+		cpumask_bits(mask)[BIT_WORD(i)] = ~0UL;
+	i -= BITS_PER_LONG;
+	if ((nr_cpus - i) < BITS_PER_LONG)
+		cpumask_bits(mask)[BIT_WORD(i)] = BIT_MASK(nr_cpus - i) - 1;
+}
+
+static inline void cpumask_clear(cpumask_t *mask)
+{
+	int i;
+	for (i = 0; i < nr_cpus; i += BITS_PER_LONG)
+		cpumask_bits(mask)[BIT_WORD(i)] = 0UL;
+}
+
+static inline bool cpumask_empty(const cpumask_t *mask)
+{
+	int i;
+	for (i = 0; i < nr_cpus; i += BITS_PER_LONG) {
+		if (i < NR_CPUS) { /* silence crazy compiler warning */
+			if (cpumask_bits(mask)[BIT_WORD(i)] != 0UL)
+				return false;
+		}
+	}
+	return true;
+}
+
+static inline bool cpumask_full(const cpumask_t *mask)
+{
+	int i;
+	for (i = 0; i < nr_cpus; i += BITS_PER_LONG) {
+		if (cpumask_bits(mask)[BIT_WORD(i)] != ~0UL) {
+			if ((nr_cpus - i) >= BITS_PER_LONG)
+				return false;
+			if (cpumask_bits(mask)[BIT_WORD(i)]
+					!= BIT_MASK(nr_cpus - i) - 1)
+				return false;
+		}
+	}
+	return true;
+}
+
+static inline int cpumask_weight(const cpumask_t *mask)
+{
+	int w = 0, i;
+
+	for (i = 0; i < nr_cpus; ++i)
+		if (cpumask_test_cpu(i, mask))
+			++w;
+	return w;
+}
+
+static inline void cpumask_copy(cpumask_t *dst, const cpumask_t *src)
+{
+	memcpy(cpumask_bits(dst), cpumask_bits(src),
+			CPUMASK_NR_LONGS * sizeof(long));
+}
+
+static inline int cpumask_next(int cpu, const cpumask_t *mask)
+{
+	while (cpu < nr_cpus && !cpumask_test_cpu(++cpu, mask))
+		;
+	return cpu;
+}
+
+#define for_each_cpu(cpu, mask)					\
+	for ((cpu) = cpumask_next(-1, mask);			\
+			(cpu) < nr_cpus; 			\
+			(cpu) = cpumask_next(cpu, mask))
+
+#endif /* _ASMARM_CPUMASK_H_ */
diff --git a/lib/arm/bitops.c b/lib/arm/bitops.c
new file mode 100644
index 0000000000000..9ad112162a29f
--- /dev/null
+++ b/lib/arm/bitops.c
@@ -0,0 +1,81 @@
+/*
+ * Adapated from
+ *   include/asm-generic/bitops/atomic.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/bitops.h>
+#include <asm/barrier.h>
+#include <asm/mmu.h>
+
+void set_bit(int nr, volatile unsigned long *addr)
+{
+	volatile unsigned long *word = addr + BIT_WORD(nr);
+	unsigned long mask = BIT_MASK(nr);
+
+	if (mmu_enabled())
+		ATOMIC_BITOP("orr", mask, word);
+	else
+		*word |= mask;
+	smp_mb();
+}
+
+void clear_bit(int nr, volatile unsigned long *addr)
+{
+	volatile unsigned long *word = addr + BIT_WORD(nr);
+	unsigned long mask = BIT_MASK(nr);
+
+	if (mmu_enabled())
+		ATOMIC_BITOP("bic", mask, word);
+	else
+		*word &= ~mask;
+	smp_mb();
+}
+
+int test_bit(int nr, const volatile unsigned long *addr)
+{
+	const volatile unsigned long *word = addr + BIT_WORD(nr);
+	unsigned long mask = BIT_MASK(nr);
+
+	return (*word & mask) != 0;
+}
+
+int test_and_set_bit(int nr, volatile unsigned long *addr)
+{
+	volatile unsigned long *word = addr + BIT_WORD(nr);
+	unsigned long mask = BIT_MASK(nr);
+	unsigned long old;
+
+	smp_mb();
+
+	if (mmu_enabled()) {
+		ATOMIC_TESTOP("orr", mask, word, old);
+	} else {
+		old = *word;
+		*word = old | mask;
+	}
+	smp_mb();
+
+	return (old & mask) != 0;
+}
+
+int test_and_clear_bit(int nr, volatile unsigned long *addr)
+{
+	volatile unsigned long *word = addr + BIT_WORD(nr);
+	unsigned long mask = BIT_MASK(nr);
+	unsigned long old;
+
+	smp_mb();
+
+	if (mmu_enabled()) {
+		ATOMIC_TESTOP("bic", mask, word, old);
+	} else {
+		old = *word;
+		*word = old & ~mask;
+	}
+	smp_mb();
+
+	return (old & mask) != 0;
+}
diff --git a/lib/arm64/asm/bitops.h b/lib/arm64/asm/bitops.h
new file mode 100644
index 0000000000000..3371c60bdc4f2
--- /dev/null
+++ b/lib/arm64/asm/bitops.h
@@ -0,0 +1,51 @@
+#ifndef _ASMARM64_BITOPS_H_
+#define _ASMARM64_BITOPS_H_
+/*
+ * Adapated from
+ *   include/linux/bitops.h
+ *   arch/arm64/lib/bitops.S
+ *
+ * Copyright (C) 2015, Red Hat Inc, Andrew Jones <drjones@redhat.com>
+ *
+ * This work is licensed under the terms of the GNU LGPL, version 2.
+ */
+
+#define BITS_PER_LONG	64
+#define BIT(nr)		(1UL << (nr))
+#define BIT_MASK(nr)	(1UL << ((nr) % BITS_PER_LONG))
+#define BIT_WORD(nr)	((nr) / BITS_PER_LONG)
+
+#define ATOMIC_BITOP(insn, mask, word)				\
+({								\
+	unsigned long tmp1, tmp2;				\
+	asm volatile(						\
+	"1:	ldxr	%0, [%2]\n"				\
+		insn"	%0, %0, %3\n"				\
+	"	stxr	%w1, %0, [%2]\n"			\
+	"	cbnz	%w1, 1b\n"				\
+	: "=&r" (tmp1), "=&r" (tmp2)				\
+	: "r" (word), "r" (mask)				\
+	: "cc");						\
+})
+
+#define ATOMIC_TESTOP(insn, mask, word, old)			\
+({								\
+	unsigned long tmp1, tmp2;				\
+	asm volatile(						\
+	"1:	ldxr	%0, [%3]\n"				\
+	"	and	%1, %0, %4\n"				\
+		insn"	%0, %0, %4\n"				\
+	"	stlxr	%w2, %0, [%3]\n"			\
+	"	cbnz	%w2, 1b\n"				\
+	: "=&r" (tmp1), "=&r" (old), "=&r" (tmp2)		\
+	: "r" (word), "r" (mask)				\
+	: "cc");						\
+})
+
+extern void set_bit(int nr, volatile unsigned long *addr);
+extern void clear_bit(int nr, volatile unsigned long *addr);
+extern int test_bit(int nr, const volatile unsigned long *addr);
+extern int test_and_set_bit(int nr, volatile unsigned long *addr);
+extern int test_and_clear_bit(int nr, volatile unsigned long *addr);
+
+#endif /* _ASMARM64_BITOPS_H_ */
diff --git a/lib/arm64/asm/cpumask.h b/lib/arm64/asm/cpumask.h
new file mode 100644
index 0000000000000..d1421e7abe310
--- /dev/null
+++ b/lib/arm64/asm/cpumask.h
@@ -0,0 +1 @@
+#include "../../arm/asm/cpumask.h"
-- 
1.9.3

[PATCH 11/18] arm/arm64: make mmu_on per cpu

[Andrew Jones]

We introduced a variable called mmu_on to the mmu implementation
because unit tests may want to run with the mmu off, yet still
call into common code that could lead to {Load,Store}-Exclusive
instructions - which is illegal. So, the mmu_on variable was added
and made query-able (through mmu_enabled()) in order to guard those
paths. But, mmu_on is really a per cpu concept, so for smp we need
to change it. As it's just a bool, we can easily make it per cpu by
changing it into a cpumask. We rename it more appropriately too.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 lib/arm/asm/mmu-api.h |  1 +
 lib/arm/mmu.c         | 15 ++++++++++++---
 2 files changed, 13 insertions(+), 3 deletions(-)

diff --git a/lib/arm/asm/mmu-api.h b/lib/arm/asm/mmu-api.h
index f2511e3dc7dee..68dc707d67241 100644
--- a/lib/arm/asm/mmu-api.h
+++ b/lib/arm/asm/mmu-api.h
@@ -2,6 +2,7 @@
 #define __ASMARM_MMU_API_H_
 extern pgd_t *mmu_idmap;
 extern bool mmu_enabled(void);
+extern void mmu_set_enabled(void);
 extern void mmu_enable(pgd_t *pgtable);
 extern void mmu_enable_idmap(void);
 extern void mmu_init_io_sect(pgd_t *pgtable, unsigned long virt_offset);
diff --git a/lib/arm/mmu.c b/lib/arm/mmu.c
index 1c024538663ce..732000a8eb088 100644
--- a/lib/arm/mmu.c
+++ b/lib/arm/mmu.c
@@ -6,16 +6,25 @@
  * This work is licensed under the terms of the GNU LGPL, version 2.
  */
 #include <asm/setup.h>
+#include <asm/thread_info.h>
+#include <asm/cpumask.h>
 #include <asm/mmu.h>
 
 extern unsigned long etext;
 
 pgd_t *mmu_idmap;
 
-static bool mmu_on;
+static cpumask_t mmu_enabled_cpumask;
 bool mmu_enabled(void)
 {
-	return mmu_on;
+	struct thread_info *ti = current_thread_info();
+	return cpumask_test_cpu(ti->cpu, &mmu_enabled_cpumask);
+}
+
+void mmu_set_enabled(void)
+{
+	struct thread_info *ti = current_thread_info();
+	cpumask_set_cpu(ti->cpu, &mmu_enabled_cpumask);
 }
 
 extern void asm_mmu_enable(phys_addr_t pgtable);
@@ -23,7 +32,7 @@ void mmu_enable(pgd_t *pgtable)
 {
 	asm_mmu_enable(__pa(pgtable));
 	flush_tlb_all();
-	mmu_on = true;
+	mmu_set_enabled();
 }
 
 void mmu_set_range_ptes(pgd_t *pgtable, unsigned long virt_offset,
-- 
1.9.3

[PATCH 12/18] arm64: implement spinlocks

[Andrew Jones]

We put this off, as it wasn't necessary without smp. Now it
is. Only need to do this for arm64, as we've already done it
already for arm.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 config/config-arm64.mak  |  1 +
 lib/arm64/asm/spinlock.h |  8 ++------
 lib/arm64/spinlock.c     | 43 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 46 insertions(+), 6 deletions(-)
 create mode 100644 lib/arm64/spinlock.c

diff --git a/config/config-arm64.mak b/config/config-arm64.mak
index 5f8550eb511e8..d61b703c8140e 100644
--- a/config/config-arm64.mak
+++ b/config/config-arm64.mak
@@ -9,6 +9,7 @@ kernel_offset = 0x80000
 
 cstart.o = $(TEST_DIR)/cstart64.o
 cflatobjs += lib/arm64/processor.o
+cflatobjs += lib/arm64/spinlock.o
 
 # arm64 specific tests
 tests =
diff --git a/lib/arm64/asm/spinlock.h b/lib/arm64/asm/spinlock.h
index 36b7b44fa4edf..43b2634b46459 100644
--- a/lib/arm64/asm/spinlock.h
+++ b/lib/arm64/asm/spinlock.h
@@ -5,11 +5,7 @@ struct spinlock {
 	int v;
 };
 
-static inline void spin_lock(struct spinlock *lock __unused)
-{
-}
-static inline void spin_unlock(struct spinlock *lock __unused)
-{
-}
+extern void spin_lock(struct spinlock *lock);
+extern void spin_unlock(struct spinlock *lock);
 
 #endif /* _ASMARM64_SPINLOCK_H_ */
diff --git a/lib/arm64/spinlock.c b/lib/arm64/spinlock.c
new file mode 100644
index 0000000000000..68b68b75ba60d
--- /dev/null
+++ b/lib/arm64/spinlock.c
@@ -0,0 +1,43 @@
+/*
+ * spinlocks
+ *
+ * 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/spinlock.h>
+#include <asm/barrier.h>
+#include <asm/mmu.h>
+
+void spin_lock(struct spinlock *lock)
+{
+	u32 val, fail;
+
+	smp_mb();
+
+	if (!mmu_enabled()) {
+		lock->v = 1;
+		return;
+	}
+
+	do {
+		asm volatile(
+		"1:	ldaxr	%w0, [%2]\n"
+		"	cbnz	%w0, 1b\n"
+		"	mov	%0, #1\n"
+		"	stxr	%w1, %w0, [%2]\n"
+		: "=&r" (val), "=&r" (fail)
+		: "r" (&lock->v)
+		: "cc" );
+	} while (fail);
+	smp_mb();
+}
+
+void spin_unlock(struct spinlock *lock)
+{
+	if (mmu_enabled())
+		asm volatile("stlrh wzr, [%0]" :: "r" (&lock->v));
+	else
+		lock->v = 0;
+	smp_mb();
+}
-- 
1.9.3

[PATCH 13/18] arm/arm64: import include/uapi/linux/psci.h

[Andrew Jones]

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 lib/arm/asm/uapi-psci.h   | 73 +++++++++++++++++++++++++++++++++++++++++++++++
 lib/arm64/asm/uapi-psci.h |  1 +
 2 files changed, 74 insertions(+)
 create mode 100644 lib/arm/asm/uapi-psci.h
 create mode 100644 lib/arm64/asm/uapi-psci.h

diff --git a/lib/arm/asm/uapi-psci.h b/lib/arm/asm/uapi-psci.h
new file mode 100644
index 0000000000000..5c6fada2b5105
--- /dev/null
+++ b/lib/arm/asm/uapi-psci.h
@@ -0,0 +1,73 @@
+#ifndef _ASMARM_UAPI_PSCI_H_
+#define _ASMARM_UAPI_PSCI_H_
+/*
+ * From include/uapi/linux/psci.h
+ */
+
+/* PSCI v0.2 interface */
+#define PSCI_0_2_FN_BASE			0x84000000
+#define PSCI_0_2_FN(n)				(PSCI_0_2_FN_BASE + (n))
+#define PSCI_0_2_64BIT				0x40000000
+#define PSCI_0_2_FN64_BASE			\
+					(PSCI_0_2_FN_BASE + PSCI_0_2_64BIT)
+#define PSCI_0_2_FN64(n)			(PSCI_0_2_FN64_BASE + (n))
+
+#define PSCI_0_2_FN_PSCI_VERSION		PSCI_0_2_FN(0)
+#define PSCI_0_2_FN_CPU_SUSPEND			PSCI_0_2_FN(1)
+#define PSCI_0_2_FN_CPU_OFF			PSCI_0_2_FN(2)
+#define PSCI_0_2_FN_CPU_ON			PSCI_0_2_FN(3)
+#define PSCI_0_2_FN_AFFINITY_INFO		PSCI_0_2_FN(4)
+#define PSCI_0_2_FN_MIGRATE			PSCI_0_2_FN(5)
+#define PSCI_0_2_FN_MIGRATE_INFO_TYPE		PSCI_0_2_FN(6)
+#define PSCI_0_2_FN_MIGRATE_INFO_UP_CPU		PSCI_0_2_FN(7)
+#define PSCI_0_2_FN_SYSTEM_OFF			PSCI_0_2_FN(8)
+#define PSCI_0_2_FN_SYSTEM_RESET		PSCI_0_2_FN(9)
+
+#define PSCI_0_2_FN64_CPU_SUSPEND		PSCI_0_2_FN64(1)
+#define PSCI_0_2_FN64_CPU_ON			PSCI_0_2_FN64(3)
+#define PSCI_0_2_FN64_AFFINITY_INFO		PSCI_0_2_FN64(4)
+#define PSCI_0_2_FN64_MIGRATE			PSCI_0_2_FN64(5)
+#define PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU	PSCI_0_2_FN64(7)
+
+/* PSCI v0.2 power state encoding for CPU_SUSPEND function */
+#define PSCI_0_2_POWER_STATE_ID_MASK		0xffff
+#define PSCI_0_2_POWER_STATE_ID_SHIFT		0
+#define PSCI_0_2_POWER_STATE_TYPE_SHIFT		16
+#define PSCI_0_2_POWER_STATE_TYPE_MASK		\
+				(0x1 << PSCI_0_2_POWER_STATE_TYPE_SHIFT)
+#define PSCI_0_2_POWER_STATE_AFFL_SHIFT		24
+#define PSCI_0_2_POWER_STATE_AFFL_MASK		\
+				(0x3 << PSCI_0_2_POWER_STATE_AFFL_SHIFT)
+
+/* PSCI v0.2 affinity level state returned by AFFINITY_INFO */
+#define PSCI_0_2_AFFINITY_LEVEL_ON		0
+#define PSCI_0_2_AFFINITY_LEVEL_OFF		1
+#define PSCI_0_2_AFFINITY_LEVEL_ON_PENDING	2
+
+/* PSCI v0.2 multicore support in Trusted OS returned by MIGRATE_INFO_TYPE */
+#define PSCI_0_2_TOS_UP_MIGRATE			0
+#define PSCI_0_2_TOS_UP_NO_MIGRATE		1
+#define PSCI_0_2_TOS_MP				2
+
+/* PSCI version decoding (independent of PSCI version) */
+#define PSCI_VERSION_MAJOR_SHIFT		16
+#define PSCI_VERSION_MINOR_MASK			\
+		((1U << PSCI_VERSION_MAJOR_SHIFT) - 1)
+#define PSCI_VERSION_MAJOR_MASK			~PSCI_VERSION_MINOR_MASK
+#define PSCI_VERSION_MAJOR(ver)			\
+		(((ver) & PSCI_VERSION_MAJOR_MASK) >> PSCI_VERSION_MAJOR_SHIFT)
+#define PSCI_VERSION_MINOR(ver)			\
+		((ver) & PSCI_VERSION_MINOR_MASK)
+
+/* PSCI return values (inclusive of all PSCI versions) */
+#define PSCI_RET_SUCCESS			0
+#define PSCI_RET_NOT_SUPPORTED			-1
+#define PSCI_RET_INVALID_PARAMS			-2
+#define PSCI_RET_DENIED				-3
+#define PSCI_RET_ALREADY_ON			-4
+#define PSCI_RET_ON_PENDING			-5
+#define PSCI_RET_INTERNAL_FAILURE		-6
+#define PSCI_RET_NOT_PRESENT			-7
+#define PSCI_RET_DISABLED			-8
+
+#endif /* _ASMARM_UAPI_PSCI_H_ */
diff --git a/lib/arm64/asm/uapi-psci.h b/lib/arm64/asm/uapi-psci.h
new file mode 100644
index 0000000000000..83d018f954e4c
--- /dev/null
+++ b/lib/arm64/asm/uapi-psci.h
@@ -0,0 +1 @@
+#include "../../arm/asm/uapi-psci.h"
-- 
1.9.3

[PATCH 14/18] arm/arm64: add some PSCI API

[Andrew Jones]

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 config/config-arm-common.mak |  1 +
 lib/arm/asm/psci.h           | 13 +++++++++++++
 lib/arm/psci.c               | 30 ++++++++++++++++++++++++++++++
 lib/arm64/asm/psci.h         | 13 +++++++++++++
 4 files changed, 57 insertions(+)
 create mode 100644 lib/arm/asm/psci.h
 create mode 100644 lib/arm/psci.c
 create mode 100644 lib/arm64/asm/psci.h

diff --git a/config/config-arm-common.mak b/config/config-arm-common.mak
index 94eac8967e234..13f5338a35a02 100644
--- a/config/config-arm-common.mak
+++ b/config/config-arm-common.mak
@@ -35,6 +35,7 @@ cflatobjs += lib/arm/io.o
 cflatobjs += lib/arm/setup.o
 cflatobjs += lib/arm/mmu.o
 cflatobjs += lib/arm/bitops.o
+cflatobjs += lib/arm/psci.o
 
 libeabi = lib/arm/libeabi.a
 eabiobjs = lib/arm/eabi_compat.o
diff --git a/lib/arm/asm/psci.h b/lib/arm/asm/psci.h
new file mode 100644
index 0000000000000..e2e66b47de480
--- /dev/null
+++ b/lib/arm/asm/psci.h
@@ -0,0 +1,13 @@
+#ifndef _ASMARM_PSCI_H_
+#define _ASMARM_PSCI_H_
+#include <libcflat.h>
+#include <asm/uapi-psci.h>
+
+#define PSCI_INVOKE_ARG_TYPE	u32
+#define PSCI_FN_CPU_ON		PSCI_0_2_FN_CPU_ON
+
+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);
+
+#endif /* _ASMARM_PSCI_H_ */
diff --git a/lib/arm/psci.c b/lib/arm/psci.c
new file mode 100644
index 0000000000000..027c4f66f1815
--- /dev/null
+++ b/lib/arm/psci.c
@@ -0,0 +1,30 @@
+/*
+ * PSCI API
+ * From arch/arm[64]/kernel/psci.c
+ *
+ * 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/psci.h>
+
+#define T PSCI_INVOKE_ARG_TYPE
+__attribute__((noinline))
+int psci_invoke(T function_id, T arg0, T arg1, T arg2)
+{
+	asm volatile(
+		"hvc #0"
+	: "+r" (function_id)
+	: "r" (arg0), "r" (arg1), "r" (arg2));
+	return function_id;
+}
+
+int psci_cpu_on(unsigned long cpuid, unsigned long entry_point)
+{
+	return psci_invoke(PSCI_FN_CPU_ON, cpuid, entry_point, 0);
+}
+
+void psci_sys_reset(void)
+{
+	psci_invoke(PSCI_0_2_FN_SYSTEM_RESET, 0, 0, 0);
+}
diff --git a/lib/arm64/asm/psci.h b/lib/arm64/asm/psci.h
new file mode 100644
index 0000000000000..c481be4bd6bab
--- /dev/null
+++ b/lib/arm64/asm/psci.h
@@ -0,0 +1,13 @@
+#ifndef _ASMARM64_PSCI_H_
+#define _ASMARM64_PSCI_H_
+#include <libcflat.h>
+#include <asm/uapi-psci.h>
+
+#define PSCI_INVOKE_ARG_TYPE	u64
+#define PSCI_FN_CPU_ON		PSCI_0_2_FN64_CPU_ON
+
+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);
+
+#endif /* _ASMARM64_PSCI_H_ */
-- 
1.9.3

[PATCH 15/18] arm/arm64: add cpu_relax() and friends

[Andrew Jones]

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 lib/arm/asm/barrier.h   | 5 +++++
 lib/arm64/asm/barrier.h | 5 +++++
 2 files changed, 10 insertions(+)

diff --git a/lib/arm/asm/barrier.h b/lib/arm/asm/barrier.h
index acaeab5123431..394a4a2da26f1 100644
--- a/lib/arm/asm/barrier.h
+++ b/lib/arm/asm/barrier.h
@@ -4,6 +4,11 @@
  * Adapted form arch/arm/include/asm/barrier.h
  */
 
+#define sev()		asm volatile("sev" : : : "memory")
+#define wfe()		asm volatile("wfe" : : : "memory")
+#define wfi()		asm volatile("wfi" : : : "memory")
+#define cpu_relax()	asm volatile(""    : : : "memory")
+
 #define isb(option) __asm__ __volatile__ ("isb " #option : : : "memory")
 #define dsb(option) __asm__ __volatile__ ("dsb " #option : : : "memory")
 #define dmb(option) __asm__ __volatile__ ("dmb " #option : : : "memory")
diff --git a/lib/arm64/asm/barrier.h b/lib/arm64/asm/barrier.h
index 8ebdfdf7f1037..dbdac9d339c7e 100644
--- a/lib/arm64/asm/barrier.h
+++ b/lib/arm64/asm/barrier.h
@@ -4,6 +4,11 @@
  * From Linux arch/arm64/include/asm/barrier.h
  */
 
+#define sev()		asm volatile("sev" : : : "memory")
+#define wfe()		asm volatile("wfe" : : : "memory")
+#define wfi()		asm volatile("wfi" : : : "memory")
+#define cpu_relax()	asm volatile(""    : : : "memory")
+
 #define isb()		asm volatile("isb" : : : "memory")
 #define dmb(opt)	asm volatile("dmb " #opt : : : "memory")
 #define dsb(opt)	asm volatile("dsb " #opt : : : "memory")
-- 
1.9.3

[PATCH 16/18] arm: clarify comment about exception stack use

[Andrew Jones]

Each mode has its own stack, but we only use it as a base pointer
to a private memory region. That region, which has a size of
sizeof(struct pt_regs), is used to store some registers during the
switch to svc mode - which will use the normal svc stack. The switch
to svc mode is done because we handle all exceptions in svc.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 arm/cstart.S | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/arm/cstart.S b/arm/cstart.S
index fd02aaab268d7..08a0b3ecc61f6 100644
--- a/arm/cstart.S
+++ b/arm/cstart.S
@@ -121,8 +121,9 @@ asm_mmu_enable:
  * Simplified version of the Linux kernel implementation
  *   arch/arm/kernel/entry-armv.S
  *
- * Each mode has an S_FRAME_SIZE sized stack initialized
- * in exceptions_init
+ * Each mode has an S_FRAME_SIZE sized memory region,
+ * and the mode's stack pointer has been initialized
+ * to the base of that region in exceptions_init.
  */
 .macro vector_stub, name, vec, mode, correction=0
 .align 5
-- 
1.9.3

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

[Andrew Jones]

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>
---
 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@redhat.com>
+ *
+ * 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@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;
+#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

[PATCH 18/18] arm/arm64: Add smp selftest

[Andrew Jones]

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 arm/selftest.c    | 60 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 arm/unittests.cfg | 11 ++++++++--
 2 files changed, 69 insertions(+), 2 deletions(-)

diff --git a/arm/selftest.c b/arm/selftest.c
index d77495747b08a..fc9ec609d875e 100644
--- a/arm/selftest.c
+++ b/arm/selftest.c
@@ -7,11 +7,16 @@
  */
 #include <libcflat.h>
 #include <alloc.h>
+#include <devicetree.h>
 #include <asm/setup.h>
 #include <asm/ptrace.h>
 #include <asm/asm-offsets.h>
 #include <asm/processor.h>
 #include <asm/thread_info.h>
+#include <asm/psci.h>
+#include <asm/smp.h>
+#include <asm/cpumask.h>
+#include <asm/barrier.h>
 
 static void assert_args(int num_args, int needed_args)
 {
@@ -297,6 +302,45 @@ static void check_vectors(void *arg __unused)
 	exit(report_summary());
 }
 
+static bool psci_check(void)
+{
+	const struct fdt_property *method;
+	int node, len, ver;
+
+	node = fdt_node_offset_by_compatible(dt_fdt(), -1, "arm,psci-0.2");
+	if (node < 0) {
+		printf("PSCI v0.2 compatibility required\n");
+		return false;
+	}
+
+	method = fdt_get_property(dt_fdt(), node, "method", &len);
+	if (method == NULL) {
+		printf("bad psci device tree node\n");
+		return false;
+	}
+
+	if (len < 4 || strcmp(method->data, "hvc") != 0) {
+		printf("psci method must be hvc\n");
+		return false;
+	}
+
+	ver = psci_invoke(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
+	printf("PSCI version %d.%d\n", PSCI_VERSION_MAJOR(ver),
+				       PSCI_VERSION_MINOR(ver));
+
+	return true;
+}
+
+static cpumask_t smp_reported;
+static void cpu_report(void)
+{
+	int cpu = smp_processor_id();
+
+	report("CPU%d online", true, cpu);
+	cpumask_set_cpu(cpu, &smp_reported);
+	halt();
+}
+
 int main(int argc, char **argv)
 {
 	report_prefix_push("selftest");
@@ -316,6 +360,22 @@ int main(int argc, char **argv)
 		void *sp = memalign(THREAD_SIZE, THREAD_SIZE);
 		start_usr(check_vectors, NULL,
 				(unsigned long)sp + THREAD_START_SP);
+
+	} else if (strcmp(argv[0], "smp") == 0) {
+
+		int cpu;
+
+		report("PSCI version", psci_check());
+
+		for_each_present_cpu(cpu) {
+			if (cpu == 0)
+				continue;
+			smp_boot_secondary(cpu, cpu_report);
+		}
+
+		cpumask_set_cpu(0, &smp_reported);
+		while (!cpumask_full(&smp_reported))
+			cpu_relax();
 	}
 
 	return report_summary();
diff --git a/arm/unittests.cfg b/arm/unittests.cfg
index efcca6bf24af6..ee655b2678a4e 100644
--- a/arm/unittests.cfg
+++ b/arm/unittests.cfg
@@ -13,8 +13,8 @@
 #
 [selftest::setup]
 file = selftest.flat
-smp  = 1
-extra_params = -m 256 -append 'setup smp=1 mem=256'
+smp = 2
+extra_params = -m 256 -append 'setup smp=2 mem=256'
 groups = selftest
 
 # Test vector setup and exception handling (kernel mode).
@@ -28,3 +28,10 @@ groups = selftest
 file = selftest.flat
 extra_params = -append 'vectors-user'
 groups = selftest
+
+# Test SMP support
+[selftest::smp]
+file = selftest.flat
+smp = $(getconf _NPROCESSORS_CONF)
+extra_params = -append 'smp'
+groups = selftest
-- 
1.9.3

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

[Andrew Jones]

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.

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

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

[Andrew Jones]

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
> 

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

[Andrew Jones]

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

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

[PATCH v4 17/18] arm/arm64: add smp_boot_secondary

[Andrew Jones]

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>
---
v4:
    - Fix yet another bug... v2 fixed secondary processors using cpu0's
      exception stacks, but broke cpu0 in the process. Until now,
      my plan was to just leave cpu0 on 'exception_stacks', set up by
      the linker, but v2's change means we can't do that. So set the
      initial stack up correctly instead.
    - Also remove the smp.h includes from arm[64]/asm-offsets.c, they
      should have been removed with the 2nd change made for v3 of this
      patch.
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                 | 59 ++++++++++++++++++++++++++++++++++++++------
 arm/cstart64.S               | 25 +++++++++++++++++++
 arm/flat.lds                 |  3 ---
 config/config-arm-common.mak |  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/psci.h         |  2 ++
 lib/arm64/asm/smp.h          |  1 +
 12 files changed, 232 insertions(+), 16 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..2b7f8f3d200ef 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
@@ -17,6 +20,22 @@
 .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
 	 *   r0 = 0
@@ -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]
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/arm/flat.lds b/arm/flat.lds
index c01f01e7b8682..efdf5d7109ffb 100644
--- a/arm/flat.lds
+++ b/arm/flat.lds
@@ -5,9 +5,6 @@ SECTIONS
     . = ALIGN(64K);
     etext = .;
     .data : {
-        exception_stacks = .;
-        . += 64K;
-        exception_stacks_end = .;
         *(.data)
     }
     . = ALIGN(16);
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/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/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

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

[Andrew Jones]

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
> 

Re: [kvm-unit-tests PATCH 00/18] arm/arm64: add smp support

[Christoffer Dall]

On Sun, Feb 01, 2015 at 07:34:28PM +0100, Andrew Jones wrote:
> This series extends the kvm-unit-tests/arm[64] framework to support smp.
> A break down of the patches is as follows
> 
> 01-02: prepare general framework for smp use
> 03-06: arm/arm64 fixups not 100% related to this series,
>        but need to post some time...
> 07-09: add thread_info (for per-thread data) and suck some global
>        data into it
> 10-11: add cpumask support (for per-cpu data) and suck some more
>        global data in
>    12: add arm64 simple spinlock implementation
> 13-14: add some PSCI support
> 15-16: further prep for smp_boot_secondary
>    17: finally add smp_boot_secondary
>    18: as usual, add a selftest to make sure it all works
> 
> These patches are also available here:
> https://github.com/rhdrjones/kvm-unit-tests/tree/arm/smp
> 
I've tested these patches on Juno and they seem to run fine, however,
since we don't support big.LITTLE yet, you have to run them under
'taskset <mask>', but the config script uses $(getconf
_NPROCESSORS_CONF), which returns 6, and QEMU fails.  The interesting
bit is that the unit-tests still report 'PASS' - not sure why.

I'm fine with merging these and fixing anything up as we go along - this
is a test harness after all and I don't have time for an in-depth review
of these patches right now, sorry.

-Christoffer

Re: [PATCH 03/18] arm: fixups: add barriers, actually set MAIR

[Christoffer Dall]

On Sun, Feb 01, 2015 at 07:34:31PM +0100, Andrew Jones wrote:
> Sprinkle in some more isbs after context-changing operations,
> as the ARM ARM states we should. I haven't seen any problems
> without them, but we should do it right. Also, *actually* set
> the MAIR in asm_mmu_enable. We were reading, not writing...
> Luckily this was just spotted while adding the isbs, rather
> than leading to a nightmare debug session some day...
> 
> Signed-off-by: Andrew Jones <drjones@redhat.com>
> ---
>  arm/cstart.S        | 8 ++++++--
>  lib/arm/processor.c | 1 +
>  2 files changed, 7 insertions(+), 2 deletions(-)
> 
> diff --git a/arm/cstart.S b/arm/cstart.S
> index da496e9eae7e0..39fac8f1e1bd8 100644
> --- a/arm/cstart.S
> +++ b/arm/cstart.S
> @@ -50,6 +50,7 @@ start:
>  .macro set_mode_stack mode, stack
>  	add	\stack, #S_FRAME_SIZE
>  	msr	cpsr_c, #(\mode | PSR_I_BIT | PSR_F_BIT)
> +	isb
>  	mov	sp, \stack
>  .endm
>  
> @@ -70,6 +71,7 @@ exceptions_init:
>  	set_mode_stack	FIQ_MODE, r1
>  
>  	msr	cpsr_cxsf, r2		@ back to svc mode
> +	isb
>  	mov	pc, lr
>  
I don't think you really need those after the cpsr, but whatever.

-Christoffer

Re: [kvm-unit-tests PATCH 00/18] arm/arm64: add smp support

[Andrew Jones]

On Thu, Feb 26, 2015 at 12:34:02PM +0100, Christoffer Dall wrote:
> On Sun, Feb 01, 2015 at 07:34:28PM +0100, Andrew Jones wrote:
> > This series extends the kvm-unit-tests/arm[64] framework to support smp.
> > A break down of the patches is as follows
> > 
> > 01-02: prepare general framework for smp use
> > 03-06: arm/arm64 fixups not 100% related to this series,
> >        but need to post some time...
> > 07-09: add thread_info (for per-thread data) and suck some global
> >        data into it
> > 10-11: add cpumask support (for per-cpu data) and suck some more
> >        global data in
> >    12: add arm64 simple spinlock implementation
> > 13-14: add some PSCI support
> > 15-16: further prep for smp_boot_secondary
> >    17: finally add smp_boot_secondary
> >    18: as usual, add a selftest to make sure it all works
> > 
> > These patches are also available here:
> > https://github.com/rhdrjones/kvm-unit-tests/tree/arm/smp
> > 
> I've tested these patches on Juno and they seem to run fine, however,
> since we don't support big.LITTLE yet, you have to run them under
> 'taskset <mask>', but the config script uses $(getconf
> _NPROCESSORS_CONF), which returns 6, and QEMU fails.  The interesting

Should I try to read the number of host cpus from some other source?
If you know something I can read that also works on big.LITTLE, then
I can change it now.

> bit is that the unit-tests still report 'PASS' - not sure why.

Ah, this is due to the weird way qemu's debugexit device sets its exit
code

hw/misc/debugexit.c:debug_exit_write()
{
  exit((val << 1) | 1);
}

To be consistent with that we made chr-testdev do the same thing (see
backends/testdev.c:testdev_eat_packet():case 'q'). Now, the
kvm-unit-tests run_tests.sh script knows about that, so it has

  eval $cmdline >> test.log
  if [ $? -le 1 ]; then
     echo -e "\e[32mPASS\e[0m $1"
  else
     echo -e "\e[31mFAIL\e[0m $1"
  fi

Yes, this sucks, as we can't tell the difference between qemu failing
to run the test, and exiting with 1 vs. the test running, passing -
exiting with (0 << 1) | 1. It's too bad debugexit didn't set a higher
bit (like 5 or 6) to flag a "debug exit". Maybe it's not too late to
change it? Paolo?

> 
> I'm fine with merging these and fixing anything up as we go along - this
> is a test harness after all and I don't have time for an in-depth review
> of these patches right now, sorry.

Thanks!
drew

Re: [kvm-unit-tests PATCH 00/18] arm/arm64: add smp support

[Paolo Bonzini]

On 26/02/2015 14:50, Andrew Jones wrote:
> Yes, this sucks, as we can't tell the difference between qemu failing
> to run the test, and exiting with 1 vs. the test running, passing -
> exiting with (0 << 1) | 1. It's too bad debugexit didn't set a higher
> bit (like 5 or 6) to flag a "debug exit". Maybe it's not too late to
> change it? Paolo?

That would indeed have been a better idea. :(  I'm not sure it's worth
it because in theory exit codes of 1 should never happen in
kvm-unit-tests.  That said, it does suck when they happen due to a bug
in the harness.

Paolo

Re: [kvm-unit-tests PATCH 00/18] arm/arm64: add smp support

[Marcelo Tosatti]

On Sun, Feb 01, 2015 at 07:34:28PM +0100, Andrew Jones wrote:
> This series extends the kvm-unit-tests/arm[64] framework to support smp.
> A break down of the patches is as follows
> 
> 01-02: prepare general framework for smp use
> 03-06: arm/arm64 fixups not 100% related to this series,
>        but need to post some time...
> 07-09: add thread_info (for per-thread data) and suck some global
>        data into it
> 10-11: add cpumask support (for per-cpu data) and suck some more
>        global data in
>    12: add arm64 simple spinlock implementation
> 13-14: add some PSCI support
> 15-16: further prep for smp_boot_secondary
>    17: finally add smp_boot_secondary
>    18: as usual, add a selftest to make sure it all works
> 
> These patches are also available here:
> https://github.com/rhdrjones/kvm-unit-tests/tree/arm/smp
> 
> Thanks in advance for reviews!
> 
> 
> Andrew Jones (18):
>   x86: expose spin_lock/unlock to lib code
>   lib/report: guard access to counters
>   arm: fixups: add barriers, actually set MAIR
>   arm64: fixup: use id_aa64mmfr0_el1 to set tcr
>   arm/arm64: processor.[ch] cleanups
>   arm/arm64: get rid of get_sp()
>   arm/arm64: introduce thread_info
>   arm/arm64: add per thread user_mode flag
>   arm/arm64: maintain per thread exception handlers
>   arm/arm64: add simple cpumask API
>   arm/arm64: make mmu_on per cpu
>   arm64: implement spinlocks
>   arm/arm64: import include/uapi/linux/psci.h
>   arm/arm64: add some PSCI API
>   arm/arm64: add cpu_relax() and friends
>   arm: clarify comment about exception stack use
>   arm/arm64: add smp_boot_secondary
>   arm/arm64: Add smp selftest
> 
>  arm/cstart.S                 |  47 +++++++++++++----
>  arm/cstart64.S               |  48 +++++++++++-------
>  arm/flat.lds                 |   6 +++
>  arm/selftest.c               |  70 +++++++++++++++++++++++--
>  arm/unittests.cfg            |  11 +++-
>  config/config-arm-common.mak |   3 ++
>  config/config-arm64.mak      |   1 +
>  lib/arm/asm-offsets.c        |   3 ++
>  lib/arm/asm/barrier.h        |   5 ++
>  lib/arm/asm/bitops.h         |  53 +++++++++++++++++++
>  lib/arm/asm/cpumask.h        | 118 +++++++++++++++++++++++++++++++++++++++++++
>  lib/arm/asm/mmu-api.h        |   1 +
>  lib/arm/asm/processor.h      |  12 ++++-
>  lib/arm/asm/psci.h           |  15 ++++++
>  lib/arm/asm/smp.h            |  56 ++++++++++++++++++++
>  lib/arm/asm/thread_info.h    |  60 ++++++++++++++++++++++
>  lib/arm/asm/uapi-psci.h      |  73 ++++++++++++++++++++++++++
>  lib/arm/bitops.c             |  81 +++++++++++++++++++++++++++++
>  lib/arm/mmu.c                |  15 ++++--
>  lib/arm/processor.c          |  44 +++++++++++-----
>  lib/arm/psci.c               |  49 ++++++++++++++++++
>  lib/arm/setup.c              |  11 +++-
>  lib/arm/smp.c                |  57 +++++++++++++++++++++
>  lib/arm64/asm-offsets.c      |   2 +
>  lib/arm64/asm/barrier.h      |   5 ++
>  lib/arm64/asm/bitops.h       |  51 +++++++++++++++++++
>  lib/arm64/asm/cpumask.h      |   1 +
>  lib/arm64/asm/processor.h    |  18 ++++++-
>  lib/arm64/asm/psci.h         |  15 ++++++
>  lib/arm64/asm/smp.h          |   1 +
>  lib/arm64/asm/spinlock.h     |   8 +--
>  lib/arm64/asm/thread_info.h  |   1 +
>  lib/arm64/asm/uapi-psci.h    |   1 +
>  lib/arm64/processor.c        |  81 +++++++++++++++++++----------
>  lib/arm64/spinlock.c         |  43 ++++++++++++++++
>  lib/report.c                 |  16 ++++++
>  lib/x86/asm/spinlock.h       |  11 ++++
>  lib/x86/smp.h                |   7 +--
>  38 files changed, 1008 insertions(+), 92 deletions(-)
>  create mode 100644 lib/arm/asm/bitops.h
>  create mode 100644 lib/arm/asm/cpumask.h
>  create mode 100644 lib/arm/asm/psci.h
>  create mode 100644 lib/arm/asm/smp.h
>  create mode 100644 lib/arm/asm/thread_info.h
>  create mode 100644 lib/arm/asm/uapi-psci.h
>  create mode 100644 lib/arm/bitops.c
>  create mode 100644 lib/arm/psci.c
>  create mode 100644 lib/arm/smp.c
>  create mode 100644 lib/arm64/asm/bitops.h
>  create mode 100644 lib/arm64/asm/cpumask.h
>  create mode 100644 lib/arm64/asm/psci.h
>  create mode 100644 lib/arm64/asm/smp.h
>  create mode 100644 lib/arm64/asm/thread_info.h
>  create mode 100644 lib/arm64/asm/uapi-psci.h
>  create mode 100644 lib/arm64/spinlock.c
>  create mode 100644 lib/x86/asm/spinlock.h
> 
> -- 
> 1.9.3

Applied, thanks.

Re: [kvm-unit-tests PATCH 00/18] arm/arm64: add smp support

[Christoffer Dall]

On Thu, Feb 26, 2015 at 02:50:38PM +0100, Andrew Jones wrote:
> On Thu, Feb 26, 2015 at 12:34:02PM +0100, Christoffer Dall wrote:
> > On Sun, Feb 01, 2015 at 07:34:28PM +0100, Andrew Jones wrote:
> > > This series extends the kvm-unit-tests/arm[64] framework to support smp.
> > > A break down of the patches is as follows
> > > 
> > > 01-02: prepare general framework for smp use
> > > 03-06: arm/arm64 fixups not 100% related to this series,
> > >        but need to post some time...
> > > 07-09: add thread_info (for per-thread data) and suck some global
> > >        data into it
> > > 10-11: add cpumask support (for per-cpu data) and suck some more
> > >        global data in
> > >    12: add arm64 simple spinlock implementation
> > > 13-14: add some PSCI support
> > > 15-16: further prep for smp_boot_secondary
> > >    17: finally add smp_boot_secondary
> > >    18: as usual, add a selftest to make sure it all works
> > > 
> > > These patches are also available here:
> > > https://github.com/rhdrjones/kvm-unit-tests/tree/arm/smp
> > > 
> > I've tested these patches on Juno and they seem to run fine, however,
> > since we don't support big.LITTLE yet, you have to run them under
> > 'taskset <mask>', but the config script uses $(getconf
> > _NPROCESSORS_CONF), which returns 6, and QEMU fails.  The interesting
> 
> Should I try to read the number of host cpus from some other source?
> If you know something I can read that also works on big.LITTLE, then
> I can change it now.
> 

I have no idea what the right scripting fix would be. But we should
really fix big.LITTLE support in KVM.  Hmmm.

> > bit is that the unit-tests still report 'PASS' - not sure why.
> 
> Ah, this is due to the weird way qemu's debugexit device sets its exit
> code
> 
> hw/misc/debugexit.c:debug_exit_write()
> {
>   exit((val << 1) | 1);
> }
> 
> To be consistent with that we made chr-testdev do the same thing (see
> backends/testdev.c:testdev_eat_packet():case 'q'). Now, the
> kvm-unit-tests run_tests.sh script knows about that, so it has
> 
>   eval $cmdline >> test.log
>   if [ $? -le 1 ]; then
>      echo -e "\e[32mPASS\e[0m $1"
>   else
>      echo -e "\e[31mFAIL\e[0m $1"
>   fi
> 
> Yes, this sucks, as we can't tell the difference between qemu failing
> to run the test, and exiting with 1 vs. the test running, passing -
> exiting with (0 << 1) | 1. It's too bad debugexit didn't set a higher
> bit (like 5 or 6) to flag a "debug exit". Maybe it's not too late to
> change it? Paolo?
> 

This would be really good to address somehow, because we don't want
to report that everything is happy when the test harness broke, that
really goes against the whole idea of this work.

Thanks,
-Christoffer