core: thread: add option to not wait on a semaphore at all

Implement a "trywait" option... if timeout is set to -1, then timeout
immediately if the semaphore isn't available.

Signed-off-by: H. Peter Anvin <hpa@zytor.com>

1 files changed, 33 insertions, 26 deletions

diff --git a/core/thread/semaphore.c b/core/thread/semaphore.c
index 9b216eda..6a2e4c13 100644
--- a/core/thread/semaphore.c
+++ b/core/thread/semaphore.c
@@ -9,40 +9,46 @@ void sem_init(struct semaphore *sem, int count)
 
 mstime_t __sem_down_slow(struct semaphore *sem, mstime_t timeout)
 {
-    struct thread *curr;
-    struct thread_block block;
     irq_state_t irq;
-    mstime_t now;
+    mstime_t rv;
 
     irq = irq_save();
 
-    /* Check if something already freed the semaphore on us */
     if (sem->count >= 0) {
-	sti();
-	return 0;
-    }
-
-    curr = current();
-    now = ms_timer();
-
-    block.thread     = curr;
-    block.semaphore  = sem;
-    block.block_time = now;
-    block.timeout    = timeout ? now+timeout : 0;
-    block.timed_out  = false;
+	/* 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;
 
-    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();
 
-    __schedule();
+	rv = block.timed_out ? -1 : ms_timer() - block.block_time;
+    }
 
     irq_restore(irq);
-    return block.timed_out ? -1 : ms_timer() - block.block_time;
+    return rv;
 }
 
 void __sem_up_slow(struct semaphore *sem)
@@ -60,7 +66,8 @@ void __sem_up_slow(struct semaphore *sem)
      */
     l = sem->list.next;
     if (l != &sem->list) {
-	struct thread_block *block = container_of(l, struct thread_block, 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;