From e6fb99beb42af4b7536774276b6a8380ca50ee6b Mon Sep 17 00:00:00 2001
From: Stefan Lankes <lankes@lfbs.rwth-aachen.de>
Date: Thu, 1 Sep 2011 13:31:41 -0700
Subject: [PATCH] add inline assembler functions to determine the MSB and LSB

---
 arch/x86/include/asm/processor.h | 30 +++++++++++++++++++++++++-----
 kernel/tasks.c                   | 15 +++++----------
 2 files changed, 30 insertions(+), 15 deletions(-)

diff --git a/arch/x86/include/asm/processor.h b/arch/x86/include/asm/processor.h
index ad962757..ad9e9f36 100644
--- a/arch/x86/include/asm/processor.h
+++ b/arch/x86/include/asm/processor.h
@@ -272,22 +272,42 @@ static inline uint32_t read_eflags(void)
 	return result;
 }
 
-/** @brief search the first bit, which is set
+/** @brief search the first most significant bit
  *
  * @param i source operand
- * @return first bit, which is set in the source operand
+ * @return 
+ * - first bit, which is set in the source operand
+ * - invalid value, if not bit ist set
  */
-static inline uint32_t last_set(uint32_t i)
+static inline size_t msb(size_t i)
 {
-	uint32_t ret;
+	size_t ret;
 
 	if (!i)
-		return 0;
+		return (sizeof(size_t)*8);
 	asm volatile ("bsr %1, %0" : "=r"(ret) : "r"(i) : "cc");
 
 	return ret;
 }
 
+/** @brief search the least significant bit
+ *
+ * @param i source operand
+ * @return 
+ * - first bit, which is set in the source operand
+ * - invalid value, if not bit ist set
+ */
+static inline size_t lsb(size_t i)
+{
+	size_t ret;
+
+	if (!i)
+		return (sizeof(size_t)*8);
+	asm volatile ("bsf %1, %0" : "=r"(ret) : "r"(i) : "cc");
+
+	return ret;
+}
+
 /** @brief Read extended instruction pointer
  * @return The EIP's value
  */
diff --git a/kernel/tasks.c b/kernel/tasks.c
index bc037aad..0689d401 100644
--- a/kernel/tasks.c
+++ b/kernel/tasks.c
@@ -69,7 +69,7 @@ task_t* get_current_task(void) {
 }
 
 uint32_t get_highest_priority(uint32_t core_id) {
-	return last_set(runqueues[core_id].prio_bitmap);
+	return msb(runqueues[core_id].prio_bitmap);
 }
 
 int multitasking_init(void) {
@@ -1228,20 +1228,15 @@ void scheduler(void)
 	}
 
 	runqueues[core_id].old_task = NULL; // reset old task
-	prio = last_set(runqueues[core_id].prio_bitmap); // determines highest priority
+	prio = msb(runqueues[core_id].prio_bitmap); // determines highest priority
 #if MAX_CORES > 1
-	if (!prio) {
+	if (prio >= sizeof(size_t)*8) {
 		load_balancing();
-		prio = last_set(runqueues[core_id].prio_bitmap); // retry...
+		prio = msb(runqueues[core_id].prio_bitmap); // retry...
 	}
 #endif
 
-	if (BUILTIN_EXPECT(prio > MAX_PRIO, 0)) {
-		kprintf("Invalid priority %u by bitmap 0x%x\n", prio, runqueues[core_id].prio_bitmap);
-		prio = 0;
-	}
-
-	if (!prio) {
+	if (prio >= sizeof(size_t)*8) {
 		if ((curr_task->status == TASK_RUNNING) || (curr_task->status == TASK_IDLE))
 			goto get_task_out;
 		curr_task = per_core(current_task) = runqueues[core_id].idle;