Semaphore complete

src/kernel/include/scheduler.h

@@ -11,5 +11,4 @@ void scheduler_tick(void);
 void scheduler_select_next_task(void);
 void task_delay(uint32_t ticks);
 
-
 #endif

src/kernel/include/semaphore.h

@@ -1,4 +1,16 @@
 #ifndef SEMAPHORE_H
 #define SEMAPHORE_H
 
+#include "task.h"
+#include <stdint.h>
+typedef struct {
+    Task *waiting;
+    uint8_t max_count;
+    uint8_t count;
+} Semaphore;
+
+void semaphore_init(Semaphore *semaphore, uint8_t max_count);
+void semaphore_wait(Semaphore *semaphore);
+void semaphore_signal(Semaphore *semaphore);
+
 #endif

src/kernel/src/semaphore.c

@@ -0,0 +1,32 @@
+#include "semaphore.h"
+#include "scheduler.h"
+#include "port.h"
+#include <stddef.h>
+
+void semaphore_init(Semaphore *semaphore, uint8_t max_count) {
+    semaphore->waiting = NULL;
+    semaphore->count = 0;
+    semaphore->max_count = max_count;
+}
+
+void semaphore_wait(Semaphore *semaphore) {
+    while (semaphore->count == 0) {
+        semaphore->waiting = current_task;
+        current_task->state = BLOCKED;
+        port_trigger_context_switch();
+    }
+
+    semaphore->count -= 1;
+}
+
+void semaphore_signal(Semaphore *semaphore) {
+    if (semaphore->count < semaphore->max_count) {
+        semaphore->count += 1;
+    }
+
+    if (semaphore->waiting != NULL) {
+        semaphore->waiting->state = READY;
+        semaphore->waiting = NULL;
+        port_trigger_context_switch();
+    }
+}