diff options
-rw-r--r-- | firmware/kernel/queue.c | 58 | ||||
-rw-r--r-- | firmware/kernel/thread-internal.h | 5 |
2 files changed, 41 insertions, 22 deletions
diff --git a/firmware/kernel/queue.c b/firmware/kernel/queue.c index 70dba46c0a..233b53c364 100644 --- a/firmware/kernel/queue.c +++ b/firmware/kernel/queue.c | |||
@@ -339,9 +339,6 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks) | |||
339 | 339 | ||
340 | oldlevel = disable_irq_save(); | 340 | oldlevel = disable_irq_save(); |
341 | 341 | ||
342 | if (ticks != TIMEOUT_NOBLOCK) | ||
343 | ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT, oldlevel); | ||
344 | |||
345 | corelock_lock(&q->cl); | 342 | corelock_lock(&q->cl); |
346 | 343 | ||
347 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME | 344 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME |
@@ -351,12 +348,17 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks) | |||
351 | 348 | ||
352 | rd = q->read; | 349 | rd = q->read; |
353 | wr = q->write; | 350 | wr = q->write; |
354 | if (rd == wr && ticks != 0) | 351 | |
352 | if(rd != wr || ticks == 0) | ||
353 | ; /* no block */ | ||
354 | else while(1) | ||
355 | { | 355 | { |
356 | ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT, oldlevel); | ||
357 | |||
356 | struct thread_entry *current = __running_self_entry(); | 358 | struct thread_entry *current = __running_self_entry(); |
357 | block_thread(current, ticks, &q->queue, NULL); | 359 | block_thread(current, ticks, &q->queue, NULL); |
358 | corelock_unlock(&q->cl); | ||
359 | 360 | ||
361 | corelock_unlock(&q->cl); | ||
360 | switch_thread(); | 362 | switch_thread(); |
361 | 363 | ||
362 | disable_irq(); | 364 | disable_irq(); |
@@ -365,29 +367,41 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks) | |||
365 | rd = q->read; | 367 | rd = q->read; |
366 | wr = q->write; | 368 | wr = q->write; |
367 | 369 | ||
368 | wait_queue_try_remove(current); | 370 | if(rd != wr) |
371 | break; | ||
372 | |||
373 | if(ticks < 0) | ||
374 | continue; /* empty again, infinite block */ | ||
375 | |||
376 | /* timeout is legit if thread is still queued and awake */ | ||
377 | if(LIKELY(wait_queue_try_remove(current))) | ||
378 | break; | ||
379 | |||
380 | /* we mustn't return earlier than expected wait time */ | ||
381 | ticks = get_tmo_tick(current) - current_tick; | ||
382 | if(ticks <= 0) | ||
383 | break; | ||
369 | } | 384 | } |
370 | 385 | ||
371 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME | 386 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME |
372 | if(ev) | 387 | if(UNLIKELY(!ev)) |
388 | ; /* just waiting for something */ | ||
389 | else | ||
373 | #endif | 390 | #endif |
391 | if(rd != wr) | ||
374 | { | 392 | { |
375 | /* no worry about a removed message here - status is checked inside | 393 | q->read = rd + 1; |
376 | locks - perhaps verify if timeout or false alarm */ | 394 | rd &= QUEUE_LENGTH_MASK; |
377 | if (rd != wr) | 395 | *ev = q->events[rd]; |
378 | { | 396 | |
379 | q->read = rd + 1; | 397 | /* Get data for a waiting thread if one */ |
380 | rd &= QUEUE_LENGTH_MASK; | 398 | queue_do_fetch_sender(q->send, rd); |
381 | *ev = q->events[rd]; | 399 | } |
382 | /* Get data for a waiting thread if one */ | 400 | else |
383 | queue_do_fetch_sender(q->send, rd); | 401 | { |
384 | } | 402 | ev->id = SYS_TIMEOUT; |
385 | else | 403 | ev->data = 0; |
386 | { | ||
387 | ev->id = SYS_TIMEOUT; | ||
388 | } | ||
389 | } | 404 | } |
390 | /* else just waiting on non-empty */ | ||
391 | 405 | ||
392 | corelock_unlock(&q->cl); | 406 | corelock_unlock(&q->cl); |
393 | restore_irq(oldlevel); | 407 | restore_irq(oldlevel); |
diff --git a/firmware/kernel/thread-internal.h b/firmware/kernel/thread-internal.h index 868e57c65c..fe053fa070 100644 --- a/firmware/kernel/thread-internal.h +++ b/firmware/kernel/thread-internal.h | |||
@@ -419,4 +419,9 @@ static inline void blocker_splay_init(struct blocker_splay *blsplay) | |||
419 | corelock_init(&blsplay->cl); | 419 | corelock_init(&blsplay->cl); |
420 | } | 420 | } |
421 | 421 | ||
422 | static inline long get_tmo_tick(struct thread_entry *thread) | ||
423 | { | ||
424 | return thread->tmo_tick; | ||
425 | } | ||
426 | |||
422 | #endif /* THREAD_INTERNAL_H */ | 427 | #endif /* THREAD_INTERNAL_H */ |