2 files changed, 46 insertions, 0 deletions

diff --git a/firmware/kernel.c b/firmware/kernel.c
index da3ddf5ca7..f408c77bae 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -35,3 +35,30 @@ void yield(void)
 {
     switch_thread();
 }
+
+/****************************************************************************
+ * Queue handling stuff
+ ****************************************************************************/
+void queue_init(struct event_queue *q)
+{
+    q->read = 0;
+    q->write = 0;
+}
+
+struct event *queue_wait(struct event_queue *q)
+{
+    while(q->read == q->write)
+    {
+        switch_thread();
+    }
+
+    return &q->events[(q->read++) & QUEUE_LENGTH_MASK];
+}
+
+void queue_post(struct event_queue *q, int id, void *data)
+{
+    int wr = (q->write++) & QUEUE_LENGTH_MASK;
+
+    q->events[wr].id = id;
+    q->events[wr].data = data;
+}
diff --git a/firmware/kernel.h b/firmware/kernel.h
index 60e8ad6db1..cef6cfe96a 100644
--- a/firmware/kernel.h
+++ b/firmware/kernel.h
@@ -25,6 +25,22 @@
 
 #define HZ      100 /* number of ticks per second */
 
+#define QUEUE_LENGTH 16 /* MUST be a power of 2 */
+#define QUEUE_LENGTH_MASK (QUEUE_LENGTH - 1)
+
+struct event
+{
+    int id;
+    void *data;
+};
+
+struct event_queue
+{
+    struct event events[QUEUE_LENGTH];
+    unsigned int read;
+    unsigned int write;
+};
+
 /* global tick variable */
 extern long current_tick;
 
@@ -32,5 +48,8 @@ extern long current_tick;
 extern void yield(void);
 extern void sleep(int ticks);
 
+extern void queue_init(struct event_queue *q);
+extern struct event *queue_wait(struct event_queue *q);
+extern void queue_post(struct event_queue *q, int id, void *data);
 
 #endif