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 = █ 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
