arm: Optimize context switch

Set CPU_ENABLE_ROBUST_THREAD_DISPATCH to TRUE.  In this case the
interrupts are always enabled during a context switch even after
interrupt processing (see #2751).  Remove the CPSR from the context
control since it contains only volatile bits.

Close #2954.

4 files changed, 55 insertions, 90 deletions

diff --git a/c/src/lib/libbsp/arm/tms570/startup/bspstart.c b/c/src/lib/libbsp/arm/tms570/startup/bspstart.c
index 7c1e9a17bf..025bb741d2 100644
--- a/c/src/lib/libbsp/arm/tms570/startup/bspstart.c
+++ b/c/src/lib/libbsp/arm/tms570/startup/bspstart.c
@@ -35,18 +35,6 @@ void bsp_start( void )
   void *need_remap_ptr;
   unsigned int need_remap_int;
 
-  #if BYTE_ORDER == BIG_ENDIAN
-    /*
-     * If CPU is big endian (TMS570 family variant)
-     * set the CPU mode to supervisor and big endian.
-     * Do not set mode if CPU is little endian
-     * (RM48 family variant) for which default mode 0x13
-     * defined in cpukit/score/cpu/arm/cpu.c
-     * is right.
-     */
-    arm_cpu_mode = 0x213;
-  #endif
-
   tms570_initialize_and_clear();
 
   /*
diff --git a/cpukit/score/cpu/arm/cpu.c b/cpukit/score/cpu/arm/cpu.c
index b5738b11f4..01a43b3cfd 100644
--- a/cpukit/score/cpu/arm/cpu.c
+++ b/cpukit/score/cpu/arm/cpu.c
@@ -15,7 +15,7 @@
  *
  *  Copyright (c) 2007 Ray xu <rayx.cn@gmail.com>
  *
- *  Copyright (c) 2009, 2016 embedded brains GmbH
+ *  Copyright (c) 2009, 2017 embedded brains GmbH
  *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
@@ -26,14 +26,10 @@
 #include "config.h"
 #endif
 
-#include <rtems/system.h>
-#include <rtems.h>
-#include <rtems/bspIo.h>
-#include <rtems/score/isr.h>
-#include <rtems/score/wkspace.h>
+#include <rtems/score/assert.h>
+#include <rtems/score/cpu.h>
 #include <rtems/score/thread.h>
 #include <rtems/score/tls.h>
-#include <rtems/score/cpu.h>
 
 #ifdef ARM_MULTILIB_VFP
   RTEMS_STATIC_ASSERT(
@@ -89,12 +85,6 @@ RTEMS_STATIC_ASSERT(
 
 #ifdef ARM_MULTILIB_ARCH_V4
 
-/*
- * This variable can be used to change the running mode of the execution
- * contexts.
- */
-uint32_t arm_cpu_mode = 0x13;
-
 void _CPU_Context_Initialize(
   Context_Control *the_context,
   void *stack_area_begin,
@@ -105,10 +95,10 @@ void _CPU_Context_Initialize(
   void *tls_area
 )
 {
+  (void) new_level;
+
   the_context->register_sp = (uint32_t) stack_area_begin + stack_area_size;
   the_context->register_lr = (uint32_t) entry_point;
-  the_context->register_cpsr = ( ( new_level != 0 ) ? ARM_PSR_I : 0 )
-    | arm_cpu_mode;
   the_context->isr_dispatch_disable = 0;
 
 #ifdef ARM_MULTILIB_HAS_THREAD_ID_REGISTER
@@ -120,25 +110,20 @@ void _CPU_Context_Initialize(
   }
 }
 
-/* Preprocessor magic for stringification of x */
-#define _CPU_ISR_LEVEL_DO_STRINGOF( x) #x
-#define _CPU_ISR_LEVEL_STRINGOF( x) _CPU_ISR_LEVEL_DO_STRINGOF( x)
-
 void _CPU_ISR_Set_level( uint32_t level )
 {
   uint32_t arm_switch_reg;
 
-  level = ( level != 0 ) ? ARM_PSR_I : 0;
+  /* Ignore the level parameter and just enable interrupts */
+  (void) level;
 
   __asm__ volatile (
     ARM_SWITCH_TO_ARM
     "mrs %[arm_switch_reg], cpsr\n"
-    "bic %[arm_switch_reg], #" _CPU_ISR_LEVEL_STRINGOF( ARM_PSR_I ) "\n"
-    "orr %[arm_switch_reg], %[level]\n"
+    "bic %[arm_switch_reg], #" RTEMS_XSTRING( ARM_PSR_I ) "\n"
     "msr cpsr, %0\n"
     ARM_SWITCH_BACK
     : [arm_switch_reg] "=&r" (arm_switch_reg)
-    : [level] "r" (level)
   );
 }
 
@@ -150,7 +135,7 @@ uint32_t _CPU_ISR_Get_level( void )
   __asm__ volatile (
     ARM_SWITCH_TO_ARM
     "mrs %[level], cpsr\n"
-    "and %[level], #" _CPU_ISR_LEVEL_STRINGOF( ARM_PSR_I ) "\n"
+    "and %[level], #" RTEMS_XSTRING( ARM_PSR_I ) "\n"
     ARM_SWITCH_BACK
     : [level] "=&r" (level) ARM_SWITCH_ADDITIONAL_OUTPUT
   );
diff --git a/cpukit/score/cpu/arm/cpu_asm.S b/cpukit/score/cpu/arm/cpu_asm.S
index f10cd90ed8..52ea77aae1 100644
--- a/cpukit/score/cpu/arm/cpu_asm.S
+++ b/cpukit/score/cpu/arm/cpu_asm.S
@@ -19,7 +19,7 @@
  *  COPYRIGHT (c) 2000 Canon Research Centre France SA.
  *  Emmanuel Raguet, mailto:raguet@crf.canon.fr
  *
- *  Copyright (c) 2013, 2016 embedded brains GmbH
+ *  Copyright (c) 2013, 2017 embedded brains GmbH
  *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
@@ -55,19 +55,21 @@
 
 DEFINE_FUNCTION_ARM(_CPU_Context_switch)
 /* Start saving context */
-	mrs	r2, CPSR
-	stmia	r0,  {r2, r4, r5, r6, r7, r8, r9, r10, r11, r13, r14}
-
 	GET_SELF_CPU_CONTROL	r2
+	stm	r0, {r4, r5, r6, r7, r8, r9, r10, r11, r13, r14}
+
+#ifdef ARM_MULTILIB_HAS_THREAD_ID_REGISTER
+	mrc	p15, 0, r3, c13, c0, 3
+#endif
+
 	ldr	r4, [r2, #PER_CPU_ISR_DISPATCH_DISABLE]
 
 #ifdef ARM_MULTILIB_VFP
-	add	r3, r0, #ARM_CONTEXT_CONTROL_D8_OFFSET
-	vstm	r3, {d8-d15}
+	add	r5, r0, #ARM_CONTEXT_CONTROL_D8_OFFSET
+	vstm	r5, {d8-d15}
 #endif
 
 #ifdef ARM_MULTILIB_HAS_THREAD_ID_REGISTER
-	mrc	p15, 0, r3, c13, c0, 3
 	str	r3, [r0, #ARM_CONTEXT_CONTROL_THREAD_ID_OFFSET]
 #endif
 
@@ -106,28 +108,31 @@ DEFINE_FUNCTION_ARM(_CPU_Context_switch)
 	clrex
 #endif
 
-	ldr	r4, [r1, #ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE]
-
 #ifdef ARM_MULTILIB_HAS_THREAD_ID_REGISTER
 	ldr	r3, [r1, #ARM_CONTEXT_CONTROL_THREAD_ID_OFFSET]
-	mcr	p15, 0, r3, c13, c0, 3
 #endif
 
+	ldr	r4, [r1, #ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE]
+
 #ifdef ARM_MULTILIB_VFP
-	add	r3, r1, #ARM_CONTEXT_CONTROL_D8_OFFSET
-	vldm	r3, {d8-d15}
+	add	r5, r1, #ARM_CONTEXT_CONTROL_D8_OFFSET
+	vldm	r5, {d8-d15}
+#endif
+
+#ifdef ARM_MULTILIB_HAS_THREAD_ID_REGISTER
+	mcr	p15, 0, r3, c13, c0, 3
 #endif
 
 	str	r4, [r2, #PER_CPU_ISR_DISPATCH_DISABLE]
 
-	ldmia	r1,  {r2, r4, r5, r6, r7, r8, r9, r10, r11, r13, r14}
-	msr	CPSR_fsxc, r2
-#ifdef __thumb__
-	bx	lr
-	nop
+	/* In ARMv5T and above the load of PC is an interworking branch */
+#if __ARM_ARCH >= 5
+	ldm	r1, {r4, r5, r6, r7, r8, r9, r10, r11, r13, pc}
 #else
-	mov	pc, lr
+	ldm	r1, {r4, r5, r6, r7, r8, r9, r10, r11, r13, r14}
+	bx	lr
 #endif
+
 /*
  *  void _CPU_Context_restore( new_context )
  *
diff --git a/cpukit/score/cpu/arm/rtems/score/cpu.h b/cpukit/score/cpu/arm/rtems/score/cpu.h
index b10bc7195c..05e236c75a 100644
--- a/cpukit/score/cpu/arm/rtems/score/cpu.h
+++ b/cpukit/score/cpu/arm/rtems/score/cpu.h
@@ -8,7 +8,7 @@
  *  This include file contains information pertaining to the ARM
  *  processor.
  *
- *  Copyright (c) 2009, 2016 embedded brains GmbH
+ *  Copyright (c) 2009, 2017 embedded brains GmbH
  *
  *  Copyright (c) 2007 Ray Xu <Rayx.cn@gmail.com>
  *
@@ -120,11 +120,7 @@
 
 #define CPU_USE_DEFERRED_FP_SWITCH FALSE
 
-#if defined(ARM_MULTILIB_ARCH_V7M)
-  #define CPU_ENABLE_ROBUST_THREAD_DISPATCH TRUE
-#else
-  #define CPU_ENABLE_ROBUST_THREAD_DISPATCH FALSE
-#endif
+#define CPU_ENABLE_ROBUST_THREAD_DISPATCH TRUE
 
 #if defined(ARM_MULTILIB_HAS_WFI)
   #define CPU_PROVIDES_IDLE_THREAD_BODY TRUE
@@ -142,20 +138,6 @@
 
 #define CPU_STRUCTURE_ALIGNMENT RTEMS_ALIGNED( CPU_CACHE_LINE_BYTES )
 
-/*
- * The interrupt mask disables only normal interrupts (IRQ).
- *
- * In order to support fast interrupts (FIQ) such that they can do something
- * useful, we have to disable the operating system support for FIQs.  Having
- * operating system support for them would require that FIQs are disabled
- * during critical sections of the operating system and application.  At this
- * level IRQs and FIQs would be equal.  It is true that FIQs could interrupt
- * the non critical sections of IRQs, so here they would have a small
- * advantage.  Without operating system support, the FIQs can execute at any
- * time (of course not during the service of another FIQ). If someone needs
- * operating system support for a FIQ, she can trigger a software interrupt and
- * service the request in a two-step process.
- */
 #define CPU_MODES_INTERRUPT_MASK 0x1
 
 #define CPU_CONTEXT_FP_SIZE sizeof( Context_Control_fp )
@@ -206,20 +188,16 @@
 #endif
 
 #ifdef ARM_MULTILIB_ARCH_V4
-  #if defined(ARM_MULTILIB_VFP)
-    #define ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE 112
-  #elif defined(ARM_MULTILIB_HAS_THREAD_ID_REGISTER)
-    #define ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE 48
-  #else
-    #define ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE 44
-  #endif
+  #define ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE 40
 #endif
 
 #ifdef RTEMS_SMP
-  #ifdef ARM_MULTILIB_VFP
-    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 116
+  #if defined(ARM_MULTILIB_VFP)
+    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 112
+  #elif defined(ARM_MULTILIB_HAS_THREAD_ID_REGISTER)
+    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 48
   #else
-    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 52
+    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 44
   #endif
 #endif
 
@@ -244,7 +222,6 @@ extern "C" {
 
 typedef struct {
 #if defined(ARM_MULTILIB_ARCH_V4)
-  uint32_t register_cpsr;
   uint32_t register_r4;
   uint32_t register_r5;
   uint32_t register_r6;
@@ -255,6 +232,7 @@ typedef struct {
   uint32_t register_fp;
   uint32_t register_sp;
   uint32_t register_lr;
+  uint32_t isr_dispatch_disable;
 #elif defined(ARM_MULTILIB_ARCH_V6M) || defined(ARM_MULTILIB_ARCH_V7M)
   uint32_t register_r4;
   uint32_t register_r5;
@@ -283,9 +261,6 @@ typedef struct {
   uint64_t register_d14;
   uint64_t register_d15;
 #endif
-#ifdef ARM_MULTILIB_ARCH_V4
-  uint32_t isr_dispatch_disable;
-#endif
 #ifdef RTEMS_SMP
   volatile bool is_executing;
 #endif
@@ -295,8 +270,6 @@ typedef struct {
   /* Not supported */
 } Context_Control_fp;
 
-extern uint32_t arm_cpu_mode;
-
 static inline void _ARM_Data_memory_barrier( void )
 {
 #ifdef ARM_MULTILIB_HAS_BARRIER_INSTRUCTIONS
@@ -331,6 +304,20 @@ static inline uint32_t arm_interrupt_disable( void )
 #if defined(ARM_MULTILIB_ARCH_V4)
   uint32_t arm_switch_reg;
 
+  /*
+   * Disable only normal interrupts (IRQ).
+   *
+   * In order to support fast interrupts (FIQ) such that they can do something
+   * useful, we have to disable the operating system support for FIQs.  Having
+   * operating system support for them would require that FIQs are disabled
+   * during critical sections of the operating system and application.  At this
+   * level IRQs and FIQs would be equal.  It is true that FIQs could interrupt
+   * the non critical sections of IRQs, so here they would have a small
+   * advantage.  Without operating system support, the FIQs can execute at any
+   * time (of course not during the service of another FIQ). If someone needs
+   * operating system support for a FIQ, she can trigger a software interrupt and
+   * service the request in a two-step process.
+   */
   __asm__ volatile (
     ARM_SWITCH_TO_ARM
     "mrs %[level], cpsr\n"