arm: Use TPIDRPRW for current per-CPU control

Use the previously unused TPIDRPRW register to get the per-CPU control
of the current processor.  This avoids instructions in
GET_SELF_CPU_CONTROL which are not available in Thumb mode.

5 files changed, 43 insertions, 14 deletions

diff --git a/c/src/lib/libbsp/arm/shared/start/start.S b/c/src/lib/libbsp/arm/shared/start/start.S
index cb66d82483..7adcb443c2 100644
--- a/c/src/lib/libbsp/arm/shared/start/start.S
+++ b/c/src/lib/libbsp/arm/shared/start/start.S
@@ -5,10 +5,10 @@
  */
 
 /*
- * Copyright (c) 2008-2014 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2008, 2016 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
- *  Obere Lagerstr. 30
+ *  Dornierstr. 4
  *  82178 Puchheim
  *  Germany
  *  <rtems@embedded-brains.de>
@@ -20,7 +20,7 @@
 
 #include <rtems/asm.h>
 #include <rtems/system.h>	
-#include <rtems/score/cpu.h>
+#include <rtems/score/percpu.h>
 	
 #include <bspopts.h>
 #include <bsp/irq.h>
@@ -184,11 +184,19 @@ _start:
 #endif
 
 #ifdef RTEMS_SMP
-	/* Read MPIDR */
+	/* Read MPIDR and get current processor index */
 	mrc	p15, 0, r0, c0, c0, 5
+	and	r0, #0xff
+
+        /*
+         * Get current per-CPU control and store it in PL1 only Thread ID
+         * Register (TPIDRPRW).
+         */
+	ldr	r1, =_Per_CPU_Information
+	add	r1, r1, r0, asl #PER_CPU_CONTROL_SIZE_LOG2
+	mcr	p15, 0, r1, c13, c0, 4
 
 	/* Calculate stack offset */
-	and	r0, #0xff
 	ldr	r1, =bsp_stack_all_size
 	mul	r1, r0
 #endif
diff --git a/cpukit/score/cpu/arm/arm_exc_interrupt.S b/cpukit/score/cpu/arm/arm_exc_interrupt.S
index fcb1510b95..61a9c3e65a 100644
--- a/cpukit/score/cpu/arm/arm_exc_interrupt.S
+++ b/cpukit/score/cpu/arm/arm_exc_interrupt.S
@@ -86,7 +86,7 @@ _ARMV4_Exception_interrupt:
 #endif /* ARM_MULTILIB_VFP */
 
 	/* Get per-CPU control of current processor */
-	GET_SELF_CPU_CONTROL	SELF_CPU_CONTROL, r1
+	GET_SELF_CPU_CONTROL	SELF_CPU_CONTROL
 
 	/* Remember INT stack pointer */
 	mov	r1, EXCHANGE_INT_SP
diff --git a/cpukit/score/cpu/arm/cpu_asm.S b/cpukit/score/cpu/arm/cpu_asm.S
index 6ee9e7418f..cf94822eaf 100644
--- a/cpukit/score/cpu/arm/cpu_asm.S
+++ b/cpukit/score/cpu/arm/cpu_asm.S
@@ -128,7 +128,7 @@ DEFINE_FUNCTION_ARM(_CPU_Context_restore)
 #ifdef RTEMS_SMP
 .L_get_potential_new_heir:
 
-	GET_SELF_CPU_CONTROL	r2, r3
+	GET_SELF_CPU_CONTROL	r2
 
 	/* We may have a new heir */
 
diff --git a/cpukit/score/cpu/arm/rtems/asm.h b/cpukit/score/cpu/arm/rtems/asm.h
index d22514d60a..ec5ddc94ac 100644
--- a/cpukit/score/cpu/arm/rtems/asm.h
+++ b/cpukit/score/cpu/arm/rtems/asm.h
@@ -187,14 +187,12 @@
 #endif /* __thumb__ */
 .endm
 
-.macro GET_SELF_CPU_CONTROL REG, TMP
-	ldr	\REG, =_Per_CPU_Information
+.macro GET_SELF_CPU_CONTROL REG
 #ifdef RTEMS_SMP
-	/* Use ARMv7 Multiprocessor Affinity Register (MPIDR) */
-	mrc	p15, 0, \TMP, c0, c0, 5
-
-	and	\TMP, \TMP, #0xff
-	add	\REG, \REG, \TMP, asl #PER_CPU_CONTROL_SIZE_LOG2
+	/* Use PL1 only Thread ID Register (TPIDRPRW) */
+	mrc	p15, 0, \REG, c13, c0, 4
+#else
+	ldr	\REG, =_Per_CPU_Information
 #endif
 .endm
 
diff --git a/cpukit/score/cpu/arm/rtems/score/cpuimpl.h b/cpukit/score/cpu/arm/rtems/score/cpuimpl.h
index 6b8b601cb0..1e9e101219 100644
--- a/cpukit/score/cpu/arm/rtems/score/cpuimpl.h
+++ b/cpukit/score/cpu/arm/rtems/score/cpuimpl.h
@@ -21,6 +21,29 @@
 extern "C" {
 #endif
 
+#ifdef ARM_MULTILIB_ARCH_V4
+
+#ifdef RTEMS_SMP
+
+static inline struct Per_CPU_Control *_ARM_Get_current_per_CPU_control( void )
+{
+  struct Per_CPU_Control *cpu_self;
+
+  /* Use PL1 only Thread ID Register (TPIDRPRW) */
+  __asm__ volatile (
+    "mrc p15, 0, %0, c13, c0, 4"
+    : "=r" ( cpu_self )
+  );
+
+  return cpu_self;
+}
+
+#define _CPU_Get_current_per_CPU_control() _ARM_Get_current_per_CPU_control()
+
+#endif /* RTEMS_SMP */
+
+#endif /* ARM_MULTILIB_ARCH_V4 */
+
 #ifdef __cplusplus
 }
 #endif