kernel: support for more than 32 total priorities

In addition to more priorities taking more memory to host them, finding
the next thread to run when it is not cached is slower since each extra
set of 32 priorities maps to a loop iteration. That loop is remove
entirely when the number of priorities is less than 32 (31 + the idle
thread).

Fixes ZEP-1303.

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

5 files changed, 48 insertions, 14 deletions

diff --git a/kernel/unified/Kconfig b/kernel/unified/Kconfig
index 22d8b050a..d1c5b342c 100644
--- a/kernel/unified/Kconfig
+++ b/kernel/unified/Kconfig
@@ -45,8 +45,16 @@ config NUM_COOP_PRIORITIES
 	This can be set to zero to disable cooperative scheduling. Cooperative
 	threads always preempt preemptible threads.
 
-	Each priority requires an extra 8 bytes of RAM. If there are more than
-	32 total priorities, an extra 4 bytes is required.
+	Each priority requires an extra 8 bytes of RAM. Each set of 32 extra
+	total priorities require an extra 4 bytes and add one possible
+	iteration to loops that search for the next thread to run.
+
+	The total number of priorities is
+
+	   NUM_COOP_PRIORITIES + NUM_PREEMPT_PRIORITIES + 1
+
+	The extra one is for the idle thread, which must run at the lowest
+	priority, and be the only thread at that priority.
 
 config NUM_PREEMPT_PRIORITIES
 	int
@@ -58,11 +66,16 @@ config NUM_PREEMPT_PRIORITIES
 
 	This can be set to 0 to disable preemptible scheduling.
 
-	The idle thread is always installed as a preemptible thread of the
-	lowest priority.
+	Each priority requires an extra 8 bytes of RAM. Each set of 32 extra
+	total priorities require an extra 4 bytes and add one possible
+	iteration to loops that search for the next thread to run.
+
+	The total number of priorities is
+
+	   NUM_COOP_PRIORITIES + NUM_PREEMPT_PRIORITIES + 1
 
-	Each priority requires an extra 8 bytes of RAM. If there are more than
-	32 total priorities, an extra 4 bytes is required.
+	The extra one is for the idle thread, which must run at the lowest
+	priority, and be the only thread at that priority.
 
 config PRIORITY_CEILING
 	int
diff --git a/kernel/unified/include/kernel_structs.h b/kernel/unified/include/kernel_structs.h
index d9db1d75a..03a5dc6c4 100644
--- a/kernel/unified/include/kernel_structs.h
+++ b/kernel/unified/include/kernel_structs.h
@@ -107,7 +107,7 @@ struct _ready_q {
 	struct k_thread *cache;
 
 	/* bitmap of priorities that contain at least one ready thread */
-	uint32_t prio_bmap[1];
+	uint32_t prio_bmap[K_NUM_PRIO_BITMAPS];
 
 	/* ready queues, one per priority */
 	sys_dlist_t q[K_NUM_PRIORITIES];
diff --git a/kernel/unified/include/ksched.h b/kernel/unified/include/ksched.h
index 2fc71c0c1..8c091eba7 100644
--- a/kernel/unified/include/ksched.h
+++ b/kernel/unified/include/ksched.h
@@ -167,17 +167,32 @@ static inline int _get_ready_q_q_index(int prio)
 	return prio + CONFIG_NUM_COOP_PRIORITIES;
 }
 
-#if (K_NUM_PRIORITIES > 32)
-	#error not supported yet
-#endif
-
 /* find out the currently highest priority where a thread is ready to run */
 /* interrupts must be locked */
 static inline int _get_highest_ready_prio(void)
 {
-	uint32_t ready = _ready_q.prio_bmap[0];
+	int bitmap = 0;
+	uint32_t ready_range;
+
+#if (K_NUM_PRIORITIES <= 32)
+	ready_range = _ready_q.prio_bmap[0];
+#else
+	for (;; bitmap++) {
+
+		__ASSERT(bitmap < K_NUM_PRIO_BITMAPS, "prio out-of-range\n");
+
+		if (_ready_q.prio_bmap[bitmap]) {
+			ready_range = _ready_q.prio_bmap[bitmap];
+			break;
+		}
+	}
+#endif
+
+	int abs_prio = (find_lsb_set(ready_range) - 1) + (bitmap << 5);
+
+	__ASSERT(abs_prio < K_NUM_PRIORITIES, "prio out-of-range\n");
 
-	return find_lsb_set(ready) - 1 - CONFIG_NUM_COOP_PRIORITIES;
+	return abs_prio - CONFIG_NUM_COOP_PRIORITIES;
 }
 
 /*
diff --git a/kernel/unified/include/nano_internal.h b/kernel/unified/include/nano_internal.h
index f122b3fb9..46ae90181 100644
--- a/kernel/unified/include/nano_internal.h
+++ b/kernel/unified/include/nano_internal.h
@@ -28,6 +28,8 @@
 #define K_NUM_PRIORITIES \
 	(CONFIG_NUM_COOP_PRIORITIES + CONFIG_NUM_PREEMPT_PRIORITIES + 1)
 
+#define K_NUM_PRIO_BITMAPS ((K_NUM_PRIORITIES + 31) >> 5)
+
 #ifndef _ASMLANGUAGE
 
 #ifdef __cplusplus
diff --git a/kernel/unified/sched.c b/kernel/unified/sched.c
index 361ac4f95..74090cb78 100644
--- a/kernel/unified/sched.c
+++ b/kernel/unified/sched.c
@@ -348,7 +348,11 @@ k_tid_t k_current_get(void)
 /* debug aid */
 void _dump_ready_q(void)
 {
-	K_DEBUG("bitmap: %x\n", _ready_q.prio_bmap[0]);
+	K_DEBUG("bitmaps: ");
+	for (int bitmap = 0; bitmap < K_NUM_PRIO_BITMAPS; bitmap++) {
+		K_DEBUG("%x", _ready_q.prio_bmap[bitmap]);
+	}
+	K_DEBUG("\n");
 	for (int prio = 0; prio < K_NUM_PRIORITIES; prio++) {
 		K_DEBUG("prio: %d, head: %p\n",
 			prio - CONFIG_NUM_COOP_PRIORITIES,