Implementation of the futex interface for pthreads
[akaros.git] / user / pthread / futex.c
1 #include <ros/common.h>
2 #include <ros/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   static bool initialized = false;
28   if(initialized)
29     return;
30   initialized = true;
31
32   mcs_pdr_init(&__futex.lock);
33   TAILQ_INIT(&__futex.queue);
34   __futex.element_cache = kmem_cache_create("futex_element_cache", 
35     sizeof(struct futex_element), __alignof__(struct futex_element),
36     0, NULL, NULL);
37 }
38
39 static void __futex_block(struct uthread *uthread, void *arg) {
40   struct futex_element *e = (struct futex_element*)arg;
41   e->pthread = (pthread_t)uthread;
42   e->pthread->state = PTH_BLK_MUTEX;
43   TAILQ_INSERT_TAIL(&__futex.queue, e, link);
44   mcs_pdr_unlock(&__futex.lock);
45 }
46
47 static inline int futex_wait(int *uaddr, int val)
48 {
49   mcs_pdr_lock(&__futex.lock);
50   if(*uaddr == val) {
51     // We unlock in the body of __futex_block
52     struct futex_element *e = kmem_cache_alloc(__futex.element_cache, 0); 
53     e->uaddr = uaddr;
54     uthread_yield(TRUE, __futex_block, e);
55   }
56   else {
57     mcs_pdr_unlock(&__futex.lock);
58   }
59   return 0;
60 }
61
62 static inline int futex_wake(int *uaddr, int count)
63 {
64   struct futex_element *e,*n = NULL;
65   mcs_pdr_lock(&__futex.lock);
66   int size = 0;
67   TAILQ_FOREACH(e, &__futex.queue, link)
68     size++;
69   e = TAILQ_FIRST(&__futex.queue);
70   while(e != NULL) {
71     if(count > 0) {
72       n = TAILQ_NEXT(e, link);
73       if(e->uaddr == uaddr) {
74         TAILQ_REMOVE(&__futex.queue, e, link);
75         uthread_runnable((struct uthread*)e->pthread);
76         kmem_cache_free(__futex.element_cache, e); 
77         count--;
78       }
79       e = n;
80     }
81     else break;
82   }
83   mcs_pdr_unlock(&__futex.lock);
84   return 0;
85 }
86
87 int futex(int *uaddr, int op, int val, const struct timespec *timeout,
88                  int *uaddr2, int val3)
89 {
90   assert(timeout == NULL);
91   assert(uaddr2 == NULL);
92   assert(val3 == 0);
93
94   futex_init();
95   switch(op) {
96     case FUTEX_WAIT:
97       return futex_wait(uaddr, val);
98     case FUTEX_WAKE:
99       return futex_wake(uaddr, val);
100     default:
101       errno = ENOSYS;
102       return -1;
103   }
104   return -1;
105 }
106