From f07df42d49c0a66d6d9ded7c975faef50fca7eca Mon Sep 17 00:00:00 2001
From: Andrew Boie <andrew.p.boie@intel.com>
Date: Fri, 6 Nov 2020 13:11:12 -0800
Subject: [PATCH] kernel: make k_current_get() work without syscall

We cache the current thread ID in a thread-local variable
at thread entry, and have k_current_get() return that,
eliminating system call overhead for this API.

DL: changed _current to use z_current_get() as it is
    being used during boot where TLS is not available.

Signed-off-by: Andrew Boie <andrew.p.boie@intel.com>
Signed-off-by: Daniel Leung <daniel.leung@intel.com>
---
 include/kernel.h         | 24 +++++++++++++++++++++++-
 include/kernel_structs.h |  2 +-
 kernel/sched.c           |  8 ++++----
 lib/os/thread_entry.c    |  7 +++++++
 4 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/include/kernel.h b/include/kernel.h
index 49a6727753a..4c2aea5f0f5 100644
--- a/include/kernel.h
+++ b/include/kernel.h
@@ -487,13 +487,35 @@ __syscall void k_yield(void);
  */
 __syscall void k_wakeup(k_tid_t thread);
 
+/**
+ * @brief Get thread ID of the current thread.
+ *
+ * This unconditionally queries the kernel via a system call.
+ *
+ * @return ID of current thread.
+ */
+__syscall k_tid_t z_current_get(void);
+
+#ifdef CONFIG_THREAD_LOCAL_STORAGE
+/* Thread-local cache of current thread ID, set in z_thread_entry() */
+extern __thread k_tid_t z_tls_current;
+#endif
+
 /**
  * @brief Get thread ID of the current thread.
  *
  * @return ID of current thread.
  *
  */
-__syscall k_tid_t k_current_get(void) __attribute_const__;
+__attribute_const__
+static inline k_tid_t k_current_get(void)
+{
+#ifdef CONFIG_THREAD_LOCAL_STORAGE
+	return z_tls_current;
+#else
+	return z_current_get();
+#endif
+}
 
 /**
  * @brief Abort a thread.
diff --git a/include/kernel_structs.h b/include/kernel_structs.h
index 50945aac8d7..e4c79a5780a 100644
--- a/include/kernel_structs.h
+++ b/include/kernel_structs.h
@@ -187,7 +187,7 @@ bool z_smp_cpu_mobile(void);
 
 #define _current_cpu ({ __ASSERT_NO_MSG(!z_smp_cpu_mobile()); \
 			arch_curr_cpu(); })
-#define _current k_current_get()
+#define _current z_current_get()
 
 #else
 #define _current_cpu (&_kernel.cpus[0])
diff --git a/kernel/sched.c b/kernel/sched.c
index c5e5670c624..c49354c1022 100644
--- a/kernel/sched.c
+++ b/kernel/sched.c
@@ -1365,7 +1365,7 @@ static inline void z_vrfy_k_wakeup(k_tid_t thread)
 #include <syscalls/k_wakeup_mrsh.c>
 #endif
 
-k_tid_t z_impl_k_current_get(void)
+k_tid_t z_impl_z_current_get(void)
 {
 #ifdef CONFIG_SMP
 	/* In SMP, _current is a field read from _current_cpu, which
@@ -1384,11 +1384,11 @@ k_tid_t z_impl_k_current_get(void)
 }
 
 #ifdef CONFIG_USERSPACE
-static inline k_tid_t z_vrfy_k_current_get(void)
+static inline k_tid_t z_vrfy_z_current_get(void)
 {
-	return z_impl_k_current_get();
+	return z_impl_z_current_get();
 }
-#include <syscalls/k_current_get_mrsh.c>
+#include <syscalls/z_current_get_mrsh.c>
 #endif
 
 int z_impl_k_is_preempt_thread(void)
diff --git a/lib/os/thread_entry.c b/lib/os/thread_entry.c
index aa3722d3ed6..4a8be64b4eb 100644
--- a/lib/os/thread_entry.c
+++ b/lib/os/thread_entry.c
@@ -13,6 +13,10 @@
 
 #include <kernel.h>
 
+#ifdef CONFIG_THREAD_LOCAL_STORAGE
+__thread k_tid_t z_tls_current;
+#endif
+
 /*
  * Common thread entry point function (used by all threads)
  *
@@ -26,6 +30,9 @@
 FUNC_NORETURN void z_thread_entry(k_thread_entry_t entry,
 				 void *p1, void *p2, void *p3)
 {
+#ifdef CONFIG_THREAD_LOCAL_STORAGE
+	z_tls_current = z_current_get();
+#endif
 	entry(p1, p2, p3);
 
 	k_thread_abort(k_current_get());