[rtems commit] cpukit/aarch64: Add Per_CPU_Control accessor

[Joel Sherrill]

Module:    rtems
Branch:    master
Commit:    3e3393ac1e5b25d4b172dd1e9b77d202753739f7
Changeset: http://git.rtems.org/rtems/commit/?id=3e3393ac1e5b25d4b172dd1e9b77d202753739f7

Author:    Kinsey Moore <kinsey.moore at oarcorp.com>
Date:      Mon Jan  3 16:45:26 2022 -0600

cpukit/aarch64: Add Per_CPU_Control accessor

Add an architecture-specific implementation for
_CPU_Get_current_per_CPU_control() to reduce overhead for getting the
current CPU's Per_CPU_Control structure.

---

 .../cpu/aarch64/include/rtems/score/cpuimpl.h      | 23 ++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/cpukit/score/cpu/aarch64/include/rtems/score/cpuimpl.h b/cpukit/score/cpu/aarch64/include/rtems/score/cpuimpl.h
index 90fd48a..ffdef2f 100644
--- a/cpukit/score/cpu/aarch64/include/rtems/score/cpuimpl.h
+++ b/cpukit/score/cpu/aarch64/include/rtems/score/cpuimpl.h
@@ -125,6 +125,29 @@ typedef struct {
   uint64_t register_fpcr;
 } CPU_Interrupt_frame;
 
+#ifdef RTEMS_SMP
+
+static inline
+struct Per_CPU_Control *_AARCH64_Get_current_per_CPU_control( void )
+{
+  struct Per_CPU_Control *cpu_self;
+  uint64_t value;
+
+  __asm__ volatile (
+    "mrs %0, TPIDR_EL1" : "=&r" ( value ) : : "memory"
+  );
+
+  /* Use EL1 Thread ID Register (TPIDR_EL1) */
+  cpu_self = (struct Per_CPU_Control *)(uintptr_t)value;
+
+  return cpu_self;
+}
+
+#define _CPU_Get_current_per_CPU_control() \
+  _AARCH64_Get_current_per_CPU_control()
+
+#endif /* RTEMS_SMP */
+
 void _CPU_Context_volatile_clobber( uintptr_t pattern );
 
 void _CPU_Context_validate( uintptr_t pattern );