In queue_wait_w_tmo, guarantee wait duration

It is possible to have a thread awoken and subsequently the message
that was placed in the queue has been removed by the time the thread
is able to check the queue. Ensure theads that failed to find a
message do not return prematurely.

It was at worst imprecise when a timeout is specified. It's entirely
incorrect if the function ever returns with SYS_TIMEOUT when using
TIMEOUT_BLOCK.

Change-Id: Ibd41eae8c787adf7a320a24603cf64ff8a6da66a

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)
 
     oldlevel = disable_irq_save();
 
-    if (ticks != TIMEOUT_NOBLOCK)
-        ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT, oldlevel);
-
     corelock_lock(&q->cl);
 
 #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)
 
     rd = q->read;
     wr = q->write;
-    if (rd == wr && ticks != 0)
+
+    if(rd != wr || ticks == 0)
+        ; /* no block */
+    else while(1)
     {
+        ASSERT_CPU_MODE(CPU_MODE_THREAD_CONTEXT, oldlevel);
+
         struct thread_entry *current = __running_self_entry();
         block_thread(current, ticks, &q->queue, NULL);
-        corelock_unlock(&q->cl);    
 
+        corelock_unlock(&q->cl);
         switch_thread();
 
         disable_irq();
@@ -365,29 +367,41 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
         rd = q->read;
         wr = q->write;
 
-        wait_queue_try_remove(current);
+        if(rd != wr)
+            break;
+
+        if(ticks < 0)
+            continue; /* empty again, infinite block */
+
+        /* timeout is legit if thread is still queued and awake */
+        if(LIKELY(wait_queue_try_remove(current)))
+            break;
+
+        /* we mustn't return earlier than expected wait time */
+        ticks = get_tmo_tick(current) - current_tick;
+        if(ticks <= 0)
+            break;
     }
 
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
-    if(ev)
+    if(UNLIKELY(!ev))
+        ; /* just waiting for something */
+    else
 #endif
+    if(rd != wr)
     {
-        /* no worry about a removed message here - status is checked inside
-           locks - perhaps verify if timeout or false alarm */
-        if (rd != wr)
-        {
-            q->read = rd + 1;
-            rd &= QUEUE_LENGTH_MASK;
-            *ev = q->events[rd];
-            /* Get data for a waiting thread if one */
-            queue_do_fetch_sender(q->send, rd);
-        }
-        else
-        {
-            ev->id = SYS_TIMEOUT;
-        }
+        q->read = rd + 1;
+        rd &= QUEUE_LENGTH_MASK;
+        *ev = q->events[rd];
+
+        /* Get data for a waiting thread if one */
+        queue_do_fetch_sender(q->send, rd);
+    }
+    else
+    {
+        ev->id   = SYS_TIMEOUT;
+        ev->data = 0;
     }
-    /* else just waiting on non-empty */
 
     corelock_unlock(&q->cl);
     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)
     corelock_init(&blsplay->cl);
 }
 
+static inline long get_tmo_tick(struct thread_entry *thread)
+{
+    return thread->tmo_tick;
+}
+
 #endif /* THREAD_INTERNAL_H */