Multiple semaphore increment

2 files changed, 34 insertions, 15 deletions

diff --git a/src/exceptions/svc.S b/src/exceptions/svc.S
index 86f61c0..fdfe391 100644
--- a/src/exceptions/svc.S
+++ b/src/exceptions/svc.S
@@ -100,6 +100,22 @@ svc_000007: // Semaphore increase
 	dmb
 	cmp r1, #1
 	bne svc_exit
+	mov r1, #1
+	bl sched_semaphore_resurrect
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+	b svc_exit
+svc_000008: // Semaphore add #
+	ldr r0, [sp, #0] // struct Semaphore* s
+	ldr r3, [sp, #1] // unsigned long # times to increase
+1:	clrex
+	ldrex r2, [r0, #0]
+	add r1, r2, #1
+	strexeq r2, r1, [r0, #0]
+	teq r2, #0
+	bne 1b
+	dmb
+	mov r1, r3
 	bl sched_semaphore_resurrect
 	ldmfd sp!, {r0-r12,lr}
 	b schedule
@@ -116,3 +132,4 @@ svc_table:
 	.word svc_000005
 	.word svc_000006
 	.word svc_000007
+	.word svc_000008
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index 42a95b0..f54811d 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -447,20 +447,22 @@ void sched_mutex_resurrect(void* m)
 	}
 }
 
-void sched_semaphore_resurrect(void* s)
+void sched_semaphore_resurrect(void* s, unsigned long count)
 {
-	// Find any signal/ semaphore to resurrect
-	struct Entry* prev = find_signal_wait_next(s);
-	if (prev == 0)
-		return;
-	struct Entry* entry = prev->next;
-	struct Thread* thread = entry->value;
-	// Resurrect the thread
-	thread->mptr = 0;
-	// Remove from wait queue
-	entry = remove_next_from_queue(prev);
-	if (entry == 0)
-		return;
-	// Add to ready queue
-	push_thread_to_queue(entry->value, THREAD_READY, ((struct Thread*)entry->value)->priority);
+	while (count--) {
+		// Find any signal/ semaphore to resurrect
+		struct Entry* prev = find_signal_wait_next(s);
+		if (prev == 0)
+			return;
+		struct Entry* entry = prev->next;
+		struct Thread* thread = entry->value;
+		// Resurrect the thread
+		thread->mptr = 0;
+		// Remove from wait queue
+		entry = remove_next_from_queue(prev);
+		if (entry == 0)
+			return;
+		// Add to ready queue
+		push_thread_to_queue(entry->value, THREAD_READY, ((struct Thread*)entry->value)->priority);
+	}
 }