arm64: add up_this_task and up_change_task macro impl

Signed-off-by: hujun5 <[email protected]>
hujun260 · Sep 29, 2024 · 5ca0fa5 · 5ca0fa5
1 parent ed0a7e8
commit 5ca0fa5
Show file tree

Hide file tree

Showing 7 changed files with 63 additions and 84 deletions.
diff --git a/arch/arm64/include/arch.h b/arch/arm64/include/arch.h
@@ -34,6 +34,7 @@
 #ifndef __ASSEMBLY__
 #  include <stdint.h>
 #  include <stddef.h>
+#  include <nuttx/nuttx.h>
 #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
@@ -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
@@ -26,6 +26,7 @@
  ****************************************************************************/
 
 #include <nuttx/config.h>
+#include <arch/arch.h>
 
 /* 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
@@ -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
@@ -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
@@ -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
@@ -31,6 +31,7 @@
 #include <nuttx/arch.h>
 #include <nuttx/irq.h>
 
+#include "sched/sched.h"
 #include "arm64_arch.h"
 #include "arm64_internal.h"
 #include "chip.h"