From 5ca0fa519fb720a37958b222637fb8c935fe775a Mon Sep 17 00:00:00 2001 From: hujun5 Date: Tue, 23 Jul 2024 21:11:45 +0800 Subject: [PATCH] arm64: add up_this_task and up_change_task macro impl Signed-off-by: hujun5 --- arch/arm64/include/arch.h | 32 +++++++++++++++++ arch/arm64/include/irq.h | 36 +++++-------------- arch/arm64/src/common/arm64_arch.h | 42 +--------------------- arch/arm64/src/common/arm64_cpustart.c | 6 +++- arch/arm64/src/common/arm64_doirq.c | 15 ++++---- arch/arm64/src/common/arm64_fatal.c | 15 ++++---- arch/arm64/src/common/arm64_registerdump.c | 1 + 7 files changed, 63 insertions(+), 84 deletions(-) diff --git a/arch/arm64/include/arch.h b/arch/arm64/include/arch.h index 14190d4262241..242364aef181d 100644 --- a/arch/arm64/include/arch.h +++ b/arch/arm64/include/arch.h @@ -34,6 +34,7 @@ #ifndef __ASSEMBLY__ # include # include +# include #endif /**************************************************************************** @@ -52,6 +53,37 @@ #endif /* CONFIG_ARCH_ADDRENV */ +/**************************************************************************** + * Name: + * read_/write_/zero_/modify_ sysreg + * + * Description: + * + * ARMv8 Architecture Registers access method + * All the macros need a memory clobber + * + ****************************************************************************/ + +#define read_sysreg(reg) \ + ({ \ + uint64_t __val; \ + __asm__ volatile ("mrs %0, " STRINGIFY(reg) \ + : "=r" (__val) :: "memory"); \ + __val; \ + }) + +#define write_sysreg(__val, reg) \ + __asm__ volatile ("msr " STRINGIFY(reg) ", %0" \ + :: "r" (__val) : "memory") + +#define zero_sysreg(reg) \ + __asm__ volatile ("msr " STRINGIFY(reg) ", xzr" \ + ::: "memory") + +#define modify_sysreg(v,m,a) \ + write_sysreg((read_sysreg(a) & ~(m)) | \ + ((uintptr_t)(v) & (m)), a) + /**************************************************************************** * Inline functions ****************************************************************************/ diff --git a/arch/arm64/include/irq.h b/arch/arm64/include/irq.h index 1f8b45a7b7549..8ea9dbdc75c14 100644 --- a/arch/arm64/include/irq.h +++ b/arch/arm64/include/irq.h @@ -397,6 +397,15 @@ static inline void up_irq_restore(irqstate_t flags) # define up_cpu_index() (0) #endif +/**************************************************************************** + * Schedule acceleration macros + ****************************************************************************/ + +#define up_current_regs() (this_task()->xcp.regs) +#define up_this_task() ((struct tcb_s *)(read_sysreg(tpidr_el1) & ~1ul)) +#define up_update_task(t) modify_sysreg(t, ~1ul, tpidr_el1) +#define up_interrupt_context() (read_sysreg(tpidr_el1) & 1) + /**************************************************************************** * Name: * up_current_regs/up_set_current_regs @@ -410,20 +419,6 @@ static inline void up_irq_restore(irqstate_t flags) * ****************************************************************************/ -noinstrument_function -static inline_function uint64_t *up_current_regs(void) -{ - uint64_t *regs; - __asm__ volatile ("mrs %0, " "tpidr_el1" : "=r" (regs)); - return regs; -} - -noinstrument_function -static inline_function void up_set_current_regs(uint64_t *regs) -{ - __asm__ volatile ("msr " "tpidr_el1" ", %0" : : "r" (regs)); -} - #define up_switch_context(tcb, rtcb) \ do { \ if (!up_interrupt_context()) \ @@ -433,19 +428,6 @@ static inline_function void up_set_current_regs(uint64_t *regs) } \ } while (0) -/**************************************************************************** - * Name: up_interrupt_context - * - * Description: Return true is we are currently executing in - * the interrupt handler context. - * - ****************************************************************************/ - -static inline bool up_interrupt_context(void) -{ - return up_current_regs() != NULL; -} - #undef EXTERN #ifdef __cplusplus } diff --git a/arch/arm64/src/common/arm64_arch.h b/arch/arm64/src/common/arm64_arch.h index aa07c2cf1c7d2..bbc73c33d0a2a 100644 --- a/arch/arm64/src/common/arm64_arch.h +++ b/arch/arm64/src/common/arm64_arch.h @@ -26,6 +26,7 @@ ****************************************************************************/ #include +#include /* Unsigned integer with bit position n set (signed in * assembly language). @@ -404,47 +405,6 @@ static inline void arch_nop(void) __ret; \ }) -/**************************************************************************** - * Name: - * read_/write_/zero_ sysreg - * - * Description: - * - * ARMv8 Architecture Registers access method - * All the macros need a memory clobber - * - ****************************************************************************/ - -#define read_sysreg(reg) \ - ({ \ - uint64_t __val; \ - __asm__ volatile ("mrs %0, " STRINGIFY(reg) \ - : "=r" (__val) :: "memory"); \ - __val; \ - }) - -#define read_sysreg_dump(reg) \ - ({ \ - uint64_t __val; \ - __asm__ volatile ("mrs %0, " STRINGIFY(reg) \ - : "=r" (__val) :: "memory"); \ - sinfo("%s, regval=0x%llx\n", \ - STRINGIFY(reg), __val); \ - __val; \ - }) - -#define write_sysreg(__val, reg) \ - ({ \ - __asm__ volatile ("msr " STRINGIFY(reg) ", %0" \ - : : "r" (__val) : "memory"); \ - }) - -#define zero_sysreg(reg) \ - ({ \ - __asm__ volatile ("msr " STRINGIFY(reg) ", xzr" \ - ::: "memory"); \ - }) - /* Non-atomic modification of registers */ #define modreg8(v,m,a) putreg8((getreg8(a) & ~(m)) | ((v) & (m)), (a)) diff --git a/arch/arm64/src/common/arm64_cpustart.c b/arch/arm64/src/common/arm64_cpustart.c index bee962f99c70a..4e88baa206f52 100644 --- a/arch/arm64/src/common/arm64_cpustart.c +++ b/arch/arm64/src/common/arm64_cpustart.c @@ -113,7 +113,11 @@ static inline void local_delay(void) static void arm64_smp_init_top(void) { - struct tcb_s *tcb = this_task(); + struct tcb_s *tcb = current_task(this_cpu()); + + /* Init idle task to percpu reg */ + + up_update_task(tcb); #ifndef CONFIG_SUPPRESS_INTERRUPTS /* And finally, enable interrupts */ diff --git a/arch/arm64/src/common/arm64_doirq.c b/arch/arm64/src/common/arm64_doirq.c index 9806c2c41c043..499b2ab3e4f42 100644 --- a/arch/arm64/src/common/arm64_doirq.c +++ b/arch/arm64/src/common/arm64_doirq.c @@ -61,13 +61,12 @@ uint64_t *arm64_doirq(int irq, uint64_t * regs) /* Nested interrupts are not supported */ - DEBUGASSERT(up_current_regs() == NULL); + DEBUGASSERT(!up_interrupt_context()); - /* Current regs non-zero indicates that we are processing an interrupt; - * current_regs is also used to manage interrupt level context switches. - */ + /* Set irq flag */ + + write_sysreg((uintptr_t)tcb | 1, tpidr_el1); - up_set_current_regs(regs); tcb->xcp.regs = regs; /* Deliver the IRQ */ @@ -110,11 +109,9 @@ uint64_t *arm64_doirq(int irq, uint64_t * regs) regs = tcb->xcp.regs; } - /* Set current_regs to NULL to indicate that we are no longer in an - * interrupt handler. - */ + /* Clear irq flag */ - up_set_current_regs(NULL); + write_sysreg((uintptr_t)tcb & ~1ul, tpidr_el1); return regs; } diff --git a/arch/arm64/src/common/arm64_fatal.c b/arch/arm64/src/common/arm64_fatal.c index 5c1b5241904c3..86770175399d3 100644 --- a/arch/arm64/src/common/arm64_fatal.c +++ b/arch/arm64/src/common/arm64_fatal.c @@ -544,13 +544,18 @@ static int arm64_exception_handler(struct regs_context *regs) void arm64_fatal_handler(struct regs_context *regs) { + struct tcb_s *tcb = this_task(); int ret; /* Nested exception are not supported */ - DEBUGASSERT(up_current_regs() == NULL); + DEBUGASSERT(!up_interrupt_context()); - up_set_current_regs((uint64_t *)regs); + tcb->xcp.regs = (uint64_t *)regs; + + /* Set irq flag */ + + write_sysreg((uintptr_t)tcb | 1, tpidr_el1); ret = arm64_exception_handler(regs); @@ -561,11 +566,9 @@ void arm64_fatal_handler(struct regs_context *regs) PANIC_WITH_REGS("panic", regs); } - /* Set CURRENT_REGS to NULL to indicate that we are no longer in an - * Exception handler. - */ + /* Clear irq flag */ - up_set_current_regs(NULL); + write_sysreg((uintptr_t)tcb & ~1ul, tpidr_el1); } void arm64_register_debug_hook(int nr, fatal_handle_func_t fn) diff --git a/arch/arm64/src/common/arm64_registerdump.c b/arch/arm64/src/common/arm64_registerdump.c index 76e6e05635573..97dc7ca1b9cb2 100644 --- a/arch/arm64/src/common/arm64_registerdump.c +++ b/arch/arm64/src/common/arm64_registerdump.c @@ -31,6 +31,7 @@ #include #include +#include "sched/sched.h" #include "arm64_arch.h" #include "arm64_internal.h" #include "chip.h"