diff options
Diffstat (limited to 'firmware/kernel/semaphore.c')
-rw-r--r-- | firmware/kernel/semaphore.c | 142 |
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 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2002 by Björn Stenberg | ||
11 | * | ||
12 | * This program is free software; you can redistribute it and/or | ||
13 | * modify it under the terms of the GNU General Public License | ||
14 | * as published by the Free Software Foundation; either version 2 | ||
15 | * of the License, or (at your option) any later version. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | |||
22 | |||
23 | /**************************************************************************** | ||
24 | * Simple mutex functions ;) | ||
25 | ****************************************************************************/ | ||
26 | |||
27 | #include <stdbool.h> | ||
28 | #include "config.h" | ||
29 | #include "kernel.h" | ||
30 | #include "semaphore.h" | ||
31 | #include "kernel-internal.h" | ||
32 | #include "thread-internal.h" | ||
33 | |||
34 | /**************************************************************************** | ||
35 | * Simple semaphore functions ;) | ||
36 | ****************************************************************************/ | ||
37 | /* Initialize the semaphore object. | ||
38 | * max = maximum up count the semaphore may assume (max >= 1) | ||
39 | * start = initial count of semaphore (0 <= count <= max) */ | ||
40 | void semaphore_init(struct semaphore *s, int max, int start) | ||
41 | { | ||
42 | KERNEL_ASSERT(max > 0 && start >= 0 && start <= max, | ||
43 | "semaphore_init->inv arg\n"); | ||
44 | s->queue = NULL; | ||
45 | s->max = max; | ||
46 | s->count = start; | ||
47 | corelock_init(&s->cl); | ||
48 | } | ||
49 | |||
50 | /* Down the semaphore's count or wait for 'timeout' ticks for it to go up if | ||
51 | * it is already 0. 'timeout' as TIMEOUT_NOBLOCK (0) will not block and may | ||
52 | * safely be used in an ISR. */ | ||
53 | int semaphore_wait(struct semaphore *s, int timeout) | ||
54 | { | ||
55 | int ret; | ||
56 | int oldlevel; | ||
57 | int count; | ||
58 | |||
59 | oldlevel = disable_irq_save(); | ||
60 | corelock_lock(&s->cl); | ||
61 | |||
62 | count = s->count; | ||
63 | |||
64 | if(LIKELY(count > 0)) | ||
65 | { | ||
66 | /* count is not zero; down it */ | ||
67 | s->count = count - 1; | ||
68 | ret = OBJ_WAIT_SUCCEEDED; | ||
69 | } | ||
70 | else if(timeout == 0) | ||
71 | { | ||
72 | /* just polling it */ | ||
73 | ret = OBJ_WAIT_TIMEDOUT; | ||
74 | } | ||
75 | else | ||
76 | { | ||
77 | /* too many waits - block until count is upped... */ | ||
78 | struct thread_entry * current = thread_self_entry(); | ||
79 | IF_COP( current->obj_cl = &s->cl; ) | ||
80 | current->bqp = &s->queue; | ||
81 | /* return value will be OBJ_WAIT_SUCCEEDED after wait if wake was | ||
82 | * explicit in semaphore_release */ | ||
83 | current->retval = OBJ_WAIT_TIMEDOUT; | ||
84 | |||
85 | if(timeout > 0) | ||
86 | block_thread_w_tmo(current, timeout); /* ...or timed out... */ | ||
87 | else | ||
88 | block_thread(current); /* -timeout = infinite */ | ||
89 | |||
90 | corelock_unlock(&s->cl); | ||
91 | |||
92 | /* ...and turn control over to next thread */ | ||
93 | switch_thread(); | ||
94 | |||
95 | return current->retval; | ||
96 | } | ||
97 | |||
98 | corelock_unlock(&s->cl); | ||
99 | restore_irq(oldlevel); | ||
100 | |||
101 | return ret; | ||
102 | } | ||
103 | |||
104 | /* Up the semaphore's count and release any thread waiting at the head of the | ||
105 | * queue. The count is saturated to the value of the 'max' parameter specified | ||
106 | * in 'semaphore_init'. */ | ||
107 | void semaphore_release(struct semaphore *s) | ||
108 | { | ||
109 | unsigned int result = THREAD_NONE; | ||
110 | int oldlevel; | ||
111 | |||
112 | oldlevel = disable_irq_save(); | ||
113 | corelock_lock(&s->cl); | ||
114 | |||
115 | if(LIKELY(s->queue != NULL)) | ||
116 | { | ||
117 | /* a thread was queued - wake it up and keep count at 0 */ | ||
118 | KERNEL_ASSERT(s->count == 0, | ||
119 | "semaphore_release->threads queued but count=%d!\n", s->count); | ||
120 | s->queue->retval = OBJ_WAIT_SUCCEEDED; /* indicate explicit wake */ | ||
121 | result = wakeup_thread(&s->queue); | ||
122 | } | ||
123 | else | ||
124 | { | ||
125 | int count = s->count; | ||
126 | if(count < s->max) | ||
127 | { | ||
128 | /* nothing waiting - up it */ | ||
129 | s->count = count + 1; | ||
130 | } | ||
131 | } | ||
132 | |||
133 | corelock_unlock(&s->cl); | ||
134 | restore_irq(oldlevel); | ||
135 | |||
136 | #if defined(HAVE_PRIORITY_SCHEDULING) && defined(is_thread_context) | ||
137 | /* No thread switch if not thread context */ | ||
138 | if((result & THREAD_SWITCH) && is_thread_context()) | ||
139 | switch_thread(); | ||
140 | #endif | ||
141 | (void)result; | ||
142 | } | ||