arch: arm: cortex_a_r: introduce use_switch

placeholder

Signed-off-by: Huifeng Zhang <Huifeng.Zhang@arm.com>

arch/arm/core/Kconfig

@@ -37,6 +37,8 @@ config CPU_AARCH32_CORTEX_R
 	select ARCH_HAS_EXTRA_EXCEPTION_INFO
 	select ARCH_HAS_CODE_DATA_RELOCATION
 	select ARCH_HAS_NOCACHE_MEMORY_SUPPORT if ARM_MPU && CPU_HAS_ARM_MPU && CPU_HAS_DCACHE
+	select USE_SWITCH
+	select USE_SWITCH_SUPPORTED
 	help
 	  This option signifies the use of a CPU of the Cortex-R family.
 
@@ -54,6 +56,8 @@ config CPU_AARCH32_CORTEX_A
 	select HAS_FLASH_LOAD_OFFSET
 	select ARCH_HAS_EXTRA_EXCEPTION_INFO
 	select ARCH_HAS_NOCACHE_MEMORY_SUPPORT
+	select USE_SWITCH
+	select USE_SWITCH_SUPPORTED
 	help
 	  This option signifies the use of a CPU of the Cortex-A family.
 

arch/arm/core/cortex_a_r/exc_exit.S

@@ -23,7 +23,11 @@ _ASM_FILE_PROLOGUE
 
 GTEXT(z_arm_exc_exit)
 GTEXT(z_arm_int_exit)
+#ifdef CONFIG_USE_SWITCH
+GTEXT(z_arm_context_switch)
+#else
 GTEXT(z_arm_do_swap)
+#endif
 GDATA(_kernel)
 
 .macro userspace_exc_exit
@@ -127,6 +131,77 @@ vfp_exit\@:
  * 	z_arm_int_exit();
  * }
  */
+#ifdef CONFIG_USE_SWITCH
+
+SECTION_SUBSEC_FUNC(TEXT, _HandlerModeExit, z_arm_int_exit)
+
+#ifdef CONFIG_STACK_SENTINEL
+	bl z_check_stack_sentinel
+#endif /* CONFIG_STACK_SENTINEL */
+
+	/* Disable nested interrupts while exiting, this should happens
+	 * before context switch also, to ensure interrupts are disabled.
+	 */
+	cpsid i
+
+	/* Decrement interrupt nesting count */
+	ldr r2, =_kernel
+	ldr r0, [r2, #_kernel_offset_to_nested]
+	sub r0, r0, #1
+	str r0, [r2, #_kernel_offset_to_nested]
+
+	/* Restore previous stack pointer */
+	pop {r2, r3}
+	add sp, sp, r3
+	/*
+	 * Restore lr_svc stored into the SVC mode stack by the mode entry
+	 * function. This ensures that the return address of the interrupted
+	 * context is preserved in case of interrupt nesting.
+	 */
+	pop {lr}
+
+	/*
+	 * Restore r0-r3, r12 and lr_irq stored into the process stack by the
+	 * mode entry function. These registers are saved by _isr_wrapper for
+	 * IRQ mode and z_arm_svc for SVC mode.
+	 *
+	 * r0-r3 are either the values from the thread before it was switched
+	 * out or they are the args to _new_thread for a new thread.
+	 */
+	cps #MODE_SYS
+
+#ifdef CONFIG_PREEMPT_ENABLED
+	/* Do not context switch if exiting a nested interrupt */
+	ldr r3, =_kernel
+	ldr r0, [r3, #_kernel_offset_to_nested]
+	cmp r0, #1
+	bhi __EXIT_INT
+
+	/* retrieve pointer to the current thread */
+	ldr r1, [r3, #_kernel_offset_to_current]
+	push {r1}
+	mov r0, #0
+	bl z_get_next_switch_handle
+
+	pop {r1}
+	cmp r0, #0
+	beq __EXIT_INT
+
+	/*
+	 * Switch thread
+	 * r0: new thread
+	 * r1: old thread
+	 */
+	bl z_arm_context_switch
+__EXIT_INT:
+#endif /* CONFIG_PREEMPT_ENABLED */
+
+	pop {r0-r3, r12, lr}
+	userspace_exc_exit
+	rfeia sp!
+
+#else
+
 SECTION_SUBSEC_FUNC(TEXT, _HandlerModeExit, z_arm_int_exit)
 
 #ifdef CONFIG_STACK_SENTINEL
@@ -187,6 +262,8 @@ __EXIT_INT:
 	userspace_exc_exit
 	rfeia sp!
 
+#endif
+
 /**
  * @brief Kernel housekeeping when exiting exception handler
  *
@@ -205,6 +282,39 @@ __EXIT_INT:
  *
  * @param fatal True if exiting from a fatal fault; otherwise, false
  */
+#ifdef CONFIG_USE_SWITCH
+
+SECTION_SUBSEC_FUNC(TEXT, _HandlerModeExit, z_arm_exc_exit)
+	/* Do not context switch if exiting a nested exception */
+	ldr r3, =_kernel
+	ldr r1, [r3, #_kernel_offset_to_nested]
+	cmp r1, #1
+	bhi __EXIT_EXC
+
+	/* If the fault is not fatal, return to the current thread context */
+	cmp r0, #0
+	beq __EXIT_EXC
+
+	pop {r0-r3, r12, lr}
+	userspace_exc_exit
+	rfeia sp!
+
+__EXIT_EXC:
+	/* Decrement exception nesting count */
+	ldr r0, [r3, #_kernel_offset_to_nested]
+	sub r0, r0, #1
+	str r0, [r3, #_kernel_offset_to_nested]
+
+	/*
+	 * Restore r0-r3, r12, lr, lr_und and spsr_und from the exception stack
+	 * and return to the current thread.
+	 */
+	ldmia sp, {r0-r3, r12, lr}^
+	add sp, #24
+	rfeia sp!
+
+#else
+
 SECTION_SUBSEC_FUNC(TEXT, _HandlerModeExit, z_arm_exc_exit)
 	/* Do not context switch if exiting a nested exception */
 	ldr r3, =_kernel
@@ -269,3 +379,5 @@ __EXIT_EXC:
 	ldmia sp, {r0-r3, r12, lr}^
 	add sp, #24
 	rfeia sp!
+
+#endif

arch/arm/core/cortex_a_r/swap_helper.S

@@ -24,14 +24,71 @@
 _ASM_FILE_PROLOGUE
 
 GTEXT(z_arm_svc)
+#ifdef CONFIG_USE_SWITCH
+GTEXT(z_arm_context_switch)
+#else
 GTEXT(z_arm_do_swap)
+#endif /* CONFIG_USE_SWITCH */
 GTEXT(z_do_kernel_oops)
 #if defined(CONFIG_USERSPACE)
 GTEXT(z_arm_do_syscall)
 #endif
 
 GDATA(_kernel)
 
+#ifdef CONFIG_USE_SWITCH
+/*
+ * Routine to handle context switches
+ *
+ * This function is directly called either by _isr_wrapper() in case of
+ * preemption, or arch_switch() in case of cooperative switching.
+ *
+ * void z_arm_context_switch(struct k_thread *new, struct k_thread *old);
+ */
+SECTION_FUNC(TEXT, z_arm_context_switch)
+
+	ldr r2, =_thread_offset_to_callee_saved
+	add r2, r1, r2
+
+	stm r2, {r4-r11, sp, lr}
+
+	/* save old thread into switch handle which is required by
+	 * z_sched_switch_spin()
+	 */
+	str r1, [r1, #___thread_t_switch_handle_OFFSET]
+
+#if defined(CONFIG_THREAD_LOCAL_STORAGE)
+	/* Grab the TLS pointer */
+	ldr r3, [r0, #_thread_offset_to_tls]
+
+	/* Store TLS pointer in the "Process ID" register.
+	 * This register is used as a base pointer to all
+	 * thread variables with offsets added by toolchain.
+	 */
+	mcr 15, 0, r3, cr13, cr0, 3
+#endif
+
+	ldr r2, =_thread_offset_to_callee_saved
+	add r2, r0, r2
+	ldm r2, {r4-r11, sp, lr}
+
+#if defined (CONFIG_ARM_MPU)
+	/* Re-program dynamic memory map */
+	push {r0, lr}
+	bl z_arm_configure_dynamic_mpu_regions
+	pop {r0, lr}
+#endif
+
+#ifdef CONFIG_INSTRUMENT_THREAD_SWITCHING
+	push {lr}
+	bl z_thread_mark_switched_in
+	pop {lr}
+#endif
+
+	bx lr
+
+#else
+
 /**
  *
  * @brief Routine to handle context switches
@@ -192,6 +249,8 @@ in_fp_inactive:
      */
     bx lr
 
+#endif /* CONFIG_USE_SWITCH */
+
 #if defined(CONFIG_FPU_SHARING)
 #define FPU_SF_SIZE ___fpu_t_SIZEOF
 #else
@@ -331,8 +390,13 @@ demux:
 #endif
 
 _context_switch:
+#ifdef CONFIG_USE_SWITCH
+    /* ; TODO: Needed more investigation; */
+    bl z_arm_context_switch
+#else
     /* handler mode exit, to PendSV */
     bl z_arm_do_swap
+#endif
 
     b z_arm_int_exit
 

arch/arm/core/cortex_a_r/thread.c

@@ -126,6 +126,11 @@ void arch_new_thread(struct k_thread *thread, k_thread_stack_t *stack,
 	 * initial values in all other registers/thread entries are
 	 * irrelevant.
 	 */
+#if defined(CONFIG_USE_SWITCH)
+	extern void z_arm_exit_exc(void);
+	thread->switch_handle = thread;
+	thread->callee_saved.lr = (uint32_t)z_arm_exc_exit;
+#endif
 }
 
 #if defined(CONFIG_MPU_STACK_GUARD) && defined(CONFIG_FPU) \

arch/arm/include/cortex_a_r/kernel_arch_func.h

@@ -50,12 +50,30 @@ static ALWAYS_INLINE void arch_kernel_init(void)
 #endif /* CONFIG_ARM_AARCH32_MMU */
 }
 
+#ifndef CONFIG_USE_SWITCH
+
 static ALWAYS_INLINE void
 arch_thread_return_value_set(struct k_thread *thread, unsigned int value)
 {
 	thread->arch.swap_return_value = value;
 }
 
+#else
+
+static inline void arch_switch(void *switch_to, void **switched_from)
+{
+	extern void z_arm_context_switch(struct k_thread *new,
+					struct k_thread *old);
+
+	struct k_thread *new = switch_to;
+	struct k_thread *old = CONTAINER_OF(switched_from, struct k_thread,
+					    switch_handle);
+
+	z_arm_context_switch(new, old);
+}
+
+#endif
+
 extern FUNC_NORETURN void z_arm_userspace_enter(k_thread_entry_t user_entry,
 					       void *p1, void *p2, void *p3,
 					       uint32_t stack_end,

include/zephyr/arch/arm/thread.h

@@ -32,6 +32,9 @@ struct _callee_saved {
 	uint32_t v7;  /* r10 */
 	uint32_t v8;  /* r11 */
 	uint32_t psp; /* r13 */
+#ifdef CONFIG_USE_SWITCH
+	uint32_t lr;  /* lr */
+#endif
 };
 
 typedef struct _callee_saved _callee_saved_t;