2 files changed, 35 insertions, 3 deletions

diff --git a/firmware/kernel.c b/firmware/kernel.c
index 8474126a0d..c6ac1fb7aa 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -71,14 +71,14 @@ void queue_init(struct event_queue *q)
     q->write = 0;
 }
 
-struct event *queue_wait(struct event_queue *q)
+void queue_wait(struct event_queue *q, struct event *ev)
 {
     while(q->read == q->write)
     {
         switch_thread();
     }
 
-    return &q->events[(q->read++) & QUEUE_LENGTH_MASK];
+    *ev = q->events[(q->read++) & QUEUE_LENGTH_MASK];
 }
 
 void queue_post(struct event_queue *q, int id, void *data)
@@ -190,3 +190,26 @@ int tick_remove_task(void (*f)(void))
     set_irq_level(oldlevel);
     return -1;
 }
+
+/****************************************************************************
+ * Simple mutex functions
+ ****************************************************************************/
+void mutex_init(struct mutex *m)
+{
+    m->count = 0;
+}
+
+void mutex_lock(struct mutex *m)
+{
+    /* Wait until the lock is open... */
+    while(m->count)
+        yield();
+
+    /* ...and lock it */
+    m->count++;
+}
+
+void mutex_unlock(struct mutex *m)
+{
+    m->count--;
+}
diff --git a/firmware/kernel.h b/firmware/kernel.h
index 951ccee947..cd5539b791 100644
--- a/firmware/kernel.h
+++ b/firmware/kernel.h
@@ -41,6 +41,11 @@ struct event_queue
     unsigned int write;
 };
 
+struct mutex
+{
+    int count;
+};
+
 /* global tick variable */
 extern long current_tick;
 
@@ -53,7 +58,11 @@ int tick_add_task(void (*f)(void));
 int tick_remove_task(void (*f)(void));
 
 extern void queue_init(struct event_queue *q);
-extern struct event *queue_wait(struct event_queue *q);
+extern void queue_wait(struct event_queue *q, struct event *ev);
 extern void queue_post(struct event_queue *q, int id, void *data);
 
+extern void mutex_init(struct mutex *m);
+extern void mutex_lock(struct mutex *m);
+extern void mutex_unlock(struct mutex *m);
+
 #endif