Don't schedule if you don't have to

6 files changed, 28 insertions, 14 deletions

diff --git a/include/sys/schedule.h b/include/sys/schedule.h
index 1e3b0d7..d03d39f 100644
--- a/include/sys/schedule.h
+++ b/include/sys/schedule.h
@@ -47,7 +47,7 @@ extern void cleanup(void);
 void yield(void);
 void sched_mutex_yield(void* m);
 void sched_semaphore_yield(void* s);
-void sched_mutex_resurrect(void* m);
-void sched_semaphore_resurrect(void* s, unsigned long count);
+unsigned long sched_mutex_resurrect(void* m);
+unsigned long sched_semaphore_resurrect(void* s, unsigned long count);
 
 #endif
diff --git a/kernel/exceptions/svc.S b/kernel/exceptions/svc.S
index 75a0cf1..452831a 100644
--- a/kernel/exceptions/svc.S
+++ b/kernel/exceptions/svc.S
@@ -72,9 +72,11 @@ svc_000005: // Release Lock
 	sev
 	// Awake any threads waiting for this lock
 	bl sched_mutex_resurrect
+	cmp r0, #0
+	beq 1f
 	ldmfd sp!, {r0-r12,lr}
 	b schedule
-	b svc_exit
+1:	b svc_exit
 svc_000006: // Semaphore decrease
 	ldr r0, [sp, #0] // struct Semaphore* s
 1:	clrex
@@ -106,9 +108,11 @@ svc_000007: // Semaphore increase
 	bne svc_exit
 	mov r1, #1
 	bl sched_semaphore_resurrect
+	cmp r0, #0
+	beq 1f
 	ldmfd sp!, {r0-r12,lr}
 	b schedule
-	b svc_exit
+1:	b svc_exit
 svc_000008: // Semaphore add #
 	ldr r0, [sp, #0] // struct Semaphore* s
 	ldr r3, [sp, #1] // unsigned long # times to increase
@@ -121,9 +125,11 @@ svc_000008: // Semaphore add #
 	dmb
 	mov r1, r3
 	bl sched_semaphore_resurrect
+	cmp r0, #0
+	beq 1f
 	ldmfd sp!, {r0-r12,lr}
 	b schedule
-	b svc_exit
+1:	b svc_exit
 svc_000009: // SYS_TIME_2
 	mrc p15, 0, r0, c9, c13, 0
 	str r0, [sp, #0]
diff --git a/kernel/sys/core.c b/kernel/sys/core.c
index 4812b00..c06ed4b 100644
--- a/kernel/sys/core.c
+++ b/kernel/sys/core.c
@@ -10,6 +10,7 @@
 #include <sys/core.h>
 #include <sys/power.h>
 #include <sys/schedule.h>
+#include <tests/test.h>
 #include <usr/main.h>
 #include <util/mutex.h>
 #include <util/status.h>
@@ -39,5 +40,6 @@ void sysinit(void)
 
 	// Start Scheduler
 	init_scheduler();
-	add_thread(main, 0, 0);
+	add_thread(test_entry, 0, 2);
+	//add_thread(main, 0, 0);
 }
diff --git a/kernel/sys/schedule.c b/kernel/sys/schedule.c
index 9b6d46e..3157c5d 100644
--- a/kernel/sys/schedule.c
+++ b/kernel/sys/schedule.c
@@ -418,12 +418,12 @@ void sched_semaphore_yield(void* s)
 		push_thread_to_queue(rt->value, THREAD_SWAIT, priority);
 }
 
-void sched_mutex_resurrect(void* m)
+unsigned long sched_mutex_resurrect(void* m)
 {
 	// Find any mutex to resurrect
 	struct Entry* prev = find_mutex_wait_next(m);
 	if (prev == 0)
-		return;
+		return 0;
 	struct Entry* entry = prev->next;
 	struct Thread* thread = entry->value;
 	// Resurrect the thread
@@ -431,7 +431,7 @@ void sched_mutex_resurrect(void* m)
 	// Remove from wait queue
 	entry = remove_next_from_queue(prev);
 	if (entry == 0)
-		return;
+		return 1;
 	// Add to ready queue
 	push_thread_to_queue(entry->value, THREAD_READY, ((struct Thread*)entry->value)->priority);
 	// Demote current thread
@@ -445,15 +445,19 @@ void sched_mutex_resurrect(void* m)
 		((struct Thread*)tentry->value)->old_priority = 0xFF;
 		prepend_thread_to_queue(tentry->value, THREAD_READY, op);
 	}
+	return 1;
 }
 
-void sched_semaphore_resurrect(void* s, unsigned long count)
+unsigned long sched_semaphore_resurrect(void* s, unsigned long count)
 {
-	while (count--) {
+	unsigned long cnt = count;
+	while (cnt) {
 		// Find any signal/ semaphore to resurrect
 		struct Entry* prev = find_signal_wait_next(s);
-		if (prev == 0)
-			return;
+		if (prev == 0 && count == cnt)
+			return 0;
+		else if (prev == 0)
+			return 1;
 		struct Entry* entry = prev->next;
 		struct Thread* thread = entry->value;
 		// Resurrect the thread
@@ -461,8 +465,10 @@ void sched_semaphore_resurrect(void* s, unsigned long count)
 		// Remove from wait queue
 		entry = remove_next_from_queue(prev);
 		if (entry == 0)
-			return;
+			return 1;
 		// Add to ready queue
 		push_thread_to_queue(entry->value, THREAD_READY, ((struct Thread*)entry->value)->priority);
+		cnt--;
 	}
+	return 1;
 }
diff --git a/kernel/tests/test.S b/usr/tests/test.S
index e80b6be..e80b6be 100644
--- a/kernel/tests/test.S
+++ b/usr/tests/test.S
diff --git a/kernel/tests/test.c b/usr/tests/test.c
index d954ade..d954ade 100644
--- a/kernel/tests/test.c
+++ b/usr/tests/test.c