001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019 #include <sos/klibc.h>
020 #include <sos/list.h>
021 #include <sos/assert.h>
022 #include <hwcore/irq.h>
023
024 #include "kwaitq.h"
025
026
027 sos_ret_t sos_kwaitq_init(struct sos_kwaitq *kwq,
028 const char *name,
029 sos_kwaitq_ordering_t ordering)
030 {
031 memset(kwq, 0x0, sizeof(struct sos_kwaitq));
032
033 #ifdef SOS_KWQ_DEBUG
034 if (! name)
035 name = "<unknown>";
036 strzcpy(kwq->name, name, SOS_KWQ_DEBUG_MAX_NAMELEN);
037 #endif
038 kwq->ordering = ordering;
039 list_init_named(kwq->waiting_list,
040 prev_entry_in_kwaitq, next_entry_in_kwaitq);
041
042 return SOS_OK;
043 }
044
045
046 sos_ret_t sos_kwaitq_dispose(struct sos_kwaitq *kwq)
047 {
048 sos_ui32_t flags;
049 sos_ret_t retval;
050
051 sos_disable_IRQs(flags);
052 if (list_is_empty_named(kwq->waiting_list,
053 prev_entry_in_kwaitq, next_entry_in_kwaitq))
054 retval = SOS_OK;
055 else
056 retval = -SOS_EBUSY;
057
058 sos_restore_IRQs(flags);
059 return retval;
060 }
061
062
063 sos_bool_t sos_kwaitq_is_empty(const struct sos_kwaitq *kwq)
064 {
065 sos_ui32_t flags;
066 sos_ret_t retval;
067
068 sos_disable_IRQs(flags);
069 retval = list_is_empty_named(kwq->waiting_list,
070 prev_entry_in_kwaitq, next_entry_in_kwaitq);
071
072 sos_restore_IRQs(flags);
073 return retval;
074 }
075
076
077 sos_ret_t sos_kwaitq_init_entry(struct sos_kwaitq_entry *kwq_entry)
078 {
079 memset(kwq_entry, 0x0, sizeof(struct sos_kwaitq_entry));
080 kwq_entry->thread = sos_thread_get_current();
081 return SOS_OK;
082 }
083
084
085
086
087 inline static sos_ret_t
088 _kwaitq_add_entry(struct sos_kwaitq *kwq,
089 struct sos_kwaitq_entry *kwq_entry,
090 sos_sched_priority_t prio)
091 {
092 struct sos_kwaitq_entry *next_entry = NULL, *entry;
093 int nb_entries;
094
095
096 SOS_ASSERT_FATAL(NULL == kwq_entry->kwaitq);
097
098
099 SOS_ASSERT_FATAL(NULL != kwq_entry->thread);
100
101
102 kwq_entry->wakeup_triggered = FALSE;
103 kwq_entry->wakeup_status = SOS_OK;
104
105
106 switch (kwq->ordering)
107 {
108 case SOS_KWQ_ORDER_FIFO:
109
110 {
111
112 list_add_tail_named(kwq->waiting_list, kwq_entry,
113 prev_entry_in_kwaitq, next_entry_in_kwaitq);
114 }
115 break;
116
117 case SOS_KWQ_ORDER_PRIO:
118
119 {
120
121
122 list_foreach_forward_named(kwq->waiting_list, entry, nb_entries,
123 prev_entry_in_kwaitq, next_entry_in_kwaitq)
124 {
125
126
127 if (SOS_SCHED_PRIO_CMP(prio,
128 sos_thread_get_priority(entry->thread))
129 > 0)
130 {
131
132 next_entry = entry;
133 break;
134 }
135 }
136
137
138 if (next_entry != NULL)
139 {
140 list_insert_before_named(kwq->waiting_list, kwq_entry, next_entry,
141 prev_entry_in_kwaitq,
142 next_entry_in_kwaitq);
143 }
144 else
145 {
146
147
148 list_add_tail_named(kwq->waiting_list, kwq_entry,
149 prev_entry_in_kwaitq, next_entry_in_kwaitq);
150 }
151 }
152 break;
153
154 default:
155 SOS_FATAL_ERROR("Invalid kwq ordering %d !\n", kwq->ordering);
156 break;
157 }
158
159
160 list_add_tail_named(kwq_entry->thread->kwaitq_list, kwq_entry,
161 prev_entry_for_thread, next_entry_for_thread);
162
163 kwq_entry->kwaitq = kwq;
164
165 return SOS_OK;
166 }
167
168
169 sos_ret_t sos_kwaitq_add_entry(struct sos_kwaitq *kwq,
170 struct sos_kwaitq_entry *kwq_entry)
171 {
172 sos_ui32_t flags;
173 sos_ret_t retval;
174
175 sos_disable_IRQs(flags);
176 retval = _kwaitq_add_entry(kwq, kwq_entry,
177 sos_thread_get_priority(kwq_entry->thread));
178 sos_restore_IRQs(flags);
179
180 return retval;
181 }
182
183
184
185
186 inline static sos_ret_t
187 _kwaitq_remove_entry(struct sos_kwaitq *kwq,
188 struct sos_kwaitq_entry *kwq_entry)
189 {
190 SOS_ASSERT_FATAL(kwq_entry->kwaitq == kwq);
191
192 list_delete_named(kwq->waiting_list, kwq_entry,
193 prev_entry_in_kwaitq, next_entry_in_kwaitq);
194
195 list_delete_named(kwq_entry->thread->kwaitq_list, kwq_entry,
196 prev_entry_for_thread, next_entry_for_thread);
197
198 kwq_entry->kwaitq = NULL;
199 return SOS_OK;
200 }
201
202
203 sos_ret_t sos_kwaitq_remove_entry(struct sos_kwaitq *kwq,
204 struct sos_kwaitq_entry *kwq_entry)
205 {
206 sos_ui32_t flags;
207 sos_ret_t retval;
208
209 sos_disable_IRQs(flags);
210 retval = _kwaitq_remove_entry(kwq, kwq_entry);
211 sos_restore_IRQs(flags);
212
213 return retval;
214 }
215
216
217 sos_ret_t sos_kwaitq_wait(struct sos_kwaitq *kwq,
218 struct sos_time *timeout)
219 {
220 sos_ui32_t flags;
221 sos_ret_t retval;
222 struct sos_kwaitq_entry kwq_entry;
223
224 sos_kwaitq_init_entry(& kwq_entry);
225
226 sos_disable_IRQs(flags);
227
228 retval = _kwaitq_add_entry(kwq, & kwq_entry,
229 sos_thread_get_priority(kwq_entry.thread));
230
231
232 sos_thread_sleep(timeout);
233
234
235
236 if (! kwq_entry.wakeup_triggered)
237 {
238
239
240 _kwaitq_remove_entry(kwq, & kwq_entry);
241 retval = -SOS_EINTR;
242 }
243 else
244 {
245 retval = kwq_entry.wakeup_status;
246 }
247
248 sos_restore_IRQs(flags);
249
250
251 return retval;
252 }
253
254
255 sos_ret_t sos_kwaitq_wakeup(struct sos_kwaitq *kwq,
256 unsigned int nb_threads,
257 sos_ret_t wakeup_status)
258 {
259 sos_ui32_t flags;
260
261 sos_disable_IRQs(flags);
262
263
264
265
266 while (! list_is_empty_named(kwq->waiting_list,
267 prev_entry_in_kwaitq, next_entry_in_kwaitq))
268 {
269 struct sos_kwaitq_entry *kwq_entry
270 = list_get_head_named(kwq->waiting_list,
271 prev_entry_in_kwaitq, next_entry_in_kwaitq);
272
273
274 if (nb_threads <= 0)
275 break;
276
277
278
279
280
281
282 if (SOS_THR_RUNNING == sos_thread_get_state(kwq_entry->thread))
283 {
284
285
286
287
288 continue;
289 }
290 else
291 {
292
293 sos_sched_set_ready(kwq_entry->thread);
294 }
295
296
297 _kwaitq_remove_entry(kwq, kwq_entry);
298 kwq_entry->wakeup_triggered = TRUE;
299 kwq_entry->wakeup_status = wakeup_status;
300
301
302 nb_threads --;
303 }
304
305 sos_restore_IRQs(flags);
306
307 return SOS_OK;
308 }
309
310
311
312 sos_ret_t sos_kwaitq_change_priority(struct sos_kwaitq *kwq,
313 struct sos_kwaitq_entry *kwq_entry,
314 sos_sched_priority_t priority)
315 {
316
317 _kwaitq_remove_entry(kwq, kwq_entry);
318 _kwaitq_add_entry(kwq, kwq_entry, priority);
319
320 return SOS_OK;
321 }