diff --git a/arch/arm/core/thread.c b/arch/arm/core/thread.c index 00dae607425..6e32868c364 100644 --- a/arch/arm/core/thread.c +++ b/arch/arm/core/thread.c @@ -379,3 +379,95 @@ int z_arch_float_disable(struct k_thread *thread) return 0; } #endif /* CONFIG_FLOAT && CONFIG_FP_SHARING */ + +void z_arch_switch_to_main_thread(struct k_thread *main_thread, + k_thread_stack_t *main_stack, + size_t main_stack_size, + k_thread_entry_t _main) +{ +#if defined(CONFIG_FLOAT) + /* Initialize the Floating Point Status and Control Register when in + * Unshared FP Registers mode (In Shared FP Registers mode, FPSCR is + * initialized at thread creation for threads that make use of the FP). + */ + __set_FPSCR(0); +#if defined(CONFIG_FP_SHARING) + /* In Sharing mode clearing FPSCR may set the CONTROL.FPCA flag. */ + __set_CONTROL(__get_CONTROL() & (~(CONTROL_FPCA_Msk))); + __ISB(); +#endif /* CONFIG_FP_SHARING */ +#endif /* CONFIG_FLOAT */ + +#ifdef CONFIG_ARM_MPU + /* Configure static memory map. This will program MPU regions, + * to set up access permissions for fixed memory sections, such + * as Application Memory or No-Cacheable SRAM area. + * + * This function is invoked once, upon system initialization. + */ + z_arm_configure_static_mpu_regions(); +#endif + + /* get high address of the stack, i.e. its start (stack grows down) */ + char *start_of_main_stack; + + start_of_main_stack = + Z_THREAD_STACK_BUFFER(main_stack) + main_stack_size; + + start_of_main_stack = (char *)STACK_ROUND_DOWN(start_of_main_stack); + + _current = main_thread; +#ifdef CONFIG_TRACING + sys_trace_thread_switched_in(); +#endif + + /* the ready queue cache already contains the main thread */ + +#ifdef CONFIG_ARM_MPU + /* + * If stack protection is enabled, make sure to set it + * before jumping to thread entry function + */ + z_arm_configure_dynamic_mpu_regions(main_thread); +#endif + +#if defined(CONFIG_BUILTIN_STACK_GUARD) + /* Set PSPLIM register for built-in stack guarding of main thread. */ +#if defined(CONFIG_CPU_CORTEX_M_HAS_SPLIM) + __set_PSPLIM((u32_t)main_stack); +#else +#error "Built-in PSP limit checks not supported by HW" +#endif +#endif /* CONFIG_BUILTIN_STACK_GUARD */ + + /* + * Set PSP to the highest address of the main stack + * before enabling interrupts and jumping to main. + */ + __asm__ volatile ( + "mov r0, %0\n\t" /* Store _main in R0 */ +#if defined(CONFIG_CPU_CORTEX_M) + "msr PSP, %1\n\t" /* __set_PSP(start_of_main_stack) */ +#endif + +#if defined(CONFIG_ARMV6_M_ARMV8_M_BASELINE) \ + || defined(CONFIG_ARMV7_R) + "cpsie i\n\t" /* __enable_irq() */ +#elif defined(CONFIG_ARMV7_M_ARMV8_M_MAINLINE) + "cpsie if\n\t" /* __enable_irq(); __enable_fault_irq() */ + "mov r1, #0\n\t" + "msr BASEPRI, r1\n\t" /* __set_BASEPRI(0) */ +#else +#error Unknown ARM architecture +#endif /* CONFIG_ARMV6_M_ARMV8_M_BASELINE */ + "isb\n\t" + "movs r1, #0\n\t" + "movs r2, #0\n\t" + "movs r3, #0\n\t" + "bl z_thread_entry\n\t" /* z_thread_entry(_main, 0, 0, 0); */ + : + : "r" (_main), "r" (start_of_main_stack) + ); + + CODE_UNREACHABLE; +} diff --git a/arch/arm/include/kernel_arch_func.h b/arch/arm/include/kernel_arch_func.h index e25980ef34a..0a35f4e5186 100644 --- a/arch/arm/include/kernel_arch_func.h +++ b/arch/arm/include/kernel_arch_func.h @@ -43,98 +43,6 @@ static ALWAYS_INLINE void z_arch_kernel_init(void) z_arm_clear_faults(); } -static ALWAYS_INLINE void -z_arch_switch_to_main_thread(struct k_thread *main_thread, - k_thread_stack_t *main_stack, - size_t main_stack_size, k_thread_entry_t _main) -{ -#if defined(CONFIG_FLOAT) - /* Initialize the Floating Point Status and Control Register when in - * Unshared FP Registers mode (In Shared FP Registers mode, FPSCR is - * initialized at thread creation for threads that make use of the FP). - */ - __set_FPSCR(0); -#if defined(CONFIG_FP_SHARING) - /* In Sharing mode clearing FPSCR may set the CONTROL.FPCA flag. */ - __set_CONTROL(__get_CONTROL() & (~(CONTROL_FPCA_Msk))); - __ISB(); -#endif /* CONFIG_FP_SHARING */ -#endif /* CONFIG_FLOAT */ - -#ifdef CONFIG_ARM_MPU - /* Configure static memory map. This will program MPU regions, - * to set up access permissions for fixed memory sections, such - * as Application Memory or No-Cacheable SRAM area. - * - * This function is invoked once, upon system initialization. - */ - z_arm_configure_static_mpu_regions(); -#endif - - /* get high address of the stack, i.e. its start (stack grows down) */ - char *start_of_main_stack; - - start_of_main_stack = - Z_THREAD_STACK_BUFFER(main_stack) + main_stack_size; - - start_of_main_stack = (char *)STACK_ROUND_DOWN(start_of_main_stack); - - _current = main_thread; -#ifdef CONFIG_TRACING - sys_trace_thread_switched_in(); -#endif - - /* the ready queue cache already contains the main thread */ - -#ifdef CONFIG_ARM_MPU - /* - * If stack protection is enabled, make sure to set it - * before jumping to thread entry function - */ - z_arm_configure_dynamic_mpu_regions(main_thread); -#endif - -#if defined(CONFIG_BUILTIN_STACK_GUARD) - /* Set PSPLIM register for built-in stack guarding of main thread. */ -#if defined(CONFIG_CPU_CORTEX_M_HAS_SPLIM) - __set_PSPLIM((u32_t)main_stack); -#else -#error "Built-in PSP limit checks not supported by HW" -#endif -#endif /* CONFIG_BUILTIN_STACK_GUARD */ - - /* - * Set PSP to the highest address of the main stack - * before enabling interrupts and jumping to main. - */ - __asm__ volatile ( - "mov r0, %0 \n\t" /* Store _main in R0 */ -#if defined(CONFIG_CPU_CORTEX_M) - "msr PSP, %1 \n\t" /* __set_PSP(start_of_main_stack) */ -#endif - -#if defined(CONFIG_ARMV6_M_ARMV8_M_BASELINE) \ - || defined(CONFIG_ARMV7_R) - "cpsie i \n\t" /* __enable_irq() */ -#elif defined(CONFIG_ARMV7_M_ARMV8_M_MAINLINE) - "cpsie if \n\t" /* __enable_irq(); __enable_fault_irq() */ - "mov r1, #0 \n\t" - "msr BASEPRI, r1 \n\t" /* __set_BASEPRI(0) */ -#else -#error Unknown ARM architecture -#endif /* CONFIG_ARMV6_M_ARMV8_M_BASELINE */ - "isb \n\t" - "movs r1, #0 \n\t" - "movs r2, #0 \n\t" - "movs r3, #0 \n\t" - "bl z_thread_entry \n\t" /* z_thread_entry(_main, 0, 0, 0); */ - : - : "r" (_main), "r" (start_of_main_stack) - ); - - CODE_UNREACHABLE; -} - static ALWAYS_INLINE void z_arch_thread_return_value_set(struct k_thread *thread, unsigned int value) { @@ -150,6 +58,11 @@ extern FUNC_NORETURN void z_arm_userspace_enter(k_thread_entry_t user_entry, extern void z_arm_fatal_error(unsigned int reason, const z_arch_esf_t *esf); +extern void z_arch_switch_to_main_thread(struct k_thread *main_thread, + k_thread_stack_t *main_stack, + size_t main_stack_size, + k_thread_entry_t _main); + #endif /* _ASMLANGUAGE */ #ifdef __cplusplus