From d151548b30c1cf7907af6e46a7c36af7c94556ad Mon Sep 17 00:00:00 2001
From: Aleksander Kaminski <aleksander.kaminski@phoenix-rtos.com>
Date: Wed, 11 Sep 2024 17:19:18 +0200
Subject: [PATCH] threads: Use binary heap for sleeping threads data structure

JIRA: RTOS-917
---
 proc/threads.c | 44 +++++++++++++++++++-------------------------
 proc/threads.h |  2 +-
 2 files changed, 20 insertions(+), 26 deletions(-)

diff --git a/proc/threads.c b/proc/threads.c
index 1b9280c9d..9d1483d35 100644
--- a/proc/threads.c
+++ b/proc/threads.c
@@ -40,7 +40,7 @@ struct {
 	time_t utcoffs;
 
 	/* Synchronized by spinlock */
-	rbtree_t sleeping;
+	bheap_t sleeping;
 
 	/* Synchronized by mutex */
 	unsigned int idcounter;
@@ -83,17 +83,16 @@ static time_t _proc_gettimeRaw(void)
 }
 
 
-static int threads_sleepcmp(rbnode_t *n1, rbnode_t *n2)
+static int threads_sleepcmp(bhnode_t *n1, bhnode_t *n2)
 {
-	thread_t *t1 = lib_treeof(thread_t, sleeplinkage, n1);
-	thread_t *t2 = lib_treeof(thread_t, sleeplinkage, n2);
+	thread_t *t1 = lib_bhof(thread_t, sleeplinkage, n1);
+	thread_t *t2 = lib_bhof(thread_t, sleeplinkage, n2);
 
-	if (t1->wakeup != t2->wakeup) {
-		return (t1->wakeup > t2->wakeup) ? 1 : -1;
-	}
-	else {
-		return (proc_getTid(t1) > proc_getTid(t2)) ? 1 : -1;
+	if (t1->wakeup == t2->wakeup) {
+		return 0;
 	}
+
+	return (t1->wakeup > t2->wakeup) ? 1 : -1;
 }
 
 /*
@@ -395,16 +394,11 @@ int perf_finish()
  */
 
 
-static void _threads_updateWakeup(time_t now, thread_t *min)
+static void _threads_updateWakeup(time_t now)
 {
-	thread_t *t;
+	thread_t *t = lib_bhof(thread_t, sleeplinkage, lib_bhPeek(&threads_common.sleeping));
 	time_t wakeup;
 
-	if (min != NULL)
-		t = min;
-	else
-		t = lib_treeof(thread_t, sleeplinkage, lib_rbMinimum(threads_common.sleeping.root));
-
 	if (t != NULL) {
 		if (now >= t->wakeup)
 			wakeup = 1;
@@ -437,7 +431,7 @@ int threads_timeintr(unsigned int n, cpu_context_t *context, void *arg)
 	now = _proc_gettimeRaw();
 
 	for (;;) {
-		t = lib_treeof(thread_t, sleeplinkage, lib_rbMinimum(threads_common.sleeping.root));
+		t = lib_bhof(thread_t, sleeplinkage, lib_bhPeek(&threads_common.sleeping));
 
 		if (t == NULL || t->wakeup > now) {
 			break;
@@ -447,7 +441,7 @@ int threads_timeintr(unsigned int n, cpu_context_t *context, void *arg)
 		hal_cpuSetReturnValue(t->context, (void *)-ETIME);
 	}
 
-	_threads_updateWakeup(now, t);
+	_threads_updateWakeup(now);
 
 	hal_spinlockClear(&threads_common.spinlock, &sc);
 
@@ -1048,7 +1042,7 @@ static void _proc_threadDequeue(thread_t *t)
 	}
 
 	if (t->wakeup) {
-		lib_rbRemove(&threads_common.sleeping, &t->sleeplinkage);
+		lib_bhRemove(&threads_common.sleeping, &t->sleeplinkage);
 	}
 
 	t->wakeup = 0;
@@ -1089,8 +1083,8 @@ static void _proc_threadEnqueue(thread_t **queue, time_t timeout, int interrupti
 
 	if (timeout) {
 		current->wakeup = timeout;
-		lib_rbInsert(&threads_common.sleeping, &current->sleeplinkage);
-		_threads_updateWakeup(_proc_gettimeRaw(), NULL);
+		lib_bhInsert(&threads_common.sleeping, &current->sleeplinkage);
+		_threads_updateWakeup(_proc_gettimeRaw());
 	}
 
 	_perf_enqueued(current);
@@ -1132,10 +1126,10 @@ int proc_threadSleep(time_t us)
 		current->wakeup = now + us;
 		current->interruptible = 1;
 
-		lib_rbInsert(&threads_common.sleeping, &current->sleeplinkage);
+		lib_bhInsert(&threads_common.sleeping, &current->sleeplinkage);
 
 		_perf_enqueued(current);
-		_threads_updateWakeup(now, NULL);
+		_threads_updateWakeup(now);
 	}
 
 	err = hal_cpuReschedule(&threads_common.spinlock, &sc);
@@ -1377,7 +1371,7 @@ static time_t _proc_nextWakeup(void)
 	time_t wakeup = 0;
 	time_t now;
 
-	thread = lib_treeof(thread_t, sleeplinkage, lib_rbMinimum(threads_common.sleeping.root));
+	thread = lib_bhof(thread_t, sleeplinkage, lib_bhPeek(&threads_common.sleeping));
 	if (thread != NULL) {
 		now = _proc_gettimeRaw();
 		if (now >= thread->wakeup)
@@ -2042,7 +2036,7 @@ int _threads_init(vm_map_t *kmap, vm_object_t *kernel)
 	for (i = 0; i < sizeof(threads_common.ready) / sizeof(thread_t *); i++)
 		threads_common.ready[i] = NULL;
 
-	lib_rbInit(&threads_common.sleeping, threads_sleepcmp, NULL);
+	lib_bhInit(&threads_common.sleeping, threads_sleepcmp);
 	lib_idtreeInit(&threads_common.id);
 
 	lib_printf("proc: Initializing thread scheduler, priorities=%d\n", sizeof(threads_common.ready) / sizeof(thread_t *));
diff --git a/proc/threads.h b/proc/threads.h
index f98206f7b..3dda113af 100644
--- a/proc/threads.h
+++ b/proc/threads.h
@@ -41,7 +41,7 @@ typedef struct _thread_t {
 	struct _thread_t *prev;
 	struct _lock_t *locks;
 
-	rbnode_t sleeplinkage;
+	bhnode_t sleeplinkage;
 	idnode_t idlinkage;
 
 	struct _process_t *process;