summaryrefslogtreecommitdiffstats
path: root/firmware/kernel/semaphore.c
diff options
context:
space:
mode:
Diffstat (limited to 'firmware/kernel/semaphore.c')
-rw-r--r--firmware/kernel/semaphore.c142
1 files changed, 142 insertions, 0 deletions
diff --git a/firmware/kernel/semaphore.c b/firmware/kernel/semaphore.c
new file mode 100644
index 0000000000..f9ff0ad987
--- /dev/null
+++ b/firmware/kernel/semaphore.c
@@ -0,0 +1,142 @@
+/***************************************************************************
+ * __________ __ ___.
+ * Open \______ \ ____ ____ | | _\_ |__ _______ ___
+ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
+ * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
+ * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
+ * \/ \/ \/ \/ \/
+ * $Id$
+ *
+ * Copyright (C) 2002 by Björn Stenberg
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Simple mutex functions ;)
+ ****************************************************************************/
+
+#include <stdbool.h>
+#include "config.h"
+#include "kernel.h"
+#include "semaphore.h"
+#include "kernel-internal.h"
+#include "thread-internal.h"
+
+/****************************************************************************
+ * Simple semaphore functions ;)
+ ****************************************************************************/
+/* Initialize the semaphore object.
+ * max = maximum up count the semaphore may assume (max >= 1)
+ * start = initial count of semaphore (0 <= count <= max) */
+void semaphore_init(struct semaphore *s, int max, int start)
+{
+ KERNEL_ASSERT(max > 0 && start >= 0 && start <= max,
+ "semaphore_init->inv arg\n");
+ s->queue = NULL;
+ s->max = max;
+ s->count = start;
+ corelock_init(&s->cl);
+}
+
+/* Down the semaphore's count or wait for 'timeout' ticks for it to go up if
+ * it is already 0. 'timeout' as TIMEOUT_NOBLOCK (0) will not block and may
+ * safely be used in an ISR. */
+int semaphore_wait(struct semaphore *s, int timeout)
+{
+ int ret;
+ int oldlevel;
+ int count;
+
+ oldlevel = disable_irq_save();
+ corelock_lock(&s->cl);
+
+ count = s->count;
+
+ if(LIKELY(count > 0))
+ {
+ /* count is not zero; down it */
+ s->count = count - 1;
+ ret = OBJ_WAIT_SUCCEEDED;
+ }
+ else if(timeout == 0)
+ {
+ /* just polling it */
+ ret = OBJ_WAIT_TIMEDOUT;
+ }
+ else
+ {
+ /* too many waits - block until count is upped... */
+ struct thread_entry * current = thread_self_entry();
+ IF_COP( current->obj_cl = &s->cl; )
+ current->bqp = &s->queue;
+ /* return value will be OBJ_WAIT_SUCCEEDED after wait if wake was
+ * explicit in semaphore_release */
+ current->retval = OBJ_WAIT_TIMEDOUT;
+
+ if(timeout > 0)
+ block_thread_w_tmo(current, timeout); /* ...or timed out... */
+ else
+ block_thread(current); /* -timeout = infinite */
+
+ corelock_unlock(&s->cl);
+
+ /* ...and turn control over to next thread */
+ switch_thread();
+
+ return current->retval;
+ }
+
+ corelock_unlock(&s->cl);
+ restore_irq(oldlevel);
+
+ return ret;
+}
+
+/* Up the semaphore's count and release any thread waiting at the head of the
+ * queue. The count is saturated to the value of the 'max' parameter specified
+ * in 'semaphore_init'. */
+void semaphore_release(struct semaphore *s)
+{
+ unsigned int result = THREAD_NONE;
+ int oldlevel;
+
+ oldlevel = disable_irq_save();
+ corelock_lock(&s->cl);
+
+ if(LIKELY(s->queue != NULL))
+ {
+ /* a thread was queued - wake it up and keep count at 0 */
+ KERNEL_ASSERT(s->count == 0,
+ "semaphore_release->threads queued but count=%d!\n", s->count);
+ s->queue->retval = OBJ_WAIT_SUCCEEDED; /* indicate explicit wake */
+ result = wakeup_thread(&s->queue);
+ }
+ else
+ {
+ int count = s->count;
+ if(count < s->max)
+ {
+ /* nothing waiting - up it */
+ s->count = count + 1;
+ }
+ }
+
+ corelock_unlock(&s->cl);
+ restore_irq(oldlevel);
+
+#if defined(HAVE_PRIORITY_SCHEDULING) && defined(is_thread_context)
+ /* No thread switch if not thread context */
+ if((result & THREAD_SWITCH) && is_thread_context())
+ switch_thread();
+#endif
+ (void)result;
+}