Update futex code to faster method for blocking
[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 };
22 static struct futex_data __futex;
23
24 static inline void futex_init()
25 {
26   mcs_pdr_init(&__futex.lock);
27   TAILQ_INIT(&__futex.queue);
28 }
29
30 static void __futex_block(struct uthread *uthread, void *arg) {
31   pthread_t pthread = (pthread_t)uthread;
32   struct futex_element *e = (struct futex_element*)arg;
33   __pthread_generic_yield(pthread);
34   pthread->state = PTH_BLK_MUTEX;
35   e->pthread = pthread;
36 }
37
38 static inline int futex_wait(int *uaddr, int val)
39 {
40   mcs_pdr_lock(&__futex.lock);
41   if(*uaddr == val) {
42     struct futex_element e;
43     e.uaddr = uaddr;
44     e.pthread = NULL;
45     TAILQ_INSERT_TAIL(&__futex.queue, &e, link);
46     mcs_pdr_unlock(&__futex.lock);
47     uthread_yield(TRUE, __futex_block, &e);
48   }
49   else {
50     mcs_pdr_unlock(&__futex.lock);
51   }
52   return 0;
53 }
54
55 static inline int futex_wake(int *uaddr, int count)
56 {
57   struct futex_element *e,*n = NULL;
58   struct futex_queue q = TAILQ_HEAD_INITIALIZER(q);
59
60   // Atomically grab all relevant futex blockers
61   // from the global futex queue
62   mcs_pdr_lock(&__futex.lock);
63   e = TAILQ_FIRST(&__futex.queue);
64   while(e != NULL) {
65     if(count > 0) {
66       n = TAILQ_NEXT(e, link);
67       if(e->uaddr == uaddr) {
68         TAILQ_REMOVE(&__futex.queue, e, link);
69         TAILQ_INSERT_TAIL(&q, e, link);
70         count--;
71       }
72       e = n;
73     }
74     else break;
75   }
76   mcs_pdr_unlock(&__futex.lock);
77
78   // Unblock them outside the lock
79   e = TAILQ_FIRST(&q);
80   while(e != NULL) {
81     n = TAILQ_NEXT(e, link);
82     TAILQ_REMOVE(&q, e, link);
83     while(e->pthread == NULL)
84       cpu_relax();
85     uthread_runnable((struct uthread*)e->pthread);
86     e = n;
87   }
88   return 0;
89 }
90
91 int futex(int *uaddr, int op, int val, const struct timespec *timeout,
92                  int *uaddr2, int val3)
93 {
94   assert(timeout == NULL);
95   assert(uaddr2 == NULL);
96   assert(val3 == 0);
97
98   run_once(futex_init());
99   switch(op) {
100     case FUTEX_WAIT:
101       return futex_wait(uaddr, val);
102     case FUTEX_WAKE:
103       return futex_wake(uaddr, val);
104     default:
105       errno = ENOSYS;
106       return -1;
107   }
108   return -1;
109 }
110