[kernel] Add semaphores and tests

app/tests/thread_tests.c

@@ -26,6 +26,7 @@
 #include <app/tests.h>
 #include <kernel/thread.h>
 #include <kernel/mutex.h>
+#include <kernel/semaphore.h>
 #include <kernel/event.h>
 #include <platform.h>
 
@@ -46,6 +47,82 @@ int sleep_test(void)
 	return 0;
 }
 
+static semaphore_t sem;
+static const int sem_total_its = 10000;
+static const int sem_thread_max_its = 1000;
+static const int sem_start_value = 10;
+static int sem_remaining_its = 0;
+static int sem_threads = 0;
+static mutex_t sem_test_mutex;
+
+static int semaphore_producer()
+{
+	printf("semaphore producer %p starting up, running for %d iterations\n", current_thread, sem_total_its);
+
+	for (int x = 0; x < sem_total_its; x++) {
+		sem_post(&sem);
+	}
+
+	return 0;
+}
+
+static int semaphore_consumer()
+{
+	unsigned int iterations = 0;
+	
+	mutex_acquire(&sem_test_mutex);
+	if (sem_remaining_its >= sem_thread_max_its) {
+		iterations = rand();
+		iterations %= sem_thread_max_its;
+	} else {
+		iterations = sem_remaining_its;
+	}
+	sem_remaining_its -= iterations;
+	mutex_release(&sem_test_mutex);
+
+	printf("semaphore consumer %p starting up, running for %u iterations\n", current_thread, iterations);
+	for (unsigned int x = 0; x < iterations; x++)
+		sem_wait(&sem);
+	printf("semaphore consumer %p done\n", current_thread);
+	atomic_add(&sem_threads, -1);
+	return 0;
+}
+
+static int semaphore_test()
+{
+	sem_init(&sem, sem_start_value);
+	mutex_init(&sem_test_mutex);
+
+	sem_remaining_its = sem_total_its;
+	while (1) {
+		mutex_acquire(&sem_test_mutex);
+		if (sem_remaining_its) {
+			thread_resume(thread_create("semaphore consumer", &semaphore_consumer, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+			atomic_add(&sem_threads, 1);
+		} else {
+			mutex_release(&sem_test_mutex);
+			break;
+		}
+		mutex_release(&sem_test_mutex);
+	}
+	
+	thread_resume(thread_create("semaphore producer", &semaphore_producer, NULL, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
+
+	while (sem_threads)
+		thread_yield();
+		
+	if (sem.count == sem_start_value)
+		printf("semaphore tests successfully complete\n");
+	else
+		printf("semaphore tests failed: %d != %d\n", sem.count, sem_start_value);
+
+	sem_destroy(&sem);
+	mutex_destroy(&sem_test_mutex);
+
+	return 0;
+}
+
+
 static volatile int shared = 0;
 static mutex_t m;
 static volatile int mutex_thread_count = 0;
@@ -364,6 +441,7 @@ static void preempt_test(void)
 int thread_tests(void) 
 {
 	mutex_test();
+	semaphore_test();
 	event_test();
 
 	atomic_test();

include/kernel/semaphore.h

@@ -0,0 +1,20 @@
+#ifndef __KERNEL_SEMAPHORE_H
+#define __KERNEL_SEMAPHORE_H
+
+#include <kernel/thread.h>
+#include <kernel/mutex.h>
+
+#define SEMAPHORE_MAGIC 'sema'
+
+typedef struct semaphore {
+	int magic;
+	int count;
+	wait_queue_t wait;
+} semaphore_t;
+
+void sem_init(semaphore_t *, unsigned int);
+void sem_destroy(semaphore_t *);
+status_t sem_post(semaphore_t *);
+status_t sem_wait(semaphore_t *);
+status_t sem_trywait(semaphore_t *);
+#endif

kernel/rules.mk

@@ -14,6 +14,8 @@ MODULE_SRCS := \
 	$(LOCAL_DIR)/main.c \
 	$(LOCAL_DIR)/mutex.c \
 	$(LOCAL_DIR)/thread.c \
-	$(LOCAL_DIR)/timer.c
+	$(LOCAL_DIR)/timer.c \
+	$(LOCAL_DIR)/semaphore.c \
+	
 
 include make/module.mk

kernel/semaphore.c

@@ -0,0 +1,66 @@
+#include <debug.h>
+#include <err.h>
+#include <kernel/semaphore.h>
+#include <kernel/thread.h>
+
+void sem_init(semaphore_t *sem, unsigned int value)
+{
+	sem->magic = SEMAPHORE_MAGIC;
+	sem->count = value;
+	wait_queue_init(&sem->wait);
+}
+
+void sem_destroy(semaphore_t *sem)
+{
+	enter_critical_section();
+	sem->count = 0;
+	wait_queue_destroy(&sem->wait, true);
+	exit_critical_section();
+}
+
+status_t sem_post(semaphore_t *sem)
+{
+	status_t ret = NO_ERROR;
+	enter_critical_section();
+
+	/*
+	 * If the count is or was negative then a thread is waiting for a resource, otherwise
+	 * it's safe to just increase the count available with no downsides
+	 */
+	if (++sem->count <= 0)
+		wait_queue_wake_one(&sem->wait, true, NO_ERROR);
+
+	exit_critical_section();
+	return ret;
+}
+
+status_t sem_wait(semaphore_t *sem)
+{
+	status_t ret = NO_ERROR;
+	enter_critical_section();
+
+	/* 
+	 * If there are no resources available then we need to 
+	 * sit in the wait queue until sem_post adds some. 
+	 */
+	if (--sem->count < 0)
+		ret = wait_queue_block(&sem->wait, INFINITE_TIME);
+
+	exit_critical_section();
+	return ret;
+}
+
+status_t sem_trywait(semaphore_t *sem)
+{
+	status_t ret = NO_ERROR;
+	enter_critical_section();
+
+	if (sem->count <= 0)
+		ret = ERR_NOT_READY;
+	else
+		sem->count--;
+	
+	exit_critical_section();
+	return ret;
+}
+