From eb0091403e6a862671a7002835f2f28c97dd3dff Mon Sep 17 00:00:00 2001
From: fef <owo@fef.moe>
Date: Thu, 28 Oct 2021 17:08:46 +0200
Subject: [PATCH] mutex: add semaphores

---
 include/gay/mutex.h | 20 ++++++++++++
 kernel/mutex.c      | 74 ++++++++++++++++++++++++++++++++++++++++++---
 2 files changed, 90 insertions(+), 4 deletions(-)

diff --git a/include/gay/mutex.h b/include/gay/mutex.h
index bbd01d5..e3c7fb0 100644
--- a/include/gay/mutex.h
+++ b/include/gay/mutex.h
@@ -46,6 +46,26 @@ void mtx_lock(struct mtx *mutex);
 void mtx_unlock(struct mtx *mutex);
 int mtx_trylock(struct mtx *mutex);
 
+struct sem {
+	atom_t count;
+	spin_t wait_queue_lock;
+	struct clist wait_queue; /* -> struct lock_waiter::clink */
+};
+
+#define SEM_DEFINE(name, initial_count) {			\
+	.count = ATOM_DEFINE(initial_count),			\
+	.wait_queue_lock = SPIN_DEFINE,				\
+	.wait_queue = CLIST_DEFINE((name).wait_queue),		\
+}
+
+#define SEM(name, initial_count) \
+	struct sem name = SEM_DEFINE(name, initial_count)
+
+void sem_init(struct sem *sem, int initial_count);
+int sem_down(struct sem *semaphore);
+int sem_up(struct sem *semaphore);
+int sem_trydown(struct sem *semaphore);
+
 /*
  * This file is part of GayBSD.
  * Copyright (c) 2021 fef <owo@fef.moe>.
diff --git a/kernel/mutex.c b/kernel/mutex.c
index f7cb223..3dd0818 100644
--- a/kernel/mutex.c
+++ b/kernel/mutex.c
@@ -94,13 +94,79 @@ void mtx_unlock(struct mtx *mtx)
 			clist_del_first_entry(&mtx->wait_queue, typeof(*waiter), clink);
 		spin_unlock(&mtx->wait_queue_lock);
 		waiter->task->state = TASK_READY;
-		switch_to(waiter->task, current);
-	} else {
-		atom_write(&mtx->lock, 0);
-		spin_unlock(&mtx->wait_queue_lock);
 	}
 }
 
+void sem_init(struct sem *sem, int initial_count)
+{
+	atom_write(&sem->count, initial_count);
+	spin_init(&sem->wait_queue_lock);
+	clist_init(&sem->wait_queue);
+}
+
+int sem_down(struct sem *sem)
+{
+#	ifdef DEBUG
+		if (in_irq()) {
+			kprintf("sem_down() called from IRQ context!\n");
+			spin_loop {
+				int old = atom_sub(&sem->count, 1);
+				if (old >= 0)
+					return old;
+				atom_inc(&sem->count);
+			}
+		}
+#	endif
+
+	int ret = atom_sub(&sem->count, 1);
+
+	if (ret < 0) {
+		struct task *task = current;
+		struct lock_waiter waiter = {
+			.task = task,
+		};
+
+		spin_lock(&sem->wait_queue_lock);
+		clist_add(&sem->wait_queue, &waiter.clink);
+		spin_unlock(&sem->wait_queue_lock);
+
+		task->state = TASK_BLOCKED;
+		schedule();
+		ret = 0;
+	}
+
+	return ret;
+}
+
+int sem_up(struct sem *sem)
+{
+	int ret = atom_add(&sem->count, 1);
+
+	if (ret <= 0) {
+		spin_lock(&sem->wait_queue_lock);
+		struct lock_waiter *waiter =
+			clist_del_first_entry(&sem->wait_queue, typeof(*waiter), clink);
+		spin_unlock(&sem->wait_queue_lock);
+
+		waiter->task->state = TASK_READY;
+		ret = 0;
+	}
+
+	return ret;
+}
+
+int sem_trydown(struct sem *sem)
+{
+	int ret = atom_sub(&sem->count, 1);
+
+	if (ret < 0) {
+		atom_inc(&sem->count);
+		ret = -EAGAIN;
+	}
+
+	return ret;
+}
+
 /*
  * This file is part of GayBSD.
  * Copyright (c) 2021 fef <owo@fef.moe>.