1 files changed, 43 insertions, 0 deletions

diff --git a/firmware/target/arm/imx233/kernel-imx233.c b/firmware/target/arm/imx233/kernel-imx233.c
index edb9b8366c..b59d3ed63f 100644
--- a/firmware/target/arm/imx233/kernel-imx233.c
+++ b/firmware/target/arm/imx233/kernel-imx233.c
@@ -21,6 +21,7 @@
 #include "kernel.h"
 #include "timrot-imx233.h"
 #include "clkctrl-imx233.h"
+#include "kernel-imx233.h"
 
 static void tick_timer(void)
 {
@@ -35,3 +36,45 @@ void tick_start(unsigned int interval_in_ms)
         HW_TIMROT_TIMCTRL__SELECT_1KHZ_XTAL, HW_TIMROT_TIMCTRL__PRESCALE_1,
         false, &tick_timer);
 }
+
+
+void arbiter_init(struct channel_arbiter_t *a, unsigned count)
+{
+    mutex_init(&a->mutex);
+    semaphore_init(&a->sema, count, count);
+    a->free_bm = (1 << count) - 1;
+    a->count = count;
+}
+
+// doesn't check in use !
+void arbiter_reserve(struct channel_arbiter_t *a, unsigned channel)
+{
+    // assume semaphore has a free slot immediately
+    if(semaphore_wait(&a->sema, TIMEOUT_NOBLOCK) != OBJ_WAIT_SUCCEEDED)
+        panicf("arbiter_reserve failed on semaphore_wait !");
+    mutex_lock(&a->mutex);
+    a->free_bm &= ~(1 << channel);
+    mutex_unlock(&a->mutex);
+}
+
+int arbiter_acquire(struct channel_arbiter_t *a, int timeout)
+{
+    int w = semaphore_wait(&a->sema, timeout);
+    if(w == OBJ_WAIT_TIMEDOUT)
+        return w;
+    mutex_lock(&a->mutex);
+    int chan = find_first_set_bit(a->free_bm);
+    if(chan >= a->count)
+        panicf("arbiter_acquire cannot find a free channel !");
+    a->free_bm &= ~(1 << chan);
+    mutex_unlock(&a->mutex);
+    return chan;
+}
+
+void arbiter_release(struct channel_arbiter_t *a, int channel)
+{
+    mutex_lock(&a->mutex);
+    a->free_bm |= 1 << channel;
+    mutex_unlock(&a->mutex);
+    semaphore_release(&a->sema);
+}