AArch64: factor CPU ID getters

This patch adds a simple utility to read the current CPU ID from C or
assembly. The underlying objective of this seemingly useless change is
to provide a clean base for C helpers. It introduces two include paths:
include/ and arch/*/include/asm

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

9 files changed, 61 insertions, 27 deletions

diff --git a/Makefile.am b/Makefile.am
index d74b361..92d146e 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -84,6 +84,7 @@ CHOSEN_NODE	:= chosen {						\
 endif
 
 CPPFLAGS	+= $(INITRD_FLAGS)
+CFLAGS		+= -Iinclude/ -I$(ARCH_SRC)/include/
 
 OFILES		+= $(addprefix $(ARCH_SRC),boot.o stack.o cache.o $(GIC) mmu.o ns.o $(BOOTMETHOD) utils.o)
 
@@ -95,7 +96,7 @@ $(IMAGE): $(OFILES) model.lds fdt.dtb $(KERNEL_IMAGE) $(FILESYSTEM)
 	$(LD) $(OFILES) -o $@ --script=model.lds
 
 %.o: %.S Makefile
-	$(CC) $(CPPFLAGS) $(CFLAGS) $(DEFINES) -c -o $@ $<
+	$(CC) $(CPPFLAGS) -D__ASSEMBLY__ $(CFLAGS) $(DEFINES) -c -o $@ $<
 
 model.lds: $(LD_SCRIPT) Makefile
 	$(CPP) $(CPPFLAGS) -ansi -DPHYS_OFFSET=$(PHYS_OFFSET) -DMBOX_OFFSET=$(MBOX_OFFSET) -DKERNEL_OFFSET=$(KERNEL_OFFSET) -DFDT_OFFSET=$(FDT_OFFSET) -DFS_OFFSET=$(FS_OFFSET) -DKERNEL=$(KERNEL_IMAGE) -DFILESYSTEM=$(FILESYSTEM) -P -C -o $@ $<
diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
index 72c33a9..f5ba572 100644
--- a/arch/aarch64/boot.S
+++ b/arch/aarch64/boot.S
@@ -13,9 +13,7 @@
 
 	.globl	_start
 _start:
-	mrs	x0, mpidr_el1
-	ldr	x1, =MPIDR_ID_BITS
-	and	x0, x0, x1
+	cpuid	x0, x1
 	bl	find_logical_id
 	cmp	x0, #MPIDR_INVALID
 	beq	err_invalid_id
diff --git a/arch/aarch64/common.S b/arch/aarch64/common.S
index 0e0fb8c..5a0baa9 100644
--- a/arch/aarch64/common.S
+++ b/arch/aarch64/common.S
@@ -7,8 +7,7 @@
  * found in the LICENSE.txt file.
  */
 
-#define MPIDR_ID_BITS	(0xff00ffffff)
-#define MPIDR_INVALID	(-1)
+#include <cpu.h>
 
 #define	CURRENTEL_EL3	(3 << 2)
 
@@ -35,3 +34,10 @@
 	msr	spsr_el3, \mode
 	eret
 	.endm
+
+	/* Put MPIDR into \dest, clobber \tmp and flags */
+	.macro cpuid dest, tmp
+	mrs	\dest, mpidr_el1
+	ldr	\tmp, =MPIDR_ID_BITS
+	ands	\dest, \dest, \tmp
+	.endm
diff --git a/arch/aarch64/gic-v3.S b/arch/aarch64/gic-v3.S
index 820d76f..0d02838 100644
--- a/arch/aarch64/gic-v3.S
+++ b/arch/aarch64/gic-v3.S
@@ -27,9 +27,7 @@ gic_secure_init:
 	/*
 	 * Only the primary CPU setups the (re)distributors.
 	 */
-	mrs	x0, mpidr_el1
-	ldr	x1, =MPIDR_ID_BITS
-	tst	x0, x1
+	cpuid	x0, x1
 	b.ne	setup_cpu_if			// secondary CPU
 
 	ldr	x1, =GIC_DIST_BASE		// GICD_CTLR
diff --git a/arch/aarch64/gic.S b/arch/aarch64/gic.S
index 5b612bb..bf3b8ea 100644
--- a/arch/aarch64/gic.S
+++ b/arch/aarch64/gic.S
@@ -18,9 +18,7 @@ gic_secure_init:
 	 * Check for the primary CPU to avoid a race on the distributor
 	 * registers.
 	 */
-	mrs	x0, mpidr_el1
-	ldr	x1, =MPIDR_ID_BITS
-	tst	x0, x1
+	cpuid	x0, x1
 	b.ne	1f				// secondary CPU
 
 	ldr	x1, =GIC_DIST_BASE		// GICD_CTLR
diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
new file mode 100644
index 0000000..df17164
--- /dev/null
+++ b/arch/aarch64/include/asm/cpu.h
@@ -0,0 +1,27 @@
+/*
+ * arch/aarch64/include/asm/cpu.h
+ *
+ * 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.
+ */
+#ifndef __ASM_AARCH64_CPU_H
+#define __ASM_AARCH64_CPU_H
+
+#define MPIDR_ID_BITS		0xff00ffffff
+
+#ifndef __ASSEMBLY__
+
+static inline unsigned long read_mpidr(void)
+{
+	unsigned long mpidr;
+
+	asm volatile ("mrs	%0, mpidr_el1\n" : "=r" (mpidr));
+	return mpidr & MPIDR_ID_BITS;
+}
+
+
+#endif /* !__ASSEMBLY__ */
+
+#endif
diff --git a/arch/aarch64/psci.S b/arch/aarch64/psci.S
index 04f1dbf..bfd54f1 100644
--- a/arch/aarch64/psci.S
+++ b/arch/aarch64/psci.S
@@ -89,9 +89,7 @@ psci_call64:
  * x1 - optional power state parameter, ignored here
  */
 psci_cpu_off:
-	mrs	x0, mpidr_el1
-	ldr	x1, =MPIDR_ID_BITS
-	and	x0, x0, x1
+	cpuid	x0, x1
 	bl	find_logical_id
 	adr	x1, branch_table
 	mov	x2, #ADDR_INVALID
@@ -142,9 +140,7 @@ start_el3:
 	bl	switch_to_idmap
 
 	/* only boot the primary cpu (entry 0 in the table) */
-	mrs	x0, mpidr_el1
-	ldr	x1, =MPIDR_ID_BITS
-	and	x0, x0, x1
+	cpuid	x0, x1
 	bl	find_logical_id
 	cbnz	x0, spin
 
@@ -159,9 +155,7 @@ start_el3:
  * When a valid address appears, branch to it.
  */
 spin:
-	mrs	x0, mpidr_el1
-	ldr	x1, =MPIDR_ID_BITS
-	and	x0, x0, x1
+	cpuid	x0, x1
 	bl	find_logical_id
 	cmp	x0, #-1
 	b.eq	spin_dead
@@ -189,9 +183,7 @@ spin:
  * primary cpu, all others will be trapped in an infinite loop.
  */
 start_no_el3:
-	mrs	x0, mpidr_el1
-	ldr	x1, =MPIDR_ID_BITS
-	and	x0, x0, x1
+	cpuid	x0, x1
 	bl	find_logical_id
 	cbz	x0, start_cpu0
 spin_dead:
diff --git a/arch/aarch64/spin.S b/arch/aarch64/spin.S
index 80d42f3..4997bac 100644
--- a/arch/aarch64/spin.S
+++ b/arch/aarch64/spin.S
@@ -33,9 +33,7 @@ start_no_el3:
 	mov	x2, xzr
 	mov	x3, xzr
 
-	mrs	x4, mpidr_el1
-	ldr	x5, =MPIDR_ID_BITS
-	tst	x4, x5
+	cpuid	x4, x5
 	b.eq	2f
 
 	/*
diff --git a/include/cpu.h b/include/cpu.h
new file mode 100644
index 0000000..0fb85be
--- /dev/null
+++ b/include/cpu.h
@@ -0,0 +1,16 @@
+/*
+ * include/cpu.h - Generic CPU features
+ *
+ * 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.
+ */
+#ifndef __CPU_H
+#define __CPU_H
+
+#include <asm/cpu.h>
+
+#define MPIDR_INVALID	(-1)
+
+#endif