From c1c84cffbb1b12739edacf4aecd9eb57a57026f0 Mon Sep 17 00:00:00 2001
From: Matheus <matheusuky@gmail.com>
Date: Wed, 3 May 2023 21:07:43 -0300
Subject: [PATCH] kernel: semaphores

---
 kernel-src/include/semaphore.h | 28 ++++++++++++++
 kernel-src/lib/semaphore.c     | 67 ++++++++++++++++++++++++++++++++++
 2 files changed, 95 insertions(+)
 create mode 100644 kernel-src/include/semaphore.h
 create mode 100644 kernel-src/lib/semaphore.c

diff --git a/kernel-src/include/semaphore.h b/kernel-src/include/semaphore.h
new file mode 100644
index 00000000..c8d4e367
--- /dev/null
+++ b/kernel-src/include/semaphore.h
@@ -0,0 +1,28 @@
+#ifndef _SEMAPHORE_H
+#define _SEMAPHORE_H
+
+#include <stdbool.h>
+#include <spinlock.h>
+#include <kernel/scheduler.h>
+
+#define SEM_TAIL 0
+#define SEM_HEAD 1
+
+typedef struct {
+	int i;
+	spinlock_t lock;
+	thread_t *tail;
+	thread_t *head;
+} semaphore_t;
+
+#define SEMAPHORE_INIT(x, v) { \
+		(x)->val = v; \
+		SPINLOCK_INIT((x)->lock); \
+		(x)->list = NULL; \
+	}
+
+int semaphore_wait(semaphore_t *sem, bool interruptible);
+void semaphore_signal(semaphore_t *sem);
+bool semaphore_test(semaphore_t *sem);
+
+#endif
diff --git a/kernel-src/lib/semaphore.c b/kernel-src/lib/semaphore.c
new file mode 100644
index 00000000..a934d2c1
--- /dev/null
+++ b/kernel-src/lib/semaphore.c
@@ -0,0 +1,67 @@
+#include <semaphore.h>
+#include <errno.h>
+#include <logging.h>
+
+static void insert(semaphore_t *sem, thread_t *thread) {
+	if (sem->head == NULL) {
+		sem->head = thread;
+		sem->tail = thread;
+		thread->next = NULL;
+		thread->prev = NULL;
+		return;
+	}
+
+	sem->tail->next = _cpu()->thread;
+	_cpu()->thread->prev = sem->tail;
+}
+
+static thread_t *get(semaphore_t *sem) {
+	thread_t *thread = sem->head;
+	__assert(thread);
+
+	sem->head = sem->head->next;
+	if (sem->head == NULL)
+		sem->tail = NULL;
+	else 
+		sem->head->prev = NULL;
+
+	return thread;
+}
+
+int semaphore_wait(semaphore_t *sem, bool interruptible) {
+	int ret = 0;
+	spinlock_acquire(&sem->lock);
+
+	if (--sem->i < 0) {
+		insert(sem, _cpu()->thread);
+		sched_preparesleep(interruptible);
+		spinlock_release(&sem->lock);
+		return sched_yield();
+	}
+
+	spinlock_release(&sem->lock);
+	return ret;
+}
+
+void semaphore_signal(semaphore_t *sem) {
+	spinlock_acquire(&sem->lock);
+	if (++sem->i <= 0) {
+		thread_t *thread = get(sem);
+		// XXX the current method doesn't respect thread priorities
+		sched_wakeup(thread, 0);
+	}
+	spinlock_release(&sem->lock);
+}
+
+bool semaphore_test(semaphore_t *sem) {
+	bool ret = false;
+	spinlock_acquire(&sem->lock);
+
+	if (sem->i > 0) {
+		--sem->i;
+		ret = true;
+	}
+
+	spinlock_release(&sem->lock);
+	return ret;
+}