unified: cache the next thread to run

When adding a thread to the ready queue, it is often known at that time
if the thread added will be the next one to run or not. So, instead of
simply updating the ready queues and the bitmask, also cache what that
thread is, so that when the scheduler is invoked, it can simply fetch it
from there. This is only done if there is a thread in the cache, since
the way the cache is updated is by comparing the priorities of the
thread being added and the cached thread.

When a thread is removed from the ready queue, if it is currently the
cached thread, it is also removed from the cache. The cache is not
updated at this time, since this would be a preemptive fetching that
could be overriden before the newly cached thread would even be
scheduled in.

Finally, when a thread is scheduled in, it now becomes the cached thread
since the fact that it is running means that by definition it was the
next one to run.

Doing this can speed up considerably some context switch times,
especially when a thread is preempted by an interrupt and the same
thread is scheduled when the interrupt exits.

Change-Id: I6dc8391cfca566699bb9b217eafe6bc6a063c8bb
Signed-off-by: Benjamin Walsh <benjamin.walsh@windriver.com>

arch/arm/include/nano_private.h

@@ -226,6 +226,7 @@ struct tcs {
 
 #ifdef CONFIG_KERNEL_V2
 struct ready_q {
+	struct k_thread *cache;
 	uint32_t prio_bmap[1];
 	sys_dlist_t q[K_NUM_PRIORITIES];
 };

arch/x86/include/nano_private.h

@@ -745,6 +745,7 @@ struct tcs {
 
 #ifdef CONFIG_KERNEL_V2
 struct ready_q {
+	struct k_thread *cache;
 	uint32_t prio_bmap[1];
 	sys_dlist_t q[K_NUM_PRIORITIES];
 };

kernel/unified/include/sched.h

@@ -32,6 +32,7 @@ extern void k_sched_unlock(void);
 extern void _pend_thread(struct tcs *thread,
 			 _wait_q_t *wait_q, int32_t timeout);
 extern void _pend_current_thread(_wait_q_t *wait_q, int32_t timeout);
+extern void _move_thread_to_end_of_prio_q(struct k_thread *thread);
 extern struct tcs *_get_next_ready_thread(void);
 extern int __must_switch_threads(void);
 extern void k_thread_priority_set(struct tcs *thread, int32_t priority);

kernel/unified/sched.c

@@ -41,7 +41,13 @@ static void _clear_ready_q_prio_bit(int prio)
 /*
  * Add thread to the ready queue, in the slot for its priority; the thread
  * must not be on a wait queue.
+ *
+ * This function, along with _move_thread_to_end_of_prio_q(), are the _only_
+ * places where a thread is put on the ready queue.
+ *
+ * Interrupts must be locked when calling this function.
  */
+
 void _add_thread_to_ready_q(struct tcs *thread)
 {
 	int q_index = _get_ready_q_q_index(thread->prio);
@@ -49,9 +55,20 @@ void _add_thread_to_ready_q(struct tcs *thread)
 
 	_set_ready_q_prio_bit(thread->prio);
 	sys_dlist_append(q, &thread->k_q_node);
+
+	struct k_thread **cache = &_nanokernel.ready_q.cache;
+
+	*cache = *cache && _is_prio_higher(thread->prio, (*cache)->prio) ?
+		 thread : *cache;
 }
 
-/* remove thread from the ready queue */
+/*
+ * This function, along with _move_thread_to_end_of_prio_q(), are the _only_
+ * places where a thread is taken off the ready queue.
+ *
+ * Interrupts must be locked when calling this function.
+ */
+
 void _remove_thread_from_ready_q(struct tcs *thread)
 {
 	int q_index = _get_ready_q_q_index(thread->prio);
@@ -61,6 +78,10 @@ void _remove_thread_from_ready_q(struct tcs *thread)
 	if (sys_dlist_is_empty(q)) {
 		_clear_ready_q_prio_bit(thread->prio);
 	}
+
+	struct k_thread **cache = &_nanokernel.ready_q.cache;
+
+	*cache = *cache == thread ? NULL : *cache;
 }
 
 /* reschedule threads if the scheduler is not locked */
@@ -142,9 +163,11 @@ void _pend_current_thread(_wait_q_t *wait_q, int32_t timeout)
 	_pend_thread(_current, wait_q, timeout);
 }
 
-/* find which one is the next thread to run */
-/* must be called with interrupts locked */
-struct tcs *_get_next_ready_thread(void)
+/*
+ * Find the next thread to run when there is no thread in the cache and update
+ * the cache.
+ */
+static struct k_thread *__get_next_ready_thread(void)
 {
 	int prio = _get_highest_ready_prio();
 	int q_index = _get_ready_q_q_index(prio);
@@ -157,9 +180,20 @@ struct tcs *_get_next_ready_thread(void)
 	struct k_thread *thread =
 		(struct k_thread *)sys_dlist_peek_head_not_empty(list);
 
+	_nanokernel.ready_q.cache = thread;
+
 	return thread;
 }
 
+/* find which one is the next thread to run */
+/* must be called with interrupts locked */
+struct k_thread *_get_next_ready_thread(void)
+{
+	struct k_thread *cache = _nanokernel.ready_q.cache;
+
+	return cache ? cache : __get_next_ready_thread();
+}
+
 /*
  * Check if there is a thread of higher prio than the current one. Should only
  * be called if we already know that the current thread is preemptible.
@@ -197,6 +231,30 @@ int k_current_priority_get(void)
 	return k_thread_priority_get(_current);
 }
 
+/*
+ * Interrupts must be locked when calling this function.
+ *
+ * This function, along with _add_thread_to_ready_q() and
+ * _remove_thread_from_ready_q(), are the _only_ places where a thread is
+ * taken off or put on the ready queue.
+ */
+void _move_thread_to_end_of_prio_q(struct k_thread *thread)
+{
+	int q_index = _get_ready_q_q_index(thread->prio);
+	sys_dlist_t *q = &_nanokernel.ready_q.q[q_index];
+
+	if (sys_dlist_is_tail(q, &thread->k_q_node)) {
+		return;
+	}
+
+	sys_dlist_remove(&thread->k_q_node);
+	sys_dlist_append(q, &thread->k_q_node);
+
+	struct k_thread **cache = &_nanokernel.ready_q.cache;
+
+	*cache = *cache == thread ? NULL : *cache;
+}
+
 /*
  * application API: the current thread yields control to threads of higher or
  * equal priorities. This is done by remove the thread from the ready queue,
@@ -209,8 +267,7 @@ void k_yield(void)
 
 	int key = irq_lock();
 
-	_remove_thread_from_ready_q(_current);
-	_add_thread_to_ready_q(_current);
+	_move_thread_to_end_of_prio_q(_current);
 
 	if (_current == _get_next_ready_thread()) {
 		irq_unlock(key);

kernel/unified/sys_clock.c

@@ -211,8 +211,7 @@ static void handle_time_slicing(int32_t ticks)
 	_time_slice_elapsed += _ticks_to_ms(ticks);
 	if (_time_slice_elapsed >= _time_slice_duration) {
 		_time_slice_elapsed = 0;
-		_remove_thread_from_ready_q(_current);
-		_add_thread_to_ready_q(_current);
+		_move_thread_to_end_of_prio_q(_current);
 	}
 }
 #else