[PATCH] kern: simple futex for gnumach (version 4)

[Marin Ramesa]

GPL license included, more bugs fixed and RPCs defined.

---
 Makefrag.am               |   2 +
 include/mach/gnumach.defs |  26 +++
 kern/futex.c              | 405 ++++++++++++++++++++++++++++++++++++++++++++++
 kern/futex.h              | 108 +++++++++++++
 4 files changed, 541 insertions(+)
 create mode 100644 kern/futex.c
 create mode 100644 kern/futex.h

diff --git a/Makefrag.am b/Makefrag.am
index c1387bd..e43f882 100644
--- a/Makefrag.am
+++ b/Makefrag.am
@@ -145,6 +145,8 @@ libkernel_a_SOURCES += \
        kern/eventcount.h \
        kern/exception.c \
        kern/exception.h \
+       kern/futex.c \
+       kern/futex.h \
        kern/host.c \
        kern/host.h \
        kern/ipc_host.c \
diff --git a/include/mach/gnumach.defs b/include/mach/gnumach.defs
index 12c4e99..8c5ea4b 100644
--- a/include/mach/gnumach.defs
+++ b/include/mach/gnumach.defs
@@ -63,3 +63,29 @@ simpleroutine thread_terminate_release(
                reply_port      : mach_port_name_t;
                address         : vm_address_t;
                size            : vm_size_t);
+
+/*
+ * When operation equals FUTEX_WAIT, this is a method for a thread to wait for 
a 
+ * change of value at a given address. 
+ *
+ * When operation equals FUTEX_WAKE, this is a method for waking up a number
+ * (specified by the argument value) of threads waiting for a change of value
+ * at a given address (threads that are in FUTEX_WAIT).
+ */
+
+simpleroutine futex_rpc(
+               thread          : thread_t;
+               address         : pointer_t;
+               value           : int;
+               operation       : int);
+
+simpleroutine futex_wait_rpc(
+               thread          : thread_t;
+               address         : pointer_t;
+               value           : int);
+
+simpleroutine futex_wake_rpc(
+               thread          : thread_t;
+               address         : pointer_t;
+               thread_num      : int);
+
diff --git a/kern/futex.c b/kern/futex.c
new file mode 100644
index 0000000..7860265
--- /dev/null
+++ b/kern/futex.c
@@ -0,0 +1,405 @@
+/*
+ * Copyright (C) 2013 Free Software Foundation, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *     Author: Marin Ramesa
+ */
+
+/*
+ * Fast userspace locking
+ *
+ */
+
+#include <kern/futex.h>
+#include <kern/kalloc.h>
+
+static futex_hash_table_t table = NULL;
+static unsigned int prev_hash_val = 0; 
+
+/*
+ * When operation equals FUTEX_WAIT, this is a method for a thread to wait for 
a 
+ * change of value at a given address. 
+ *
+ * When operation equals FUTEX_WAKE, this is a method for waking up a number
+ * (specified by the argument value) of threads waiting for a change of value
+ * at a given address (threads that are in FUTEX_WAIT).
+ *
+ * Return values:
+ *     FUTEX_RESUMED_BY_WAKE - thread has been resumed by FUTEX_WAKE
+ *     FUTEX_NO_THREAD_SUSPENDED - value at the address has been changed while 
inside 
+ *             FUTEX_WAIT and no thread has been suspended
+ *     FUTEX_SOME_NUMBER_RESUMED - value threads have been resumed by 
FUTEX_WAKE
+ *     FUTEX_NO_THREAD - FUTEX_WAKE did not found a suspended thread at the 
passed address
+ *     FUTEX_EXISTS - futex already exists at the new address
+ *     FUTEX_MEMORY_ERROR - memory error
+ *     FUTEX_UNKNOWN_OPERATION - unknown operation
+ *     FUTEX_RESOURCE_SHORTAGE - no virtual memory or the mutex is not 
per-process
+ *
+ */
+int futex(int *address, int value, int operation)
+{
+       switch (operation) {
+               case FUTEX_WAIT:
+                       return futex_wait(address, value);
+               case FUTEX_WAKE:
+                       return futex_wake(address, value);
+               default:
+                       return FUTEX_UNKNOWN_OPERATION;
+       }       
+}
+
+int futex_init(int *address, futex_t futex, futex_hash_table_t hash_table, 
unsigned int hash_val)
+{
+       simple_lock_init(&futex->futex_wait_lock);
+
+       if (hash_table == NULL) {
+               hash_table = futex_hash_table_init(INITIAL_HASH_TABLE_SIZE, 
current_thread()->task);
+               if (hash_table == NULL)
+                       return FUTEX_RESOURCE_SHORTAGE;
+       }
+
+       if ((hash_table->futex_elements = 
+               (futex_t)kalloc((hash_table->size + 1) * sizeof(struct futex))) 
== NULL) {
+
+               return FUTEX_RESOURCE_SHORTAGE;
+
+       }
+
+       futex->address = address;
+       futex->num_futexed_threads = 0;
+       futex->next_futex = &(hash_table->futex_elements[hash_val]);
+       futex->prev_futex = &(hash_table->futex_elements[prev_hash_val]);
+
+       if ((vm_map_lookup(futex->map, (vm_offset_t)address, VM_PROT_READ, 
futex->version, futex->object,
+                               futex->offset, futex->out_prot, futex->wired) 
!= KERN_SUCCESS)) {
+               hash_table->size++;
+               unsigned int temp_hash_val = futex_hash(hash_table, address);
+               
kfree((vm_offset_t)&(hash_table->futex_elements[temp_hash_val]), sizeof(struct 
futex));
+               hash_table->size--;
+               return FUTEX_MEMORY_ERROR;
+       }
+
+       hash_table->size++;
+
+       return 0;       
+}
+
+int futex_wait(int *address, int value)
+{
+       futex_t futex;
+       boolean_t per_process_mutex;
+
+       lookup:
+
+       per_process_mutex = TRUE;
+
+       futex = futex_hash_table_lookup_address(table, address, 
&per_process_mutex);
+
+       if (per_process_mutex == FALSE)
+               return FUTEX_RESOURCE_SHORTAGE;
+
+       if (futex != NULL) {
+
+               simple_lock(&futex->futex_wait_lock);
+
+               /* If the value is still the same. */
+               if (*address == value) {
+
+                       /* Add the thread to the futex. */
+                       if ((futex->futexed_threads = 
+                               
(thread_t)kalloc((futex->num_futexed_threads+1)*sizeof(struct thread))) == 
NULL) {
+                               simple_unlock(&futex->futex_wait_lock);
+                               return FUTEX_RESOURCE_SHORTAGE;
+                       }
+
+                       futex->futexed_threads[++futex->num_futexed_threads] = 
*(current_thread());
+
+                       goto suspend;
+               } else 
+                       goto no_suspend;
+
+       } else {
+               
+               int i = futex_hash_table_add_address(table, address); 
+
+               if (i == 1) return FUTEX_RESOURCE_SHORTAGE;
+               if (i == 2) return FUTEX_EXISTS;
+               if (i != 0) return i;
+               
+               goto lookup;
+
+       }
+
+       suspend:
+               simple_unlock(&futex->futex_wait_lock);
+               thread_suspend(current_thread());
+               return FUTEX_RESUMED_BY_WAKE;
+
+       no_suspend:
+               simple_unlock(&futex->futex_wait_lock);
+               return FUTEX_NO_THREAD_SUSPENDED;
+}              
+
+int futex_wake(int *address, int thread_num)
+{
+       futex_t futex, temp_futex;
+
+       unsigned int hash_val = futex_hash(table, address);
+       boolean_t per_process_mutex = TRUE;
+
+       temp_futex = futex_hash_table_lookup_address(table, address, 
&per_process_mutex);
+
+       if (per_process_mutex == FALSE)
+               return FUTEX_RESOURCE_SHORTAGE;
+
+       if (temp_futex != NULL) {
+
+               for (futex = &(table->futex_elements[hash_val]); futex != NULL; 
+                       futex = futex->next_futex) {
+
+                       if ((*(futex->offset) == *(futex->next_futex->offset)) 
&&
+                               (futex->object == futex->next_futex->object)) {
+                       
+                               (void) futex_wake(futex->next_futex->address, 
thread_num);
+
+                       }
+               }
+
+               for (futex = &(table->futex_elements[hash_val]); futex != NULL; 
+                       futex = futex->prev_futex) {
+
+                       if ((*(futex->offset) == *(futex->prev_futex->offset)) 
&&
+                               (futex->object == futex->prev_futex->object)) {
+
+                               (void) futex_wake(futex->prev_futex->address, 
thread_num);
+
+                       }
+               }
+
+       } else
+               goto no_thread;
+
+       futex = futex_hash_table_lookup_address(table, address, 
&per_process_mutex);
+
+       if (per_process_mutex == FALSE)
+               return FUTEX_RESOURCE_SHORTAGE;
+
+       if (futex != NULL) {
+
+               simple_lock(&futex->futex_wait_lock);
+
+               /* If the number of threads to be awakened is greater then the 
number
+                * of threads in the futex, wake all.
+                */
+               if (thread_num > futex->num_futexed_threads)
+                       thread_num = futex->num_futexed_threads - 1;
+               int i;
+               vm_size_t size_free = 0;
+               for (i = 0; i < thread_num; i++) {
+                       thread_resume(&(futex->futexed_threads[i]));
+                       futex->num_futexed_threads--;
+                       size_free++;
+               }
+               kfree((vm_offset_t)&(futex->futexed_threads[i]), size_free * 
sizeof(struct thread));
+       
+               if (futex->num_futexed_threads == 0)
+                       if (futex_hash_table_delete_futex(futex, table, 
address) == -1) {
+                               simple_unlock(&futex->futex_wait_lock);
+                               return FUTEX_RESOURCE_SHORTAGE;
+                       }
+
+               simple_unlock(&futex->futex_wait_lock);
+
+               return FUTEX_SOME_NUMBER_RESUMED;
+       } else {
+               no_thread:
+                       return FUTEX_NO_THREAD;
+       }
+}      
+
+futex_hash_table_t futex_hash_table_init(size_t size, task_t task)
+{
+       /* If we're still in the same task. */
+       if (current_thread()->task == task) {
+
+               futex_hash_table_t new_table;
+
+               simple_lock_init(&new_table->futex_hash_table_lock);
+
+               if ((new_table = 
+                       (futex_hash_table_t)kalloc(sizeof(struct 
futex_hash_table))) == NULL)
+                       return NULL;
+
+               if ((new_table->futex_elements = 
+                       (futex_t)kalloc(size * sizeof(struct futex))) == NULL)
+                       return NULL;
+
+               new_table->size = size;
+
+               new_table->task = task;
+
+               return new_table;
+       } else
+               return NULL;
+}
+
+unsigned int futex_hash(futex_hash_table_t hash_table, int *address)
+{
+       unsigned int hash_val = 0;
+
+       hash_val = (unsigned int)address + (hash_val << 5) - hash_val;
+
+       if (hash_table != NULL)
+               return hash_val % hash_table->size;
+       else
+               return 0;
+}
+
+futex_t futex_hash_table_lookup_address(futex_hash_table_t hash_table, int 
*address, 
+                                        boolean_t *per_process_mutex)
+{
+       futex_t futex;
+
+       if (hash_table == NULL)
+               return NULL;
+
+       if (current_thread()->task == hash_table->task) {
+
+               simple_lock(&hash_table->futex_hash_table_lock);
+
+               unsigned int hash_val = futex_hash(hash_table, address);
+
+               for (futex = &(hash_table->futex_elements[hash_val]); futex != 
NULL; 
+                       futex = futex->next_futex) {
+
+                       if (address == futex->address) {
+                               
simple_unlock(&hash_table->futex_hash_table_lock);
+                               return futex;
+
+                       }
+               }
+
+               for (futex = &(hash_table->futex_elements[hash_val]); futex != 
NULL; 
+                       futex = futex->prev_futex) {
+
+                       if (address == futex->address) {
+                               
simple_unlock(&hash_table->futex_hash_table_lock);
+                               return futex;
+
+                       }
+               }
+
+               simple_unlock(&hash_table->futex_hash_table_lock);
+       
+       } else
+               *per_process_mutex = FALSE;
+
+       return NULL;
+}
+
+int futex_hash_table_add_address(futex_hash_table_t hash_table, int *address)
+{
+       futex_t new_futex;
+       futex_t current_futex;
+
+       unsigned int hash_val = futex_hash(hash_table, address);
+
+       if (hash_table != NULL) {
+       
+               simple_lock(&hash_table->futex_hash_table_lock);
+       }
+
+       if ((new_futex = (futex_t)kalloc(sizeof(struct futex))) == NULL) {
+               if (hash_table != NULL)
+                       simple_unlock(&hash_table->futex_hash_table_lock);
+               return 1;
+       }
+
+       boolean_t per_process_mutex = TRUE;
+
+       current_futex = futex_hash_table_lookup_address(hash_table, address, 
&per_process_mutex);
+       if (current_futex != NULL) {
+               simple_unlock(&hash_table->futex_hash_table_lock);
+               return 2;
+       }
+       if (per_process_mutex == FALSE) return 1;
+
+       int ret;
+
+       if ((ret = futex_init(address, new_futex, hash_table, hash_val)) != 0) {
+               simple_unlock(&hash_table->futex_hash_table_lock);
+               return ret;
+       }
+
+       hash_table->futex_elements[hash_val] = *new_futex;
+
+       prev_hash_val = hash_val;
+
+       if (hash_table != NULL)
+               simple_unlock(&hash_table->futex_hash_table_lock);
+
+       return 0;
+}      
+
+int futex_hash_table_delete_futex(futex_t futex, futex_hash_table_t 
hash_table, int *address)
+{
+       if (current_thread()->task == hash_table->task) {
+
+               /* New next futex in queue. */
+               futex_t new_next_futex = futex->next_futex;
+
+               simple_lock(&hash_table->futex_hash_table_lock);
+
+               unsigned int hash_val = futex_hash(hash_table, address);
+
+               hash_table->futex_elements[hash_val].prev_futex->next_futex = 
new_next_futex;
+
+               /* New previous futex of the new next futex in queue. */
+               
hash_table->futex_elements[hash_val].prev_futex->next_futex->prev_futex = 
+                       hash_table->futex_elements[hash_val].prev_futex;
+
+               kfree((vm_offset_t)&(hash_table->futex_elements[hash_val]), 
sizeof(struct futex));
+
+               hash_table->size--;
+
+               if (hash_table->size == 0) {
+                       kfree((vm_offset_t)hash_table, sizeof(struct 
futex_hash_table));
+                       hash_table = NULL;
+               }
+
+               simple_unlock(&hash_table->futex_hash_table_lock);
+
+               return 0;
+
+       } else
+               return -1;
+}
+
+int futex_rpc(thread_t thread, int *address, int value, int operation)
+{
+       return futex(address, value, operation);
+}
+
+int futex_wait_rpc(thread_t thread, int *address, int value)
+{
+       return futex_wait(address, value);
+}
+
+int futex_wake_rpc(thread_t thread, int *address, int thread_num)
+{
+       return futex_wake(address, thread_num);
+}
+
diff --git a/kern/futex.h b/kern/futex.h
new file mode 100644
index 0000000..0a22663
--- /dev/null
+++ b/kern/futex.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2013 Free Software Foundation, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *     Author: Marin Ramesa
+ */
+
+#ifndef _KERN_FUTEX_H_
+#define _KERN_FUTEX_H_
+
+#include <kern/thread.h>
+#include <vm/vm_map.h>
+#include <kern/lock.h>
+#include <kern/task.h>
+
+/* Definitions for futex operations. */
+#define FUTEX_WAIT     0
+#define FUTEX_WAKE     1
+
+/* Definitions for return values. */
+#define FUTEX_RESUMED_BY_WAKE 0
+#define FUTEX_NO_THREAD_SUSPENDED -1
+#define FUTEX_SOME_NUMBER_RESUMED 1
+#define FUTEX_NO_THREAD -2
+#define FUTEX_EXISTS -3
+#define FUTEX_MEMORY_ERROR -4
+#define FUTEX_UNKNOWN_OPERATION -5
+#define FUTEX_RESOURCE_SHORTAGE -6
+
+/* Initial size of the hash table. */
+#define INITIAL_HASH_TABLE_SIZE 8
+
+typedef struct futex *futex_t;
+
+struct futex {
+       
+       /* Futex lock. */
+       decl_simple_lock_data( , futex_wait_lock);
+
+       /* Futex address. */
+       int *address;
+
+       /* Map futex is in. */
+       vm_map_t *map;
+
+       vm_map_version_t *version;
+       vm_object_t *object;
+       vm_offset_t *offset;
+       vm_prot_t *out_prot;
+       boolean_t *wired;
+
+       /* Next futex in queue. */
+       futex_t next_futex;
+
+       /* Previous futex in queue. */
+       futex_t prev_futex;
+
+       /* Number of futexed threads at the same address. */
+       unsigned int num_futexed_threads;
+
+       /* Array of futexed threads at the same address. */
+       thread_t futexed_threads;
+};
+
+struct futex_hash_table {
+       
+       /* Futex hash table lock. */
+       decl_simple_lock_data( , futex_hash_table_lock);
+
+       /* Size of the hash table. */
+       size_t size;
+
+       /* Task to which the hash table belongs. */
+       task_t task;
+
+       /* Array of futexes. */
+       futex_t futex_elements;
+};
+
+typedef struct futex_hash_table *futex_hash_table_t;
+
+extern int futex(int *address, int value, int operation);
+int futex_init(int *address, futex_t futex, futex_hash_table_t hash_table, 
unsigned int hash_val);
+extern int futex_wait(int *address, int value);
+extern int futex_wake(int *address, int thread_num);
+futex_hash_table_t futex_hash_table_init(size_t size, task_t task);
+unsigned int futex_hash(futex_hash_table_t hash_table, int *address);
+futex_t futex_hash_table_lookup_address(futex_hash_table_t hash_table, int 
*address, boolean_t *per_process_mutex);
+int futex_hash_table_add_address(futex_hash_table_t hash_table, int *address);
+int futex_hash_table_delete_futex(futex_t futex, futex_hash_table_t 
hash_table, int *address);
+extern int futex_rpc(thread_t thread, int *address, int value, int operation);
+extern int futex_wait_rpc(thread_t thread, int *address, int value);
+extern int futex_wake_rpc(thread_t thread, int *address, int thread_num);
+
+#endif /* _KERN_FUTEX_H_ */
-- 
1.8.1.4