SMP: Fix start multitasking for some targets

The previous SMP multitasking start assumed that the initial heir thread of a
processor starts execution in _Thread_Handler().  The _Thread_Handler() sets
the interrupt state explicitly by _ISR_Set_level() before it calls the thread
entry.  Under certain timing conditions, processors may perform an initial
context switch to a thread which already executes its thread body (see
smptests/smpstart01).  In this case, interrupts are disabled after the context
switch on targets which do not save/restore the interrupt state during a
context switch (aarch64, arm, and riscv).

Close #4627.

6 files changed, 56 insertions, 0 deletions

diff --git a/cpukit/score/cpu/aarch64/cpu_asm.S b/cpukit/score/cpu/aarch64/cpu_asm.S
index 2379698336..6321acde90 100644
--- a/cpukit/score/cpu/aarch64/cpu_asm.S
+++ b/cpukit/score/cpu/aarch64/cpu_asm.S
@@ -203,4 +203,21 @@ DEFINE_FUNCTION_AARCH64(_CPU_Context_restore)
 #endif
 
 	b	.L_check_is_executing
+
+DEFINE_FUNCTION_AARCH64(_AArch64_Start_multitasking)
+#ifdef AARCH64_MULTILIB_ARCH_V8_ILP32
+	/* Sanitize input for ILP32 ABI */
+	mov	w0, w0
+#endif
+
+	mov	x1, x0
+	GET_SELF_CPU_CONTROL	reg_2
+
+	/* Switch the stack to the temporary interrupt stack of this processor */
+	add	sp, x2, #(PER_CPU_INTERRUPT_FRAME_AREA + CPU_INTERRUPT_FRAME_SIZE)
+
+	/* Enable interrupts */
+	msr	DAIFClr, #0x2
+
+	b	.L_check_is_executing
 #endif
diff --git a/cpukit/score/cpu/aarch64/include/rtems/score/cpu.h b/cpukit/score/cpu/aarch64/include/rtems/score/cpu.h
index e1d9f0a5c2..fdc0e3d929 100644
--- a/cpukit/score/cpu/aarch64/include/rtems/score/cpu.h
+++ b/cpukit/score/cpu/aarch64/include/rtems/score/cpu.h
@@ -285,6 +285,10 @@ void _CPU_Context_Initialize(
   {
     context->is_executing = is_executing;
   }
+
+  RTEMS_NO_RETURN void _AArch64_Start_multitasking( Context_Control *heir );
+
+  #define _CPU_Start_multitasking( _heir ) _AArch64_Start_multitasking( _heir )
 #endif
 
 #define _CPU_Context_Restart_self( _the_context ) \
diff --git a/cpukit/score/cpu/arm/cpu_asm.S b/cpukit/score/cpu/arm/cpu_asm.S
index 54d6f70b0d..d52a43f70d 100644
--- a/cpukit/score/cpu/arm/cpu_asm.S
+++ b/cpukit/score/cpu/arm/cpu_asm.S
@@ -182,6 +182,18 @@ DEFINE_FUNCTION_ARM(_CPU_Context_restore)
 	str	r5, [r2, #PER_CPU_OFFSET_EXECUTING]
 
 	b	.L_check_is_executing
+
+DEFINE_FUNCTION_ARM(_ARM_Start_multitasking)
+	mov	r1, r0
+	GET_SELF_CPU_CONTROL	r2
+
+	/* Switch the stack to the temporary interrupt stack of this processor */
+	add	sp, r2, #(PER_CPU_INTERRUPT_FRAME_AREA + CPU_INTERRUPT_FRAME_SIZE)
+
+	/* Enable IRQ interrupts */
+	cpsie	i
+
+	b	.L_check_is_executing
 #endif
 
 #endif /* ARM_MULTILIB_ARCH_V4 */
diff --git a/cpukit/score/cpu/arm/include/rtems/score/cpu.h b/cpukit/score/cpu/arm/include/rtems/score/cpu.h
index da521528a4..7ac180ac26 100644
--- a/cpukit/score/cpu/arm/include/rtems/score/cpu.h
+++ b/cpukit/score/cpu/arm/include/rtems/score/cpu.h
@@ -443,6 +443,10 @@ void _CPU_Context_Initialize(
   {
     context->is_executing = is_executing;
   }
+
+  RTEMS_NO_RETURN void _ARM_Start_multitasking( Context_Control *heir );
+
+  #define _CPU_Start_multitasking( _heir ) _ARM_Start_multitasking( _heir )
 #endif
 
 #define _CPU_Context_Restart_self( _the_context ) \
diff --git a/cpukit/score/cpu/riscv/include/rtems/score/cpu.h b/cpukit/score/cpu/riscv/include/rtems/score/cpu.h
index 05ef2709ba..471c6c6c3e 100644
--- a/cpukit/score/cpu/riscv/include/rtems/score/cpu.h
+++ b/cpukit/score/cpu/riscv/include/rtems/score/cpu.h
@@ -509,6 +509,10 @@ static inline void _CPU_Context_Set_is_executing(
   context->is_executing = is_executing;
 }
 
+RTEMS_NO_RETURN void _RISCV_Start_multitasking( Context_Control *heir );
+
+#define _CPU_Start_multitasking( _heir ) _RISCV_Start_multitasking( _heir )
+
 #endif /* RTEMS_SMP */
 
 /** Type that can store a 32-bit integer or a pointer. */
diff --git a/cpukit/score/cpu/riscv/riscv-context-switch.S b/cpukit/score/cpu/riscv/riscv-context-switch.S
index 3c0368886e..cfaff444b0 100644
--- a/cpukit/score/cpu/riscv/riscv-context-switch.S
+++ b/cpukit/score/cpu/riscv/riscv-context-switch.S
@@ -40,6 +40,9 @@
 PUBLIC(_CPU_Context_switch)
 PUBLIC(_CPU_Context_switch_no_return)
 PUBLIC(_CPU_Context_restore)
+#ifdef RTEMS_SMP
+PUBLIC(_RISCV_Start_multitasking)
+#endif
 
 SYM(_CPU_Context_switch):
 SYM(_CPU_Context_switch_no_return):
@@ -175,4 +178,16 @@ SYM(_CPU_Context_restore):
 	sw	a5, PER_CPU_OFFSET_EXECUTING(a2)
 
 	j	.Ltry_update_is_executing
+
+SYM(_RISCV_Start_multitasking):
+	mv	a1, a0
+	GET_SELF_CPU_CONTROL	a2
+
+	/* Switch the stack to the temporary interrupt stack of this processor */
+	addi	sp, a2, PER_CPU_INTERRUPT_FRAME_AREA + CPU_INTERRUPT_FRAME_SIZE
+
+	/* Enable interrupts */
+	csrrs	zero, mstatus, RISCV_MSTATUS_MIE
+
+	j	.Ltry_update_is_executing
 #endif