1 files changed, 19 insertions, 0 deletions

diff --git a/firmware/kernel.c b/firmware/kernel.c
index 6da8a1b057..2b4b09f52b 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -102,6 +102,25 @@ void queue_wait(struct event_queue *q, struct event *ev)
     *ev = q->events[(q->read++) & QUEUE_LENGTH_MASK];
 }
 
+void queue_wait_w_tmo(struct event_queue *q, struct event *ev, int ticks)
+{
+    unsigned int timeout = current_tick + ticks;
+
+    while(q->read == q->write && TIME_BEFORE( current_tick, timeout ))
+    {
+        switch_thread();
+    }
+
+    if(q->read != q->write)
+    {
+        *ev = q->events[(q->read++) & QUEUE_LENGTH_MASK];
+    }
+    else
+    {
+        ev->id = SYS_TIMEOUT;
+    }
+}
+
 void queue_post(struct event_queue *q, int id, void *data)
 {
     int wr;