aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/exceptions/svc.S17
-rw-r--r--src/sys/schedule.c32
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);
+ }
}