3 files changed, 48 insertions, 0 deletions
diff --git a/cpukit/score/cpu/arm/cpu.c b/cpukit/score/cpu/arm/cpu.c
index 24a9249300..91109e4e1d 100644
--- a/cpukit/score/cpu/arm/cpu.c
+++ b/cpukit/score/cpu/arm/cpu.c
@@ -50,6 +50,14 @@
   );
 #endif
 
+#ifdef RTEMS_SMP
+  RTEMS_STATIC_ASSERT(
+    offsetof( Context_Control, is_executing )
+      == ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET,
+    ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET
+  );
+#endif
+
 RTEMS_STATIC_ASSERT(
   sizeof( CPU_Exception_frame ) == ARM_EXCEPTION_FRAME_SIZE,
   ARM_EXCEPTION_FRAME_SIZE
@@ -93,6 +101,10 @@ void _CPU_Context_Initialize(
   the_context->thread_id = (uint32_t) tls_area;
 #endif
 
+#ifdef RTEMS_SMP
+  the_context->is_executing = false;
+#endif
+
   if ( tls_area != NULL ) {
     _TLS_TCB_at_area_begin_initialize( tls_area );
   }
diff --git a/cpukit/score/cpu/arm/cpu_asm.S b/cpukit/score/cpu/arm/cpu_asm.S
index bae7207f2c..f2c4afe39e 100644
--- a/cpukit/score/cpu/arm/cpu_asm.S
+++ b/cpukit/score/cpu/arm/cpu_asm.S
@@ -67,12 +67,32 @@ DEFINE_FUNCTION_ARM(_CPU_Context_switch)
 	str	r3, [r0, #ARM_CONTEXT_CONTROL_THREAD_ID_OFFSET]
 #endif
 
+#ifdef RTEMS_SMP
+	/* Indicate that this context is no longer executing */
+	dmb
+	mov	r3, #0
+	strb	r3, [r0, #ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET]
+#endif
+
 /* Start restoring context */
 _restore:
 #ifdef ARM_MULTILIB_HAS_LOAD_STORE_EXCLUSIVE
 	clrex
 #endif
 
+#ifdef RTEMS_SMP
+	/* Wait for context to stop execution if necessary */
+1:
+	ldrb	r3, [r1, #ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET]
+	cmp	r3, #0
+	bne	1b
+
+	/* Indicate that this context is executing */
+	dmb
+	mov	r3, #1
+	strb	r3, [r1, #ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET]
+#endif
+
 #ifdef ARM_MULTILIB_HAS_THREAD_ID_REGISTER
 	ldr	r3, [r1, #ARM_CONTEXT_CONTROL_THREAD_ID_OFFSET]
 	mcr	p15, 0, r3, c13, c0, 3
diff --git a/cpukit/score/cpu/arm/rtems/score/cpu.h b/cpukit/score/cpu/arm/rtems/score/cpu.h
index cb9dc7c409..dc57a78a67 100644
--- a/cpukit/score/cpu/arm/rtems/score/cpu.h
+++ b/cpukit/score/cpu/arm/rtems/score/cpu.h
@@ -216,6 +216,14 @@
   #define ARM_CONTEXT_CONTROL_D8_OFFSET 48
 #endif
 
+#ifdef RTEMS_SMP
+  #ifdef ARM_MULTILIB_VFP_D32
+    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 112
+  #else
+    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 48
+  #endif
+#endif
+
 #define ARM_EXCEPTION_FRAME_SIZE 76
 
 #define ARM_EXCEPTION_FRAME_REGISTER_SP_OFFSET 52
@@ -280,6 +288,9 @@ typedef struct {
   uint64_t register_d14;
   uint64_t register_d15;
 #endif
+#ifdef RTEMS_SMP
+  volatile bool is_executing;
+#endif
 } Context_Control;
 
 typedef struct {
@@ -410,6 +421,11 @@ void _CPU_Context_Initialize(
 #define _CPU_Context_Get_SP( _context ) \
   (_context)->register_sp
 
+#ifdef RTEMS_SMP
+  #define _CPU_Context_Get_is_executing( _context ) \
+    (_context)->is_executing
+#endif
+
 #define _CPU_Context_Restart_self( _the_context ) \
    _CPU_Context_restore( (_the_context) );