From 87dd43766d8fcbb9a519f5753911326adeae3f60 Mon Sep 17 00:00:00 2001 From: Huifeng Zhang Date: Wed, 2 Aug 2023 14:30:41 +0800 Subject: [PATCH] arch: arm: cortex_a_r: Hold curr_cpu instance to TPIDRURO Store the current CPU's struct _cpu instance into TPIDRURO, so that the CPU core can get its struct _cpu instance by reading TPIDRURO. This is useful in the SMP system. Signed-off-by: Huifeng Zhang --- arch/arm/core/cortex_a_r/exc.S | 13 +++--- arch/arm/core/cortex_a_r/exc_exit.S | 40 ++++++++-------- arch/arm/core/cortex_a_r/fault.c | 10 ++-- arch/arm/core/cortex_a_r/isr_wrapper.S | 17 +++---- arch/arm/core/cortex_a_r/macro_priv.inc | 19 ++++++++ arch/arm/core/cortex_a_r/prep_c.c | 3 ++ arch/arm/core/cortex_a_r/swap_helper.S | 46 +++++++++---------- arch/arm/include/cortex_a_r/exc.h | 4 +- include/zephyr/arch/arm/arch_inlines.h | 6 +-- .../zephyr/arch/arm/cortex_a_r/lib_helpers.h | 1 + include/zephyr/arch/arm/cortex_a_r/tpidruro.h | 20 ++++++++ 11 files changed, 112 insertions(+), 67 deletions(-) create mode 100644 arch/arm/core/cortex_a_r/macro_priv.inc create mode 100644 include/zephyr/arch/arm/cortex_a_r/tpidruro.h diff --git a/arch/arm/core/cortex_a_r/exc.S b/arch/arm/core/cortex_a_r/exc.S index 87742b3980..1bff0d4bcf 100644 --- a/arch/arm/core/cortex_a_r/exc.S +++ b/arch/arm/core/cortex_a_r/exc.S @@ -27,6 +27,7 @@ #include #include #include +#include "macro_priv.inc" _ASM_FILE_PROLOGUE @@ -86,10 +87,10 @@ GTEXT(z_arm_data_abort) #endif /* Increment exception nesting count */ - ldr r2, =_kernel - ldr r1, [r2, #_kernel_offset_to_nested] + get_cpu r2 + ldr r1, [r2, #___cpu_t_nested_OFFSET] add r1, r1, #1 - str r1, [r2, #_kernel_offset_to_nested] + str r1, [r2, #___cpu_t_nested_OFFSET] .endm .macro exception_exit @@ -128,10 +129,10 @@ SECTION_SUBSEC_FUNC(TEXT, __exc, z_arm_undef_instruction) sub sp, #24 /* Increment exception nesting count */ - ldr r2, =_kernel - ldr r1, [r2, #_kernel_offset_to_nested] + get_cpu r2 + ldr r1, [r2, #___cpu_t_nested_OFFSET] add r1, r1, #1 - str r1, [r2, #_kernel_offset_to_nested] + str r1, [r2, #___cpu_t_nested_OFFSET] #if defined(CONFIG_FPU_SHARING) sub sp, #___fpu_t_SIZEOF diff --git a/arch/arm/core/cortex_a_r/exc_exit.S b/arch/arm/core/cortex_a_r/exc_exit.S index 67fc5fa6e0..499fa17d30 100644 --- a/arch/arm/core/cortex_a_r/exc_exit.S +++ b/arch/arm/core/cortex_a_r/exc_exit.S @@ -18,6 +18,7 @@ #include #include #include +#include "macro_priv.inc" _ASM_FILE_PROLOGUE @@ -52,8 +53,8 @@ GDATA(_kernel) bne system_thread_exit\@ /* Restore user stack pointer */ - ldr r0, =_kernel - ldr r0, [r0, #_kernel_offset_to_current] + get_cpu r0 + ldr r0, [r0, #___cpu_t_current_OFFSET] cps #MODE_SYS ldr sp, [r0, #_thread_offset_to_sp_usr] /* sp_usr */ cps #MODE_SVC @@ -68,8 +69,8 @@ system_thread_exit\@: * If the floating point context pointer is null, then a context was * saved so restore the float context from the exception stack frame. */ - ldr r2, =_kernel - ldr r1, [r2, #_kernel_offset_to_fp_ctx] + get_cpu r2 + ldr r1, [r2, #___cpu_t_fp_ctx_OFFSET] cmp r1, #0 beq vfp_restore\@ @@ -79,7 +80,7 @@ system_thread_exit\@: */ cmp r0, #0 moveq r1, #0 - streq r1, [r2, #_kernel_offset_to_fp_ctx] + streq r1, [r2, #___cpu_t_fp_ctx_OFFSET] b vfp_exit\@ vfp_restore\@: @@ -140,23 +141,24 @@ SECTION_SUBSEC_FUNC(TEXT, _HandlerModeExit, z_arm_int_exit) #ifdef CONFIG_PREEMPT_ENABLED /* Do not context switch if exiting a nested interrupt */ - ldr r3, =_kernel - ldr r0, [r3, #_kernel_offset_to_nested] + get_cpu r3 + ldr r0, [r3, #___cpu_t_nested_OFFSET] cmp r0, #1 bhi __EXIT_INT - ldr r1, [r3, #_kernel_offset_to_current] - ldr r0, [r3, #_kernel_offset_to_ready_q_cache] + ldr r1, [r3, #___cpu_t_current_OFFSET] + ldr r2, =_kernel + ldr r0, [r2, #_kernel_offset_to_ready_q_cache] cmp r0, r1 blne z_arm_do_swap __EXIT_INT: #endif /* CONFIG_PREEMPT_ENABLED */ /* Decrement interrupt nesting count */ - ldr r2, =_kernel - ldr r0, [r2, #_kernel_offset_to_nested] + get_cpu r2 + ldr r0, [r2, #___cpu_t_nested_OFFSET] sub r0, r0, #1 - str r0, [r2, #_kernel_offset_to_nested] + str r0, [r2, #___cpu_t_nested_OFFSET] /* Restore previous stack pointer */ pop {r2, r3} @@ -207,8 +209,8 @@ __EXIT_INT: */ 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] + get_cpu r3 + ldr r1, [r3, #___cpu_t_nested_OFFSET] cmp r1, #1 bhi __EXIT_EXC @@ -239,10 +241,10 @@ SECTION_SUBSEC_FUNC(TEXT, _HandlerModeExit, z_arm_exc_exit) bl z_arm_do_swap /* Decrement exception nesting count */ - ldr r3, =_kernel - ldr r0, [r3, #_kernel_offset_to_nested] + get_cpu r3 + ldr r0, [r3, #___cpu_t_nested_OFFSET] sub r0, r0, #1 - str r0, [r3, #_kernel_offset_to_nested] + str r0, [r3, #___cpu_t_nested_OFFSET] /* Return to the switched thread */ cps #MODE_SYS @@ -255,9 +257,9 @@ SECTION_SUBSEC_FUNC(TEXT, _HandlerModeExit, z_arm_exc_exit) __EXIT_EXC: /* Decrement exception nesting count */ - ldr r0, [r3, #_kernel_offset_to_nested] + ldr r0, [r3, #___cpu_t_nested_OFFSET] sub r0, r0, #1 - str r0, [r3, #_kernel_offset_to_nested] + str r0, [r3, #___cpu_t_nested_OFFSET] #if defined(CONFIG_FPU_SHARING) add sp, sp, #___fpu_t_SIZEOF diff --git a/arch/arm/core/cortex_a_r/fault.c b/arch/arm/core/cortex_a_r/fault.c index 752fe5a95f..4b1624cd54 100644 --- a/arch/arm/core/cortex_a_r/fault.c +++ b/arch/arm/core/cortex_a_r/fault.c @@ -147,7 +147,7 @@ bool z_arm_fault_undef_instruction_fp(void) __set_FPEXC(FPEXC_EN); - if (_kernel.cpus[0].nested > 1) { + if (_current_cpu->nested > 1) { /* * If the nested count is greater than 1, the undefined * instruction exception came from an irq/svc context. (The @@ -155,12 +155,12 @@ bool z_arm_fault_undef_instruction_fp(void) * the undef exception would increment it to 2). */ struct __fpu_sf *spill_esf = - (struct __fpu_sf *)_kernel.cpus[0].fp_ctx; + (struct __fpu_sf *)_current_cpu->fp_ctx; if (spill_esf == NULL) return false; - _kernel.cpus[0].fp_ctx = NULL; + _current_cpu->fp_ctx = NULL; /* * If the nested count is 2 and the current thread has used the @@ -170,9 +170,9 @@ bool z_arm_fault_undef_instruction_fp(void) * saved exception stack frame, then save the floating point * context because it is about to be overwritten. */ - if (((_kernel.cpus[0].nested == 2) + if (((_current_cpu->nested == 2) && (_current->base.user_options & K_FP_REGS)) - || ((_kernel.cpus[0].nested > 2) + || ((_current_cpu->nested > 2) && (spill_esf->undefined & FPEXC_EN))) { /* * Spill VFP registers to specified exception stack diff --git a/arch/arm/core/cortex_a_r/isr_wrapper.S b/arch/arm/core/cortex_a_r/isr_wrapper.S index dc5fa9cac0..1504ba3835 100644 --- a/arch/arm/core/cortex_a_r/isr_wrapper.S +++ b/arch/arm/core/cortex_a_r/isr_wrapper.S @@ -22,6 +22,7 @@ #include #include #include +#include "macro_priv.inc" _ASM_FILE_PROLOGUE @@ -57,8 +58,8 @@ SECTION_FUNC(TEXT, _isr_wrapper) cmp r0, #MODE_USR bne isr_system_thread - ldr r0, =_kernel - ldr r0, [r0, #_kernel_offset_to_current] + get_cpu r0 + ldr r0, [r0, #___cpu_t_current_OFFSET] /* Save away user stack pointer */ cps #MODE_SYS @@ -108,10 +109,10 @@ _vfp_not_enabled: * Mark where to store the floating context for the undefined * instruction handler */ - ldr r2, =_kernel - ldr r0, [r2, #_kernel_offset_to_fp_ctx] + get_cpu r2 + ldr r0, [r2, #___cpu_t_fp_ctx_OFFSET] cmp r0, #0 - streq sp, [r2, #_kernel_offset_to_fp_ctx] + streq sp, [r2, #___cpu_t_fp_ctx_OFFSET] #endif /* CONFIG_FPU_SHARING */ /* @@ -139,10 +140,10 @@ _vfp_not_enabled: push {r2, r3} /* Increment interrupt nesting count */ - ldr r2, =_kernel - ldr r0, [r2, #_kernel_offset_to_nested] + get_cpu r2 + ldr r0, [r2, #___cpu_t_nested_OFFSET] add r0, r0, #1 - str r0, [r2, #_kernel_offset_to_nested] + str r0, [r2, #___cpu_t_nested_OFFSET] #ifdef CONFIG_TRACING_ISR bl sys_trace_isr_enter diff --git a/arch/arm/core/cortex_a_r/macro_priv.inc b/arch/arm/core/cortex_a_r/macro_priv.inc new file mode 100644 index 0000000000..dfd192a70f --- /dev/null +++ b/arch/arm/core/cortex_a_r/macro_priv.inc @@ -0,0 +1,19 @@ +/* + * Copyright (c) 2023 Arm Limited (or its affiliates). All rights reserved. + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef _MACRO_PRIV_INC_ +#define _MACRO_PRIV_INC_ + +#include + +.macro get_cpu rreg0 + /* + * Get CPU pointer. + */ + mrc p15, 0, \rreg0, c13, c0, 3 + and \rreg0, #TPIDRURO_CURR_CPU +.endm + +#endif /* _MACRO_PRIV_INC_ */ diff --git a/arch/arm/core/cortex_a_r/prep_c.c b/arch/arm/core/cortex_a_r/prep_c.c index cb5a6cc163..95dbb49cf1 100644 --- a/arch/arm/core/cortex_a_r/prep_c.c +++ b/arch/arm/core/cortex_a_r/prep_c.c @@ -147,6 +147,9 @@ extern FUNC_NORETURN void z_cstart(void); */ void z_arm_prep_c(void) { + /* Initialize tpidruro with our struct _cpu instance address */ + write_tpidruro((uintptr_t)&_kernel.cpus[0]); + relocate_vector_table(); #if defined(CONFIG_CPU_HAS_FPU) z_arm_floating_point_init(); diff --git a/arch/arm/core/cortex_a_r/swap_helper.S b/arch/arm/core/cortex_a_r/swap_helper.S index 0031148cae..548bb446aa 100644 --- a/arch/arm/core/cortex_a_r/swap_helper.S +++ b/arch/arm/core/cortex_a_r/swap_helper.S @@ -20,6 +20,7 @@ #include #include #include +#include "macro_priv.inc" _ASM_FILE_PROLOGUE @@ -49,9 +50,9 @@ SECTION_FUNC(TEXT, z_arm_do_swap) pop {r0, lr} #endif /* CONFIG_INSTRUMENT_THREAD_SWITCHING */ - /* load _kernel into r1 and current k_thread into r2 */ - ldr r1, =_kernel - ldr r2, [r1, #_kernel_offset_to_current] + /* load current _cpu into r1 and current k_thread into r2 */ + get_cpu r1 + ldr r2, [r1, #___cpu_t_current_OFFSET] #if defined(CONFIG_ARM_STORE_EXC_RETURN) /* Store LSB of LR (EXC_RETURN) to the thread's 'mode' word. */ @@ -81,7 +82,7 @@ SECTION_FUNC(TEXT, z_arm_do_swap) * float registers have not been saved away, so write them to the * exception stack frame. */ - ldr r0, [r1, #_kernel_offset_to_fp_ctx] + ldr r0, [r1, #___cpu_t_fp_ctx_OFFSET] cmp r0, #0 beq out_store_thread_context @@ -106,13 +107,14 @@ out_fp_inactive: * frame, so zero out the global pointer to note this. */ mov r0, #0 - str r0, [r1, #_kernel_offset_to_fp_ctx] + str r0, [r1, #___cpu_t_fp_ctx_OFFSET] #endif /* CONFIG_FPU_SHARING */ /* fetch the thread to run from the ready queue cache */ - ldr r2, [r1, #_kernel_offset_to_ready_q_cache] + ldr r3, =_kernel + ldr r2, [r3, #_kernel_offset_to_ready_q_cache] - str r2, [r1, #_kernel_offset_to_current] + str r2, [r1, #___cpu_t_current_OFFSET] #if defined(CONFIG_THREAD_LOCAL_STORAGE) /* Grab the TLS pointer */ @@ -139,11 +141,6 @@ out_fp_inactive: movs r3, #0 str r3, [r2, #_thread_offset_to_basepri] -_thread_irq_disabled: - /* load _kernel into r1 and current k_thread into r2 */ - ldr r1, =_kernel - ldr r2, [r1, #_kernel_offset_to_current] - /* addr of callee-saved regs in thread in r0 */ ldr r0, =_thread_offset_to_callee_saved add r0, r2 @@ -175,9 +172,9 @@ in_fp_inactive: /* r2 contains k_thread */ mov r0, r2 /* Re-program dynamic memory map */ - push {r2, lr} + push {r0, lr} bl z_arm_configure_dynamic_mpu_regions - pop {r2, lr} + pop {r0, lr} #endif #ifdef CONFIG_INSTRUMENT_THREAD_SWITCHING @@ -217,8 +214,8 @@ SECTION_FUNC(TEXT, z_arm_svc) cmp r0, #MODE_USR bne svc_system_thread - ldr r0, =_kernel - ldr r0, [r0, #_kernel_offset_to_current] + get_cpu r0 + ldr r0, [r0, #___cpu_t_current_OFFSET] /* Save away user stack pointer */ cps #MODE_SYS @@ -265,10 +262,10 @@ _vfp_not_enabled: * Mark where to store the floating context for the undefined * instruction handler */ - ldr r2, =_kernel - ldr r0, [r2, #_kernel_offset_to_fp_ctx] + get_cpu r2 + ldr r0, [r2, #___cpu_t_fp_ctx_OFFSET] cmp r0, #0 - streq sp, [r2, #_kernel_offset_to_fp_ctx] + streq sp, [r2, #___cpu_t_fp_ctx_OFFSET] #endif /* CONFIG_FPU_SHARING */ mov ip, sp @@ -282,15 +279,16 @@ _vfp_not_enabled: push {lr} /* Align stack at double-word boundary */ + /* TODO: Question, why push {r2, r3} here */ and r3, sp, #4 sub sp, sp, r3 push {r2, r3} /* Increment interrupt nesting count */ - ldr r2, =_kernel - ldr r0, [r2, #_kernel_offset_to_nested] + get_cpu r2 + ldr r0, [r2, #___cpu_t_nested_OFFSET] add r0, r0, #1 - str r0, [r2, #_kernel_offset_to_nested] + str r0, [r2, #___cpu_t_nested_OFFSET] /* Get SVC number */ mrs r0, spsr @@ -403,8 +401,8 @@ _do_syscall: ldr r6, =K_SYSCALL_BAD valid_syscall_id: - ldr r0, =_kernel - ldr r0, [r0, #_kernel_offset_to_current] + get_cpu r0 + ldr r0, [r0, #___cpu_t_current_OFFSET] ldr r1, [r0, #_thread_offset_to_mode] bic r1, #1 /* Store (privileged) mode in thread's mode state variable */ diff --git a/arch/arm/include/cortex_a_r/exc.h b/arch/arm/include/cortex_a_r/exc.h index b77febbc04..afeae80106 100644 --- a/arch/arm/include/cortex_a_r/exc.h +++ b/arch/arm/include/cortex_a_r/exc.h @@ -35,12 +35,12 @@ extern volatile irq_offload_routine_t offload_routine; /* Check the CPSR mode bits to see if we are in IRQ or FIQ mode */ static ALWAYS_INLINE bool arch_is_in_isr(void) { - return (_kernel.cpus[0].nested != 0U); + return (arch_curr_cpu()->nested != 0U); } static ALWAYS_INLINE bool arch_is_in_nested_exception(const z_arch_esf_t *esf) { - return (_kernel.cpus[0].nested > 1U) ? (true) : (false); + return (arch_curr_cpu()->nested > 1U) ? (true) : (false); } #if defined(CONFIG_USERSPACE) diff --git a/include/zephyr/arch/arm/arch_inlines.h b/include/zephyr/arch/arm/arch_inlines.h index 5d86858b05..96822e0eff 100644 --- a/include/zephyr/arch/arm/arch_inlines.h +++ b/include/zephyr/arch/arm/arch_inlines.h @@ -8,14 +8,14 @@ #define ZEPHYR_INCLUDE_ARCH_ARM_AARCH32_ARCH_INLINES_H #include +#include +#include -#ifndef CONFIG_SMP static ALWAYS_INLINE _cpu_t *arch_curr_cpu(void) { /* Dummy implementation always return the first cpu */ - return &_kernel.cpus[0]; + return (_cpu_t *)(read_tpidruro() & TPIDRURO_CURR_CPU); } -#endif static ALWAYS_INLINE uint32_t arch_proc_id(void) { diff --git a/include/zephyr/arch/arm/cortex_a_r/lib_helpers.h b/include/zephyr/arch/arm/cortex_a_r/lib_helpers.h index a4e74b0daa..209c4dd732 100644 --- a/include/zephyr/arch/arm/cortex_a_r/lib_helpers.h +++ b/include/zephyr/arch/arm/cortex_a_r/lib_helpers.h @@ -72,6 +72,7 @@ MAKE_REG_HELPER(mair0, 0, 10, 2, 0); MAKE_REG_HELPER(vbar, 0, 12, 0, 0); MAKE_REG_HELPER(cntv_ctl, 0, 14, 3, 1); MAKE_REG_HELPER(ctr, 0, 0, 0, 1); +MAKE_REG_HELPER(tpidruro, 0, 13, 0, 3); MAKE_REG64_HELPER(ICC_SGI1R, 0, 12); MAKE_REG64_HELPER(cntvct, 1, 14); MAKE_REG64_HELPER(cntv_cval, 3, 14); diff --git a/include/zephyr/arch/arm/cortex_a_r/tpidruro.h b/include/zephyr/arch/arm/cortex_a_r/tpidruro.h new file mode 100644 index 0000000000..25eb275af9 --- /dev/null +++ b/include/zephyr/arch/arm/cortex_a_r/tpidruro.h @@ -0,0 +1,20 @@ +/* + * Copyright (c) 2023 Arm Limited (or its affiliates). All rights reserved. + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief tpidruro bits allocation + * + * Among other things, the tpidruro holds the address for the current + * CPU's struct _cpu instance. But such a pointer is at least 4-bytes + * aligned. That leaves two of free bits for other purposes. + */ + +#ifndef ZEPHYR_INCLUDE_ARCH_ARM_CORTEX_A_R_TPIDRURO_H_ +#define ZEPHYR_INCLUDE_ARCH_ARM_CORTEX_A_R_TPIDRURO_H_ + +#define TPIDRURO_CURR_CPU 0xFFFFFFFCUL + +#endif /* ZEPHYR_INCLUDE_ARCH_ARM_CORTEX_A_R_TPIDRURO_H_ */