kernel: finish implementation of private and shared futexes

kernel/futex.c

@@ -1,13 +1,13 @@
 #include <kernel/address-space.h>
 #include <kernel/futex.h>
+#include <kernel/sched.h>
+#include <kernel/task.h>
 #include <mango/status.h>
 
-enum futex_flags {
-	FUTEX_CREATE = 0x01u,
-};
+#define FUTEX_CREATE 0x40u
 
-static struct btree futex_list = {0};
-static spin_lock_t futex_list_lock = SPIN_LOCK_INIT;
+static struct btree shared_futex_list = {0};
+static spin_lock_t shared_futex_list_lock = SPIN_LOCK_INIT;
 
 static struct vm_cache futex_cache = {
 	.c_name = "futex",
@@ -15,7 +15,7 @@ static struct vm_cache futex_cache = {
 };
 
 BTREE_DEFINE_SIMPLE_INSERT(struct futex, f_node, f_key, put_futex)
-BTREE_DEFINE_SIMPLE_GET(struct futex, futex_key_t, f_node, f_key, get_futex)
+BTREE_DEFINE_SIMPLE_GET(struct futex, uintptr_t, f_node, f_key, get_futex)
 
 kern_status_t futex_init(void)
 {
@@ -23,31 +23,72 @@ kern_status_t futex_init(void)
 	return KERN_OK;
 }
 
-static struct futex *get_data(futex_key_t key, enum futex_flags flags)
+static kern_status_t get_data(
+	futex_key_t key,
+	unsigned int flags,
+	struct futex **out,
+	spin_lock_t **out_lock,
+	unsigned long *irq_flags)
 {
-	spin_lock(&futex_list_lock);
-	struct futex *futex = get_futex(&futex_list, key);
-	spin_unlock(&futex_list_lock);
+	spin_lock_t *lock = NULL;
+	struct btree *futex_list = NULL;
+	if (flags & FUTEX_PRIVATE) {
+		struct task *self = current_task();
+		lock = &self->t_base.ob_lock;
+		futex_list = &self->t_futex;
+	} else if (flags & FUTEX_SHARED) {
+		lock = &shared_futex_list_lock;
+		futex_list = &shared_futex_list;
+	} else {
+		return KERN_INVALID_ARGUMENT;
+	}
+
+	spin_lock_irqsave(lock, irq_flags);
+	struct futex *futex = get_futex(futex_list, key);
 
 	if (!futex && !(flags & FUTEX_CREATE)) {
-		return NULL;
+		spin_unlock_irqrestore(lock, *irq_flags);
+		return KERN_NO_ENTRY;
 	}
 
 	futex = vm_cache_alloc(&futex_cache, VM_NORMAL);
+	if (!futex) {
+		spin_unlock_irqrestore(lock, *irq_flags);
+		return KERN_NO_MEMORY;
+	}
+
 	futex->f_key = key;
 
-	spin_lock(&futex_list_lock);
-	put_futex(&futex_list, futex);
-	spin_unlock(&futex_list_lock);
+	put_futex(futex_list, futex);
 
-	return futex;
+	*out = futex;
+	*out_lock = lock;
+	return KERN_OK;
 }
 
-kern_status_t futex_get(
-	struct address_space *space,
-	kern_futex_t *futex,
-	futex_key_t *out)
+static kern_status_t cleanup_data(struct futex *futex, unsigned int flags)
 {
+	struct btree *futex_list = NULL;
+	if (flags & FUTEX_PRIVATE) {
+		struct task *self = current_task();
+		futex_list = &self->t_futex;
+	} else if (flags & FUTEX_SHARED) {
+		futex_list = &shared_futex_list;
+	} else {
+		return KERN_INVALID_ARGUMENT;
+	}
+
+	btree_delete(futex_list, &futex->f_node);
+	vm_cache_free(&futex_cache, futex);
+
+	return KERN_OK;
+}
+
+static kern_status_t futex_get_shared(kern_futex_t *futex, futex_key_t *out)
+{
+	struct task *self = current_task();
+	struct address_space *space = self->t_address_space;
+
 	unsigned long flags;
 	address_space_lock_irqsave(space, &flags);
 	kern_status_t status = address_space_translate(
@@ -59,12 +100,112 @@ kern_status_t futex_get(
 	return status;
 }
 
-kern_status_t futex_wait(futex_key_t futex, kern_futex_t new_val)
+static kern_status_t futex_get_private(kern_futex_t *futex, futex_key_t *out)
 {
-	return KERN_UNIMPLEMENTED;
+	*out = (futex_key_t)futex;
+	return KERN_OK;
 }
 
-kern_status_t futex_wake(futex_key_t futex, size_t nwaiters)
+kern_status_t futex_get(
+	kern_futex_t *futex,
+	futex_key_t *out,
+	unsigned int flags)
 {
-	return KERN_UNIMPLEMENTED;
+	if (flags & FUTEX_PRIVATE) {
+		return futex_get_private(futex, out);
+	}
+
+	if (flags & FUTEX_SHARED) {
+		return futex_get_shared(futex, out);
+	}
+
+	return KERN_INVALID_ARGUMENT;
+}
+
+static kern_status_t futex_read(
+	struct futex *futex,
+	unsigned int flags,
+	kern_futex_t *out)
+{
+	if (flags & FUTEX_PRIVATE) {
+		virt_addr_t addr = futex->f_key;
+		*out = *(kern_futex_t *)addr;
+		return KERN_OK;
+	}
+
+	if (flags & FUTEX_SHARED) {
+		phys_addr_t paddr = futex->f_key;
+		virt_addr_t vaddr = (virt_addr_t)vm_phys_to_virt(paddr);
+		if (!vaddr) {
+			return KERN_MEMORY_FAULT;
+		}
+
+		*out = *(kern_futex_t *)vaddr;
+		return KERN_OK;
+	}
+
+	return KERN_INVALID_ARGUMENT;
+}
+
+kern_status_t futex_wait(
+	futex_key_t key,
+	kern_futex_t new_val,
+	unsigned int flags)
+{
+	spin_lock_t *lock = NULL;
+	unsigned long irq_flags = 0;
+	struct futex *futex = NULL;
+	kern_status_t status = get_data(key, flags, &futex, &lock, &irq_flags);
+	if (status != KERN_OK) {
+		return status;
+	}
+
+	kern_futex_t current_val = 0;
+	status = futex_read(futex, flags, &current_val);
+	if (status != KERN_OK) {
+		spin_unlock_irqrestore(lock, irq_flags);
+		return status;
+	}
+
+	if (current_val != new_val) {
+		spin_unlock_irqrestore(lock, irq_flags);
+		return KERN_BAD_STATE;
+	}
+
+	struct wait_item waiter;
+	thread_wait_begin(&waiter, &futex->f_waiters);
+	spin_unlock_irqrestore(lock, irq_flags);
+
+	schedule(SCHED_NORMAL);
+
+	spin_lock_irqsave(lock, &irq_flags);
+	thread_wait_end(&waiter, &futex->f_waiters);
+
+	if (waitqueue_empty(&futex->f_waiters)) {
+		cleanup_data(futex, flags);
+	}
+
+	spin_unlock_irqrestore(lock, irq_flags);
+
+	return KERN_OK;
+}
+
+kern_status_t futex_wake(futex_key_t key, size_t nwaiters, unsigned int flags)
+{
+	spin_lock_t *lock = NULL;
+	unsigned long irq_flags = 0;
+	struct futex *futex = NULL;
+	kern_status_t status = get_data(key, flags, &futex, &lock, &irq_flags);
+	if (status != KERN_OK) {
+		return status;
+	}
+
+	if (nwaiters == FUTEX_WAKE_ALL) {
+		wakeup_queue(&futex->f_waiters);
+	} else {
+		wakeup_n(&futex->f_waiters, nwaiters);
+	}
+
+	spin_unlock_irqrestore(lock, irq_flags);
+	return KERN_OK;
 }