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