1 #include <sys/cpu.h>
2 #include "thread.h"
3 
4 void sem_init(struct semaphore *sem, int count)
5 {
6     if (!!sem) {
7 	sem->list.next = sem->list.prev = &sem->list;
8 	sem->count = count;
9     }
10 }
11 
12 mstime_t __sem_down_slow(struct semaphore *sem, mstime_t timeout)
13 {
14     irq_state_t irq;
15     mstime_t rv;
16 
17     irq = irq_save();
18 
19     if (!sem_is_valid(sem)) {
20 	rv = -1;
21     } else if (sem->count >= 0) {
22 	/* Something already freed the semaphore on us */
23 	rv = 0;
24     } else if (timeout == -1) {
25 	/* Immediate timeout */
26 	sem->count++;
27 	rv = -1;
28     } else {
29 	/* Put the thread to sleep... */
30 
31 	struct thread_block block;
32 	struct thread *curr = current();
33 	mstime_t now = ms_timer();
34 
35 	block.thread     = curr;
36 	block.semaphore  = sem;
37 	block.block_time = now;
38 	block.timeout    = timeout ? now+timeout : 0;
39 	block.timed_out  = false;
40 
41 	curr->blocked    = &block;
42 
43 	/* Add to the end of the wakeup list */
44 	block.list.prev       = sem->list.prev;
45 	block.list.next       = &sem->list;
46 	sem->list.prev        = &block.list;
47 	block.list.prev->next = &block.list;
48 
49 	__schedule();
50 
51 	rv = block.timed_out ? -1 : ms_timer() - block.block_time;
52     }
53 
54     irq_restore(irq);
55     return rv;
56 }
57 
58 void __sem_up_slow(struct semaphore *sem)
59 {
60     irq_state_t irq;
61     struct thread_list *l;
62 
63     irq = irq_save();
64 
65     /*
66      * It's possible that something did a down on the semaphore, but
67      * didn't get to add themselves to the queue just yet.  In that case
68      * we don't have to do anything, since the bailout clause in
69      * __sem_down_slow will take care of it.
70      */
71     if (!!sem) {
72 	l = sem->list.next;
73 	if (l != &sem->list) {
74 	    struct thread_block *block;
75 	    block = container_of(l, struct thread_block, list);
76 
77 	    sem->list.next = block->list.next;
78 	    block->list.next->prev = &sem->list;
79 
80 	    block->thread->blocked = NULL;
81 
82 	    __schedule();
83 	}
84     }
85 
86     irq_restore(irq);
87 }
88