88 lines
1.8 KiB
C
88 lines
1.8 KiB
C
#include <sys/cpu.h>
|
|
#include "thread.h"
|
|
|
|
void sem_init(struct semaphore *sem, int count)
|
|
{
|
|
if (!!sem) {
|
|
sem->list.next = sem->list.prev = &sem->list;
|
|
sem->count = count;
|
|
}
|
|
}
|
|
|
|
mstime_t __sem_down_slow(struct semaphore *sem, mstime_t timeout)
|
|
{
|
|
irq_state_t irq;
|
|
mstime_t rv;
|
|
|
|
irq = irq_save();
|
|
|
|
if (!sem_is_valid(sem)) {
|
|
rv = -1;
|
|
} else if (sem->count >= 0) {
|
|
/* Something already freed the semaphore on us */
|
|
rv = 0;
|
|
} else if (timeout == -1) {
|
|
/* Immediate timeout */
|
|
sem->count++;
|
|
rv = -1;
|
|
} else {
|
|
/* Put the thread to sleep... */
|
|
|
|
struct thread_block block;
|
|
struct thread *curr = current();
|
|
mstime_t now = ms_timer();
|
|
|
|
block.thread = curr;
|
|
block.semaphore = sem;
|
|
block.block_time = now;
|
|
block.timeout = timeout ? now+timeout : 0;
|
|
block.timed_out = false;
|
|
|
|
curr->blocked = █
|
|
|
|
/* Add to the end of the wakeup list */
|
|
block.list.prev = sem->list.prev;
|
|
block.list.next = &sem->list;
|
|
sem->list.prev = &block.list;
|
|
block.list.prev->next = &block.list;
|
|
|
|
__schedule();
|
|
|
|
rv = block.timed_out ? -1 : ms_timer() - block.block_time;
|
|
}
|
|
|
|
irq_restore(irq);
|
|
return rv;
|
|
}
|
|
|
|
void __sem_up_slow(struct semaphore *sem)
|
|
{
|
|
irq_state_t irq;
|
|
struct thread_list *l;
|
|
|
|
irq = irq_save();
|
|
|
|
/*
|
|
* It's possible that something did a down on the semaphore, but
|
|
* didn't get to add themselves to the queue just yet. In that case
|
|
* we don't have to do anything, since the bailout clause in
|
|
* __sem_down_slow will take care of it.
|
|
*/
|
|
if (!!sem) {
|
|
l = sem->list.next;
|
|
if (l != &sem->list) {
|
|
struct thread_block *block;
|
|
block = container_of(l, struct thread_block, list);
|
|
|
|
sem->list.next = block->list.next;
|
|
block->list.next->prev = &sem->list;
|
|
|
|
block->thread->blocked = NULL;
|
|
|
|
__schedule();
|
|
}
|
|
}
|
|
|
|
irq_restore(irq);
|
|
}
|