From c3b4d65a36a1c81d8dba33ad4e978312468d56af Mon Sep 17 00:00:00 2001
From: Mathieu Maret <mathieu.maret@gmail.com>
Date: Tue, 9 Nov 2021 20:13:40 +0100
Subject: [PATCH] Add semaphore

---
 core/synchro.c | 60 ++++++++++++++++++++++++++++++++++++++++++++++----
 core/synchro.h |  6 +++++
 core/wait.c    | 24 +++++++++++++++++++-
 core/wait.h    |  5 +++--
 4 files changed, 88 insertions(+), 7 deletions(-)

diff --git a/core/synchro.c b/core/synchro.c
index ce2f5cd..f9ab375 100644
--- a/core/synchro.c
+++ b/core/synchro.c
@@ -6,7 +6,6 @@
 #include "list.h"
 #include "time.h"
 
-
 int mutexInit(struct mutex *m)
 {
 
@@ -16,8 +15,6 @@ int mutexInit(struct mutex *m)
         return ENOMEM;
 
     waitQueueInit(m->wait);
-    list_init(m->wait->thread);
-
 
     return 0;
 }
@@ -73,9 +70,64 @@ int mutexUnlock(struct mutex *m)
 
     m->owner = NULL;
 
-    wakeUp(m->wait);
+    waitUp(m->wait);
 
     restore_IRQs(flags);
 
     return 0;
 }
+
+int semInit(struct semaphore *sem, int initVal)
+{
+    sem->count = initVal;
+    waitQueueInit(&sem->wait);
+    return 0;
+}
+
+int semFree(struct semaphore *sem)
+{
+    return waitQueueFree(&sem->wait);
+}
+
+int semDown(struct semaphore *sem, unsigned long timeout)
+{
+    uint32_t flags;
+    int ret = 0;
+
+    disable_IRQs(flags);
+    sem->count--;
+    if (sem->count < 0) {
+        ret = waitTimeout(&sem->wait, timeout);
+        if (ret) {
+            sem->count++;
+        }
+    }
+    restore_IRQs(flags);
+    return ret;
+}
+
+int semTryDown(struct semaphore *sem)
+{
+    uint32_t flags;
+    int ret = -EBUSY;
+
+    disable_IRQs(flags);
+    if (sem->count > 1) {
+        ret = 0;
+        sem->count--;
+    }
+    restore_IRQs(flags);
+    return ret;
+}
+
+int semUp(struct semaphore *sem)
+{
+    uint32_t flags;
+    int ret = 0;
+
+    disable_IRQs(flags);
+    sem->count++;
+    ret = waitUpNb(&sem->wait, 1);
+    restore_IRQs(flags);
+    return ret;
+}
diff --git a/core/synchro.h b/core/synchro.h
index 107cb43..f3e935c 100644
--- a/core/synchro.h
+++ b/core/synchro.h
@@ -5,3 +5,9 @@ int mutexInit(struct mutex *);
 int mutexFree(struct mutex *);
 int mutexLock(struct mutex *);
 int mutexUnlock(struct mutex *);
+
+int semInit(struct semaphore *, int initVal);
+int semFree(struct semaphore *);
+int semDown(struct semaphore *, unsigned long timeout);
+int semTryDown(struct semaphore *);
+int semUp(struct semaphore *);
diff --git a/core/wait.c b/core/wait.c
index c6281d1..cfa6776 100644
--- a/core/wait.c
+++ b/core/wait.c
@@ -8,6 +8,7 @@ static struct wait_queue *waitQueues = NULL;
 int waitQueueInit(struct wait_queue *wq)
 {
     uint32_t flags;
+    list_init(wq->thread);
     disable_IRQs(flags);
     list_add_tail(waitQueues, wq);
     restore_IRQs(flags);
@@ -23,7 +24,7 @@ int waitQueueFree(struct wait_queue *wq)
     return 0;
 }
 
-int wakeUp(struct wait_queue *wq)
+int waitUp(struct wait_queue *wq)
 {
     struct thread *th;
     uint32_t flags;
@@ -39,6 +40,27 @@ int wakeUp(struct wait_queue *wq)
     return 0;
 }
 
+int waitUpNb(struct wait_queue *wq, unsigned int nbThread)
+{
+    struct thread *th;
+    uint32_t flags;
+
+    disable_IRQs(flags);
+    list_collapse(wq->thread, th)
+    {
+        threadAddThread(th);
+        if(nbThread){
+            nbThread--;
+            if(!nbThread)
+                break;
+        }
+    }
+
+    restore_IRQs(flags);
+
+    return 0;
+}
+
 int wait(struct wait_queue *wq)
 {
     return waitTimeout(wq, 0);
diff --git a/core/wait.h b/core/wait.h
index f0d0212..be0ebd3 100644
--- a/core/wait.h
+++ b/core/wait.h
@@ -20,7 +20,8 @@ int waitQueueInit(struct wait_queue *);
 int waitQueueFree(struct wait_queue *);
 int wait(struct wait_queue *);
 int waitTimeout(struct wait_queue *wq, unsigned long msec);
-int wakeUp(struct wait_queue *);
+int waitUp(struct wait_queue *);
+int waitUpNb(struct wait_queue *wq, unsigned int nbThread);
 
 #define wait_event(wq, condition)                                                             \
     do {                                                                                      \
@@ -31,7 +32,7 @@ int wakeUp(struct wait_queue *);
 
 struct semaphore {
     int count;
-    struct wait_queue *wait;
+    struct wait_queue wait;
 };
 
 struct mutex {