score: PR2183: Fix context switch on SMP

Fix context switch on SMP for ARM, PowerPC and SPARC.

Atomically test and set the is executing indicator of the heir context
to ensure that at most one processor uses the heir context.  Break the
busy wait loop also due to heir updates.

6 files changed, 91 insertions, 28 deletions

diff --git a/cpukit/score/cpu/arm/arm_exc_interrupt.S b/cpukit/score/cpu/arm/arm_exc_interrupt.S
index c80a404615..e8026c869b 100644
--- a/cpukit/score/cpu/arm/arm_exc_interrupt.S
+++ b/cpukit/score/cpu/arm/arm_exc_interrupt.S
@@ -31,7 +31,6 @@
 #endif
 
 #include <rtems/asm.h>
-#include <rtems/score/percpu.h>
 
 #ifdef ARM_MULTILIB_ARCH_V4
 
@@ -49,17 +48,6 @@
 #define CONTEXT_LIST {r0, r1, r2, r3, EXCHANGE_LR, EXCHANGE_SPSR, SELF_CPU_CONTROL, r12}
 #define CONTEXT_SIZE 32
 
-.macro GET_SELF_CPU_CONTROL REG, TMP
-	ldr	\REG, =_Per_CPU_Information
-#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
-#endif
-.endm
-
 .arm
 .globl _ARMV4_Exception_interrupt
 _ARMV4_Exception_interrupt:
diff --git a/cpukit/score/cpu/arm/cpu_asm.S b/cpukit/score/cpu/arm/cpu_asm.S
index 7502923974..d4355b4e98 100644
--- a/cpukit/score/cpu/arm/cpu_asm.S
+++ b/cpukit/score/cpu/arm/cpu_asm.S
@@ -19,6 +19,8 @@
  *  COPYRIGHT (c) 2000 Canon Research Centre France SA.
  *  Emmanuel Raguet, mailto:raguet@crf.canon.fr
  *
+ *  Copyright (c) 2013-2014 embedded brains GmbH
+ *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
  *  http://www.rtems.org/license/LICENSE.
@@ -30,7 +32,6 @@
 #endif
 
 #include <rtems/asm.h>
-#include <rtems/score/cpu.h>
 
 #ifdef ARM_MULTILIB_ARCH_V4
 
@@ -73,21 +74,25 @@ DEFINE_FUNCTION_ARM(_CPU_Context_switch)
 	mov	r3, #0
 	strb	r3, [r0, #ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET]
 
-	/* Wait for heir context to stop execution */
-1:
-	ldrb	r3, [r1, #ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET]
-	cmp	r3, #0
-	bne	1b
+.L_check_is_executing:
+
+	/* Check the is executing indicator of the heir context */
+	add	r3, r1, #ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET
+	ldrexb	r4, [r3]
+	cmp	r4, #0
+	bne	.L_check_thread_dispatch_necessary
 
-	/* The heir context executes now on this processor */
+	/* Try to update the is executing indicator of the heir context */
+	mov	r4, #1
+	strexb	r5, r4, [r3]
+	cmp	r5, #0
+	bne	.L_check_thread_dispatch_necessary
 	dmb
-	mov	r3, #1
-	strb	r3, [r1, #ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET]
 #endif
 
 /* Start restoring context */
 .L_restore:
-#ifdef ARM_MULTILIB_HAS_LOAD_STORE_EXCLUSIVE
+#if !defined(RTEMS_SMP) && defined(ARM_MULTILIB_HAS_LOAD_STORE_EXCLUSIVE)
 	clrex
 #endif
 
@@ -120,4 +125,35 @@ DEFINE_FUNCTION_ARM(_CPU_Context_restore)
         mov     r1, r0
         b       .L_restore
 
+#ifdef RTEMS_SMP
+.L_check_thread_dispatch_necessary:
+
+	GET_SELF_CPU_CONTROL	r2, r3
+
+	/* Check if a thread dispatch is necessary */
+	ldrb	r4, [r2, #PER_CPU_DISPATCH_NEEDED]
+	cmp	r4, #0
+	beq	.L_check_is_executing
+
+	/* We have a new heir */
+
+	/* Clear the thread dispatch necessary flag */
+	mov	r4, #0
+	strb	r4, [r2, #PER_CPU_DISPATCH_NEEDED]
+	dmb
+
+	/* Read the executing and heir */
+	ldr	r4, [r2, #PER_CPU_OFFSET_EXECUTING]
+	ldr	r5, [r2, #PER_CPU_OFFSET_HEIR]
+
+	/* Calculate the heir context pointer */
+	sub	r4, r1, r4
+	add	r1, r5, r4
+
+	/* Update the executing */
+	str	r5, [r2, #PER_CPU_OFFSET_EXECUTING]
+
+	b	.L_check_is_executing
+#endif
+
 #endif /* ARM_MULTILIB_ARCH_V4 */
diff --git a/cpukit/score/cpu/arm/rtems/asm.h b/cpukit/score/cpu/arm/rtems/asm.h
index 6539a38913..d22514d60a 100644
--- a/cpukit/score/cpu/arm/rtems/asm.h
+++ b/cpukit/score/cpu/arm/rtems/asm.h
@@ -41,8 +41,7 @@
 #ifndef ASM
 #define ASM
 #endif
-#include <rtems/score/cpuopts.h>
-#include <rtems/score/arm.h>
+#include <rtems/score/percpu.h>
 
 /**
  * @defgroup ScoreCPUARMASM ARM Assembler Support
@@ -188,6 +187,17 @@
 #endif /* __thumb__ */
 .endm
 
+.macro GET_SELF_CPU_CONTROL REG, TMP
+	ldr	\REG, =_Per_CPU_Information
+#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
+#endif
+.endm
+
 /** @} */
 
 #endif /* _RTEMS_ASM_H */
diff --git a/cpukit/score/cpu/no_cpu/rtems/score/cpu.h b/cpukit/score/cpu/no_cpu/rtems/score/cpu.h
index 9570fb6c8d..0fcc6e719c 100644
--- a/cpukit/score/cpu/no_cpu/rtems/score/cpu.h
+++ b/cpukit/score/cpu/no_cpu/rtems/score/cpu.h
@@ -578,11 +578,40 @@ typedef struct {
 #ifdef RTEMS_SMP
     /**
      * @brief On SMP configurations the thread context must contain a boolean
-     * indicator if this context is executing on a processor.
+     * indicator to signal if this context is executing on a processor.
      *
      * This field must be updated during a context switch.  The context switch
      * to the heir must wait until the heir context indicates that it is no
-     * longer executing on a processor.
+     * longer executing on a processor.  The context switch must also check if
+     * a thread dispatch is necessary to honor updates of the heir thread for
+     * this processor.  This indicator must be updated using an atomic test and
+     * set operation to ensure that at most one processor uses the heir
+     * context at the same time.
+     *
+     * @code
+     * void _CPU_Context_switch(
+     *   Context_Control *executing,
+     *   Context_Control *heir
+     * )
+     * {
+     *   save( executing );
+     *
+     *   executing->is_executing = false;
+     *   memory_barrier();
+     *
+     *   if ( test_and_set( &heir->is_executing ) ) {
+     *     do {
+     *       Per_CPU_Control *cpu_self = _Per_CPU_Get_snapshot();
+     *
+     *       if ( cpu_self->dispatch_necessary ) {
+     *         heir = _Thread_Get_heir_and_make_it_executing( cpu_self );
+     *       }
+     *     } while ( test_and_set( &heir->is_executing ) );
+     *   }
+     *
+     *   restore( heir );
+     * }
+     * @endcode
      */
     volatile bool is_executing;
 #endif
diff --git a/cpukit/score/cpu/powerpc/rtems/score/cpu.h b/cpukit/score/cpu/powerpc/rtems/score/cpu.h
index 3cad329157..13f50ade77 100644
--- a/cpukit/score/cpu/powerpc/rtems/score/cpu.h
+++ b/cpukit/score/cpu/powerpc/rtems/score/cpu.h
@@ -303,7 +303,7 @@ typedef struct {
   PPC_GPR_TYPE gpr31;
   uint32_t gpr2;
   #ifdef RTEMS_SMP
-    volatile bool is_executing;
+    volatile uint32_t is_executing;
   #endif
   #ifdef __ALTIVEC__
     /*
diff --git a/cpukit/score/cpu/sparc/rtems/score/cpu.h b/cpukit/score/cpu/sparc/rtems/score/cpu.h
index c01000584f..39b78258c1 100644
--- a/cpukit/score/cpu/sparc/rtems/score/cpu.h
+++ b/cpukit/score/cpu/sparc/rtems/score/cpu.h
@@ -475,7 +475,7 @@ typedef struct {
   uint32_t   isr_dispatch_disable;
 
 #if defined(RTEMS_SMP)
-  volatile bool is_executing;
+  volatile uint32_t is_executing;
 #endif
 } Context_Control;