diff options
| -rw-r--r-- | src/exceptions/svc.S | 17 | ||||
| -rw-r--r-- | src/sys/schedule.c | 32 |
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); + } } |