e4d896c17d2e689c0eeb20456cfcef3860e2c1c9
[akaros.git] / user / pthread / futex.c
1 #include <ros/common.h>
2 #include <futex.h>
3 #include <sys/queue.h>
4 #include <pthread.h>
5 #include <assert.h>
6 #include <stdio.h>
7 #include <errno.h>
8 #include <slab.h>
9 #include <mcs.h>
10
11 struct futex_element {
12   TAILQ_ENTRY(futex_element) link;
13   pthread_t pthread;
14   int *uaddr;
15 };
16 TAILQ_HEAD(futex_queue, futex_element);
17
18 struct futex_data {
19   struct mcs_pdr_lock lock;
20   struct futex_queue queue;
21   struct kmem_cache *element_cache;
22 };
23 static struct futex_data __futex;
24
25 static inline void futex_init()
26 {
27   mcs_pdr_init(&__futex.lock);
28   TAILQ_INIT(&__futex.queue);
29   __futex.element_cache = kmem_cache_create("futex_element_cache", 
30     sizeof(struct futex_element), __alignof__(struct futex_element),
31     0, NULL, NULL);
32 }
33
34 static void __futex_block(struct uthread *uthread, void *arg) {
35   struct futex_element *e = (struct futex_element*)arg;
36   e->pthread = (pthread_t)uthread;
37         __pthread_generic_yield(e->pthread);
38   e->pthread->state = PTH_BLK_MUTEX;
39   TAILQ_INSERT_TAIL(&__futex.queue, e, link);
40   mcs_pdr_unlock(&__futex.lock);
41 }
42
43 static inline int futex_wait(int *uaddr, int val)
44 {
45   mcs_pdr_lock(&__futex.lock);
46   if(*uaddr == val) {
47     // We unlock in the body of __futex_block
48     struct futex_element *e = kmem_cache_alloc(__futex.element_cache, 0); 
49     e->uaddr = uaddr;
50     uthread_yield(TRUE, __futex_block, e);
51   }
52   else {
53     mcs_pdr_unlock(&__futex.lock);
54   }
55   return 0;
56 }
57
58 static inline int futex_wake(int *uaddr, int count)
59 {
60   struct futex_element *e,*n = NULL;
61   mcs_pdr_lock(&__futex.lock);
62   e = TAILQ_FIRST(&__futex.queue);
63   while(e != NULL) {
64     if(count > 0) {
65       n = TAILQ_NEXT(e, link);
66       if(e->uaddr == uaddr) {
67         TAILQ_REMOVE(&__futex.queue, e, link);
68         uthread_runnable((struct uthread*)e->pthread);
69         kmem_cache_free(__futex.element_cache, e); 
70         count--;
71       }
72       e = n;
73     }
74     else break;
75   }
76   mcs_pdr_unlock(&__futex.lock);
77   return 0;
78 }
79
80 int futex(int *uaddr, int op, int val, const struct timespec *timeout,
81                  int *uaddr2, int val3)
82 {
83   assert(timeout == NULL);
84   assert(uaddr2 == NULL);
85   assert(val3 == 0);
86
87   run_once(futex_init());
88   switch(op) {
89     case FUTEX_WAIT:
90       return futex_wait(uaddr, val);
91     case FUTEX_WAKE:
92       return futex_wake(uaddr, val);
93     default:
94       errno = ENOSYS;
95       return -1;
96   }
97   return -1;
98 }
99