From 76b46ff484884e3ef01cbeb2cae571f89e61a0c0 Mon Sep 17 00:00:00 2001
From: Matthias Kruk <m@m10k.eu>
Date: Sun, 9 Feb 2020 00:45:07 +0900
Subject: [PATCH] klibc/sem: Add semaphore implementation

---
 config.h              |   1 +
 kernel/include/sem.h  |  18 +++++++
 kernel/klibc/Makefile |   2 +-
 kernel/klibc/sem.c    | 123 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 143 insertions(+), 1 deletion(-)
 create mode 100644 kernel/include/sem.h
 create mode 100644 kernel/klibc/sem.c

diff --git a/config.h b/config.h
index 6c9834b..ead8518 100644
--- a/config.h
+++ b/config.h
@@ -31,6 +31,7 @@
 #define CONFIG_USER_STACK_SIZE      4096
 #define CONFIG_PROC_MAXTASKS        8
 #define CONFIG_MUTEX_WAITQLEN       8
+#define CONFIG_SEM_WAITQLEN         8
 
 #define CONFIG_APIC     0
 
diff --git a/kernel/include/sem.h b/kernel/include/sem.h
new file mode 100644
index 0000000..87b39e5
--- /dev/null
+++ b/kernel/include/sem.h
@@ -0,0 +1,18 @@
+#ifndef __SEM_H
+#define __SEM_H
+
+#include <config.h>
+#include <spinlock.h>
+#include <arch.h>
+
+typedef struct {
+	spinlock_t sem_lock;
+	int        sem_count;
+	task_t     *sem_waitq[CONFIG_SEM_WAITQLEN];
+} sem_t;
+
+int sem_wait(sem_t*);
+int sem_trywait(sem_t*);
+int sem_post(sem_t*);
+
+#endif /* __SEM_H */
diff --git a/kernel/klibc/Makefile b/kernel/klibc/Makefile
index 0b67c09..40a2c94 100644
--- a/kernel/klibc/Makefile
+++ b/kernel/klibc/Makefile
@@ -1,6 +1,6 @@
 OUTPUT = klibc.a
 OBJECTS = string.o spinlock.o mutex.o posixcall.o coraxcall.o heap.o stdio.o signal.o \
-          kq.o
+          kq.o sem.o
 PHONY = clean
 INCLUDES = -I../include -I../../include -I../..
 CFLAGS = -m32 -Wall -nostdlib -nodefaultlibs -nostartfiles -ffreestanding $(INCLUDES)
diff --git a/kernel/klibc/sem.c b/kernel/klibc/sem.c
new file mode 100644
index 0000000..97acbaa
--- /dev/null
+++ b/kernel/klibc/sem.c
@@ -0,0 +1,123 @@
+#include <config.h>
+#include <corax/types.h>
+#include <corax/errno.h>
+#include <spinlock.h>
+#include <arch.h>
+#include <sched.h>
+#include <sem.h>
+
+inline static int _nq(sem_t *sem, task_t *t)
+{
+	int i;
+
+	for(i = 0; i < CONFIG_SEM_WAITQLEN; i++) {
+		if(!sem->sem_waitq[i]) {
+			sem->sem_waitq[i] = t;
+			return(i);
+		}
+	}
+
+	return(-ENOMEM);
+}
+
+inline static void _wakeup_all(sem_t *sem)
+{
+	int i;
+
+	for(i = 0; i < CONFIG_SEM_WAITQLEN; i++) {
+		if(sem->sem_waitq[i]) {
+			sched_wakeup(sem->sem_waitq[i], 0);
+		}
+	}
+
+	return;
+}
+
+int sem_wait(sem_t *sem)
+{
+	task_t *ctask;
+	int ret_val;
+	int slot;
+
+	ctask = task_get_current();
+	ret_val = -EAGAIN;
+	slot = -1;
+
+	do {
+		spinlock_lock(&(sem->sem_lock));
+
+		if(sem->sem_count > 0) {
+			/* count is positive - we can proceed */
+			sem->sem_count--;
+
+			/* if we had been in the queue, clean up our slot */
+			if(slot >= 0) {
+				sem->sem_waitq[slot] = NULL;
+			}
+
+			ret_val = 0;
+		} else {
+			/* we have to wait */
+
+			if(slot < 0) {
+				/* get a slot in the wait queue */
+				slot = _nq(sem, ctask);
+			}
+		}
+
+		spinlock_unlock(&(sem->sem_lock));
+
+		if(slot >= 0) {
+			/*
+			 * If we have a valid slot, go to bed and wait for
+			 * someone to wake us up.
+			 */
+			sched_sleep(NULL);
+		}
+
+	} while(ret_val < 0);
+
+	return(ret_val);
+}
+
+int sem_trywait(sem_t *sem)
+{
+	int ret_val;
+
+	ret_val = -EAGAIN;
+
+	spinlock_lock(&(sem->sem_lock));
+
+	if(sem->sem_count > 0) {
+		sem->sem_count--;
+		ret_val = 0;
+	}
+
+	spinlock_unlock(&(sem->sem_lock));
+
+	return(ret_val);
+}
+
+int sem_post(sem_t *sem)
+{
+	int new_count;
+	int ret_val;
+
+	ret_val = -EOVERFLOW;
+
+	spinlock_lock(&(sem->sem_lock));
+
+	new_count = sem->sem_count + 1;
+
+	/* make sure the counter doesn't overflow */
+	if(new_count > sem->sem_count) {
+		sem->sem_count = new_count;
+		_wakeup_all(sem);
+
+		ret_val = 0;
+	}
+
+	spinlock_unlock(&(sem->sem_lock));
+
+	return(ret_val);
+}
-- 
2.47.3