AArch64: extract common utilities from PSCI

Helpers find_logical_id and setup_vector, as well as the number of
expected CPUs, will be needed outside of PSCI during future refactoring
work. This patch puts them into utils.S, and moves the number of CPUs
one level up, in a macro defined by the Makefile.

Signed-off-by: Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
Signed-off-by: Mark Rutland <mark.rutland@arm.com>

4 files changed, 70 insertions, 48 deletions

diff --git a/Makefile.am b/Makefile.am
index eecaa10..8a2069d 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -13,8 +13,11 @@ UART_BASE	:= $(shell perl -I $(top_srcdir) $(top_srcdir)/findbase.pl $(KERNEL_DT
 SYSREGS_BASE	:= $(shell perl -I $(top_srcdir) $(top_srcdir)/findbase.pl $(KERNEL_DTB) 0 'arm,vexpress-sysreg')
 CNTFRQ		:= 0x01800000	# 24Mhz
 
+NR_CPUS		:= $(shell echo $(CPU_IDS) | tr ',' ' ' | wc -w)
+
 DEFINES		= -DCNTFRQ=$(CNTFRQ)
 DEFINES		+= -DCPU_IDS=$(CPU_IDS)
+DEFINES		+= -DNR_CPUS=$(NR_CPUS)
 DEFINES		+= -DSYSREGS_BASE=$(SYSREGS_BASE)
 DEFINES		+= -DUART_BASE=$(UART_BASE)
 
@@ -81,7 +84,7 @@ endif
 
 CPPFLAGS	+= $(INITRD_FLAGS)
 
-OFILES		+= $(addprefix $(ARCH_SRC),boot.o cache.o $(GIC) mmu.o ns.o $(BOOTMETHOD))
+OFILES		+= $(addprefix $(ARCH_SRC),boot.o cache.o $(GIC) mmu.o ns.o $(BOOTMETHOD) utils.o)
 
 all: $(IMAGE)
 
diff --git a/arch/aarch64/common.S b/arch/aarch64/common.S
index 0abf430..0e0fb8c 100644
--- a/arch/aarch64/common.S
+++ b/arch/aarch64/common.S
@@ -8,6 +8,7 @@
  */
 
 #define MPIDR_ID_BITS	(0xff00ffffff)
+#define MPIDR_INVALID	(-1)
 
 #define	CURRENTEL_EL3	(3 << 2)
 
diff --git a/arch/aarch64/psci.S b/arch/aarch64/psci.S
index 49a41e8..04f1dbf 100644
--- a/arch/aarch64/psci.S
+++ b/arch/aarch64/psci.S
@@ -20,7 +20,6 @@
 #error No CPU MPIDRs provided.
 #endif
 
-#define MPIDR_INVALID		(-1)
 #define ADDR_INVALID		(-1)
 
 	.macro	ventry	label
@@ -57,25 +56,8 @@ vector:
 	ventry	err_exception
 
 	.data
-	/*
-	 * Array of the CPU ID (MPIDR & MPIDR_ID_BITS) of each CPU in the system.
-	 * The index into the array is used as a logical id, and an index into
-	 * the branch table. The branch table is automatically padded to the
-	 * same size as the id table.
-	 *
-	 * The first CPU in the table is considered to be the primary CPU, and
-	 * is the only CPU to immediately branch off to the kernel.
-	 */
-	.align 3
-id_table:
-	.quad CPU_IDS
-__id_end:
-	.quad MPIDR_INVALID
-
-.equ	nr_cpus, ((__id_end - id_table) / 8)
-
 branch_table:
-	.rept (nr_cpus)
+	.rept (NR_CPUS)
 	.quad ADDR_INVALID
 	.endr
 
@@ -154,35 +136,8 @@ psci_cpu_on:
 	eret
 
 
-/*
- * Takes masked MPIDR in x0, returns logical id in x0
- * Returns -1 for unknown MPIDRs
- * Clobbers x1, x2, x3
- */
-find_logical_id:
-__find_logical_index:
-	adr	x2, id_table
-	mov	x1, xzr
-1:	mov	x3, #nr_cpus	// check we haven't walked off the end of the array
-	cmp	x1, x3
-	b.gt	3f
-	ldr	x3, [x2, x1, lsl #3]
-	cmp	x3, x0
-	b.eq	2f
-	add	x1, x1, #1
-	b 1b
-2:	mov	x0, x1
-	ret
-3:	mov	x0, #-1
-	ret
-
-setup_vector:
-	ldr	x0, =vector
-	msr	VBAR_EL3, x0
-	isb
-	ret
-
 start_el3:
+	ldr	x0, =vector
 	bl	setup_vector
 	bl	switch_to_idmap
 
diff --git a/arch/aarch64/utils.S b/arch/aarch64/utils.S
new file mode 100644
index 0000000..cca0c11
--- /dev/null
+++ b/arch/aarch64/utils.S
@@ -0,0 +1,63 @@
+/*
+ * arch/aarch64/utils.S - basic utilities
+ *
+ * Copyright (C) 2015 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+
+	.globl find_logical_id
+	.globl setup_vector
+
+#include "common.S"
+
+	.data
+
+	/*
+	 * Array of the CPU ID (MPIDR & MPIDR_ID_BITS) of each CPU in the system.
+	 * The index into the array is used as a logical id, and an index into
+	 * the branch table. The branch table is automatically padded to the
+	 * same size as the id table.
+	 *
+	 * The first CPU in the table is considered to be the primary CPU, and
+	 * is the only CPU to immediately branch off to the kernel.
+	 */
+	.align 3
+id_table:
+	.quad CPU_IDS
+__id_end:
+	.quad MPIDR_INVALID
+
+	.text
+
+/*
+ * Takes masked MPIDR in x0, returns logical id in x0
+ * Returns -1 for unknown MPIDRs
+ * Sets the Z flag when CPU is primary
+ * Clobbers x1, x2, x3
+ */
+find_logical_id:
+	ldr	x2, =id_table
+	mov	x1, xzr
+1:	mov	x3, #NR_CPUS	// check we haven't walked off the end of the array
+	cmp	x1, x3
+	b.gt	3f
+	ldr	x3, [x2, x1, lsl #3]
+	cmp	x3, x0
+	b.eq	2f
+	add	x1, x1, #1
+	b 1b
+2:	subs	x0, x1, #0
+	ret
+3:	mov	x0, #MPIDR_INVALID
+	ret
+
+/*
+ * Setup EL3 vectors
+ * x0: vector address
+ */
+setup_vector:
+	msr	VBAR_EL3, x0
+	isb
+	ret