arch: arm: switching stack pointer with assembly code

With -O0 optimizion, gcc compiler doesn't inline "static inline"
marked function. So when function call return from function
set_and_switch_to_psp which is to switch sp from MSP to PSP, the
ending "mov sp, r7" instruction will overwrite the just updated
sp value(PSP) with the beginning stack pointer(should be MSP)
stored in r7 register, so the switch doesn't happen. And it causes
unpredictable problems in the initialization process, the backward
analysis for this problem can be found on Github issue #15794.

Fixes: #15794.

Signed-off-by: Wentong Wu <[email protected]>

Author: wentongwu

arch/arm/core/cortex_m/prep_c.c

@@ -22,7 +22,6 @@
 #include <linker/linker-defs.h>
 #include <kernel_internal.h>
 #include <arch/arm/cortex_m/cmsis.h>
-#include <cortex_m/stack.h>
 
 #if defined(__GNUC__)
 /*
@@ -37,44 +36,6 @@
 
 #include <string.h>
 
-static inline void switch_sp_to_psp(void)
-{
-	__set_CONTROL(__get_CONTROL() | CONTROL_SPSEL_Msk);
-	/*
-	 * When changing the stack pointer, software must use an ISB instruction
-	 * immediately after the MSR instruction. This ensures that instructions
-	 * after the ISB instruction execute using the new stack pointer.
-	 */
-	__ISB();
-}
-
-static inline void set_and_switch_to_psp(void)
-{
-	u32_t process_sp;
-
-	process_sp = (u32_t)&_interrupt_stack + CONFIG_ISR_STACK_SIZE;
-	__set_PSP(process_sp);
-	switch_sp_to_psp();
-}
-
-void lock_interrupts(void)
-{
-#if defined(CONFIG_ARMV6_M_ARMV8_M_BASELINE)
-	__disable_irq();
-#elif defined(CONFIG_ARMV7_M_ARMV8_M_MAINLINE)
-	__set_BASEPRI(_EXC_IRQ_DEFAULT_PRIO);
-#else
-#error Unknown ARM architecture
-#endif /* CONFIG_ARMV6_M_ARMV8_M_BASELINE */
-}
-
-#ifdef CONFIG_INIT_STACKS
-static inline void init_stacks(void)
-{
-	memset(&_interrupt_stack, 0xAA, CONFIG_ISR_STACK_SIZE);
-}
-#endif
-
 #ifdef CONFIG_CPU_CORTEX_M_HAS_VTOR
 
 #ifdef CONFIG_XIP
@@ -191,14 +152,6 @@ extern void z_IntLibInit(void);
 #endif
 void _PrepC(void)
 {
-#ifdef CONFIG_INIT_STACKS
-	init_stacks();
-#endif
-	/*
-	 * Set PSP and use it to boot without using MSP, so that it
-	 * gets set to _interrupt_stack during initialization.
-	 */
-	set_and_switch_to_psp();
 	relocate_vector_table();
 	enable_floating_point();
 	z_bss_zero();

arch/arm/core/cortex_m/reset.S

@@ -19,6 +19,8 @@
 _ASM_FILE_PROLOGUE
 
 GTEXT(__reset)
+GTEXT(memset)
+GDATA(_interrupt_stack)
 #if defined(CONFIG_PLATFORM_SPECIFIC_INIT)
 GTEXT(_PlatformInit)
 #endif
@@ -61,12 +63,46 @@ SECTION_SUBSEC_FUNC(TEXT,_reset_section,__start)
 #endif
 
     /* lock interrupts: will get unlocked when switch to main task */
-    bl	lock_interrupts
+#if defined(CONFIG_ARMV6_M_ARMV8_M_BASELINE)
+    cpsid i
+#elif defined(CONFIG_ARMV7_M_ARMV8_M_MAINLINE)
+    movs.n r0, #_EXC_IRQ_DEFAULT_PRIO
+    msr BASEPRI, r0
+#else
+#error Unknown ARM architecture
+#endif
+
 #ifdef CONFIG_WDOG_INIT
     /* board-specific watchdog initialization is necessary */
     bl _WdogInit
 #endif
 
+#ifdef CONFIG_INIT_STACKS
+    ldr r0, =_interrupt_stack
+    ldr r1, =0xaa
+    ldr r2, =CONFIG_ISR_STACK_SIZE
+    bl memset
+#endif
+
+    /*
+     * Set PSP and use it to boot without using MSP, so that it
+     * gets set to _interrupt_stack during initialization.
+     */
+    ldr r0, =_interrupt_stack
+    ldr r1, =CONFIG_ISR_STACK_SIZE
+    adds r0, r0, r1
+    msr PSP, r0
+    mrs r0, CONTROL
+    movs r1, #2
+    orrs r0, r1 /* CONTROL_SPSEL_Msk */
+    msr CONTROL, r0
+    /*
+     * When changing the stack pointer, software must use an ISB instruction
+     * immediately after the MSR instruction. This ensures that instructions
+     * after the ISB instruction execute using the new stack pointer.
+     */
+    isb
+
     /*
      * 'bl' jumps the furthest of the branch instructions that are
      * supported on all platforms. So it is used when jumping to _PrepC