5ddb893b86c7e24c726b9a362ccacc1949267528
[akaros.git] / user / c3po / threads / mutex.c
1
2 /**
3  * Simple mutexes, rwlocks and condition variables
4  **/
5
6 #include "threadlib_internal.h"
7 #include "threadlib.h"
8
9 #include "util.h"
10 #include <error.h>
11 #include <stdlib.h>
12 #include <sys/time.h>
13
14 #ifndef DEBUG_mutex_c
15 #undef debug
16 #define debug(...)
17 #undef tdebug
18 #define tdebug(...)
19 #endif
20
21
22 static void queue_init(queue_t *q, int n);
23 static void queue_free(queue_t *q) __attribute__ ((unused));
24 static void enqueue(queue_t *q, void *v);
25 static void *dequeue(queue_t *q);
26 static int queue_isempty(queue_t *q);
27 static int queue_remove(queue_t *q, void *v);
28
29 inline int thread_mutex_init(mutex_t *m, char *name) 
30 {
31   m->state = UNLOCKED;
32   m->name = name;
33   m->count = 0;
34   m->owner = NULL;
35   queue_init(&m->wait_queue, 0);   // This will be allocated when a thread actually waits on the mutex
36   thread_latch_init( m->latch );
37   return TRUE;
38 }
39
40
41 static inline int _thread_mutex_lock(mutex_t *m, int istry)
42 {
43   if (m == NULL)
44     return_errno(FALSE, EINVAL);
45
46   thread_latch( m->latch );
47
48   // If unlocked, just lock it
49   if (m->state == UNLOCKED)
50     {
51       // Need to have a spin lock on the mutex if we have multiple kernel threads
52       debug("thread_mutex_lock: locking mutex directly\n");
53       m->state = LOCKED;
54       m->owner = current_thread;
55       m->count = 1;
56       thread_unlatch( m->latch );
57       return TRUE;
58     }
59     
60   assert(m->count >= 1);
61
62   // already locked by caller
63   if (m->owner == current_thread)
64     {
65       // recursive lock
66       m->count++;
67       debug("thread_mutex_lock: recursive locking\n");
68       thread_unlatch( m->latch );
69       return TRUE;
70     }
71
72   // Locked by someone else.  
73   if (istry)
74     return_errno_unlatch(FALSE, EBUSY, m->latch);
75
76   // add current thread to waiting queue
77   assert(current_thread);
78   enqueue(&m->wait_queue, current_thread);
79
80   // suspend the current thread
81   thread_unlatch( m->latch );
82   CAP_SET_SYSCALL();
83   thread_suspend_self(0);
84   CAP_CLEAR_SYSCALL();
85
86   // wake up.  Verify that we now own the lock
87   thread_latch( m->latch );
88   assert(m->state == LOCKED);
89   assert(m->owner == current_thread);
90   assert(m->count == 1);
91   thread_unlatch( m->latch );
92
93   return TRUE;
94 }
95
96 inline int thread_mutex_lock(mutex_t *m)
97 {
98   return _thread_mutex_lock(m, FALSE);
99 }
100
101 inline int thread_mutex_trylock(mutex_t *m)
102 {
103   return _thread_mutex_lock(m, TRUE);
104 }
105
106 inline int thread_mutex_unlock(mutex_t *m)
107 {
108   thread_t *t;
109
110   if (m == NULL)
111     return_errno(FALSE, EINVAL);
112
113   thread_latch(m->latch);
114   if (m->state != LOCKED) 
115     return_errno_unlatch(FALSE, EDEADLK, m->latch);
116   if (m->owner != current_thread)
117     return_errno_unlatch(FALSE, EPERM, m->latch);
118
119   m->count--;
120
121   // still locked by the current thread
122   if (m->count > 0)
123     return_errno_unlatch(TRUE, 0, m->latch);
124
125   // get the first waiter
126   t = (thread_t *)dequeue(&m->wait_queue);
127
128   // no threads waiting
129   if(t == NULL) {
130     m->state = UNLOCKED;
131     m->count = 0;
132     m->owner = NULL;
133   }
134
135   // resume the waiter
136   else {
137     m->owner = t;
138     m->count = 1;
139     thread_resume(t);
140   }
141
142   thread_unlatch(m->latch);
143   return TRUE;
144 }
145
146
147 /*
148 **  Read-Write Locks
149 */
150
151 int thread_rwlock_init(rwlock_t *rwlock)
152 {
153     if (rwlock == NULL)
154         return_errno(FALSE, EINVAL);
155     rwlock->rw_state = THREAD_RWLOCK_INITIALIZED;
156     rwlock->rw_readers = 0;
157     thread_mutex_init(&rwlock->rw_mutex_rd, NULL);
158     thread_mutex_init(&rwlock->rw_mutex_rw, NULL);
159     return TRUE;
160 }
161
162 static int _thread_rwlock_lock(rwlock_t *rwlock, int op, int tryonly)
163 {
164     /* consistency checks */
165     if (rwlock == NULL)
166         return_errno(FALSE, EINVAL);
167     if (!(rwlock->rw_state & THREAD_RWLOCK_INITIALIZED))
168         return_errno(FALSE, EDEADLK);
169
170     /* lock lock */
171     if (op == RWLOCK_RW) {
172         /* read-write lock is simple */
173       if (!_thread_mutex_lock(&(rwlock->rw_mutex_rw), tryonly))
174         return FALSE;
175       rwlock->rw_mode = RWLOCK_RW;
176     }
177     else {
178         /* read-only lock is more complicated to get right */
179         if (!_thread_mutex_lock(&(rwlock->rw_mutex_rd), tryonly))
180             return FALSE;
181         rwlock->rw_readers++;
182         if (rwlock->rw_readers == 1) {
183             if (!_thread_mutex_lock(&(rwlock->rw_mutex_rw), tryonly)) {
184                 rwlock->rw_readers--;
185                 thread_mutex_unlock(&(rwlock->rw_mutex_rd));
186                 return FALSE;
187             }
188         }
189         rwlock->rw_mode = RWLOCK_RD;
190         thread_mutex_unlock(&(rwlock->rw_mutex_rd));
191     }
192     return TRUE;
193 }
194
195 int thread_rwlock_lock(rwlock_t *l, int op)
196 {
197   return _thread_rwlock_lock(l, op, FALSE);
198 }
199
200 int thread_rwlock_trylock(rwlock_t *l, int op)
201 {
202   return _thread_rwlock_lock(l, op, TRUE);
203 }
204
205 int thread_rwlock_unlock(rwlock_t *rwlock)
206 {
207     /* consistency checks */
208     if (rwlock == NULL)
209         return_errno(FALSE, EINVAL);
210     if (!(rwlock->rw_state & THREAD_RWLOCK_INITIALIZED))
211         return_errno(FALSE, EDEADLK);
212
213     /* unlock lock */
214     if (rwlock->rw_mode == RWLOCK_RW) {
215         /* read-write unlock is simple */
216         if (!thread_mutex_unlock(&(rwlock->rw_mutex_rw)))
217             return FALSE;
218     }
219     else {
220         /* read-only unlock is more complicated to get right */
221         if (!_thread_mutex_lock(&(rwlock->rw_mutex_rd), FALSE))
222             return FALSE;
223         rwlock->rw_readers--;
224         if (rwlock->rw_readers == 0) {
225             if (!thread_mutex_unlock(&(rwlock->rw_mutex_rw))) {
226                 rwlock->rw_readers++;
227                 thread_mutex_unlock(&(rwlock->rw_mutex_rd));
228                 return FALSE;
229             }
230         }
231         rwlock->rw_mode = RWLOCK_RD;
232         thread_mutex_unlock(&(rwlock->rw_mutex_rd));
233     }
234     return TRUE;
235 }
236
237 /*
238 **  Condition Variables
239 */
240
241 int thread_cond_init(cond_t *cond)
242 {
243     if (cond == NULL)
244         return_errno(FALSE, EINVAL);
245     cond->cn_state   = THREAD_COND_INITIALIZED;
246     cond->cn_waiters = 0;
247     queue_init(&cond->wait_queue, 0);
248     return TRUE;
249 }
250
251 int thread_cond_timedwait(cond_t *cond, mutex_t *mutex, const struct timespec *abstime)
252 {
253   unsigned long timeout = 0;
254   int sus_rv = 0;
255     /* consistency checks */
256     if (cond == NULL || mutex == NULL)
257         return_errno(FALSE, EINVAL);
258     if (!(cond->cn_state & THREAD_COND_INITIALIZED))
259         return_errno(FALSE, EDEADLK);
260
261     /* add us to the number of waiters */
262     cond->cn_waiters++;
263
264     /* unlock mutex (caller had to lock it first) */
265     thread_mutex_unlock(mutex);
266
267     /* wait until the condition is signaled */
268     assert (current_thread);
269     enqueue(&cond->wait_queue, current_thread);
270
271     if (abstime) {
272       struct timeval tv;
273       gettimeofday(&tv, NULL);
274       timeout = (abstime->tv_sec - tv.tv_sec) * 1000000 +
275         (abstime->tv_nsec / 1000 - tv.tv_usec);
276     }
277
278     CAP_SET_SYSCALL();
279     sus_rv = thread_suspend_self(timeout);
280     CAP_CLEAR_SYSCALL();
281     
282     /* relock mutex */
283     thread_mutex_lock(mutex);
284
285         // FIXME: this is wrong, we should retry after INTERRUPTED
286     if (sus_rv == TIMEDOUT || sus_rv == INTERRUPTED) {
287       /* we timed out */
288       /* remove us from the number of waiters */
289       /* our thread could possibly be already removed by thread_cond_signal() */
290       if (queue_remove(&cond->wait_queue, current_thread))
291         cond->cn_waiters--;
292
293       return_errno(FALSE, ETIMEDOUT);
294     } else
295       return TRUE;
296 }
297
298 int thread_cond_wait(cond_t *cond, mutex_t *mutex)
299 {
300   return thread_cond_timedwait(cond, mutex, NULL);
301 }
302
303
304 static int _thread_cond_signal(cond_t *cond, int broadcast)
305 {
306     /* consistency checks */
307     if (cond == NULL)
308         return_errno(FALSE, EINVAL);
309     if (!(cond->cn_state & THREAD_COND_INITIALIZED))
310         return_errno(FALSE, EDEADLK);
311
312     // do something only if there is at least one waiters (POSIX semantics)
313     if (cond->cn_waiters > 0) {
314       // signal the condition
315       do {
316         thread_t *t = dequeue(&cond->wait_queue);
317         assert (t != NULL);
318         thread_resume(t);   // t could also be a timed out thread, but it doesn't matter
319         cond->cn_waiters--;
320       } while (broadcast && !queue_isempty(&cond->wait_queue));
321       
322       // and give other threads a chance to grab the CPU 
323       CAP_SET_SYSCALL();
324       thread_yield();
325       CAP_CLEAR_SYSCALL();
326     }
327
328     /* return to caller */
329     return TRUE;
330 }
331
332 int thread_cond_signal(cond_t *cond) {
333   return _thread_cond_signal(cond, FALSE);
334 }
335
336 int thread_cond_broadcast(cond_t *cond) {
337   return _thread_cond_signal(cond, TRUE);
338 }
339
340 // Simple queue implementation
341 #define WRAP_LEN(head, tail, n) (head) <= (tail) ? ((tail) - (head)) \
342                                 : ((n) + (tail) - (head))
343 #define WRAP_DEC(x, n) ((x) == 0 ? (n) - 1 : (x) - 1)
344 #define WRAP_INC(x, n) ((x) == ((n) - 1) ? 0 : (x) + 1)
345
346 static void queue_init(queue_t *q, int n) {
347   q->size = n + 1;
348   q->data = (void **)malloc((n + 1) * sizeof(void *));
349   assert(q->data);
350   q->head = 0;
351   q->tail = 0;
352 }
353
354 static void queue_free(queue_t *q) {
355   free(q->data);
356 }
357
358 static void enqueue(queue_t *q, void *v) {
359   int cur_size = WRAP_LEN(q->head, q->tail, q->size);
360   if (cur_size == q->size - 1) {
361
362     /* Enlarge the queue */
363     int newsize;
364     if (q->size == 0)
365       newsize = 2;
366     else 
367       newsize = q->size + q->size;
368
369     q->data = realloc(q->data, newsize * sizeof(void *));
370     assert(q->data);
371     if (q->head > q->tail) {   /* do we need to move data? */
372       memmove(q->data + newsize - (q->size - q->head), q->data + q->head, (q->size - q->head)
373               * sizeof(void *));  
374       q->head = newsize - (q->size - q->head);
375       assert(*(q->data + newsize - 1) == *(q->data + q->size - 1) ); // the old last element should now be at the end of the larger block
376     }
377     q->size = newsize;
378   }
379   *(q->data + q->tail) = v;
380   q->tail = (q->tail == q->size - 1) ? 0 : q->tail + 1;
381 }
382
383 static void *dequeue(queue_t *q) {
384   void *r;
385
386   // the queue is empty
387   if(q->head == q->tail) 
388     return NULL;
389
390   r = *(q->data + q->head);
391   q->head = (q->head == q->size - 1) ? 0 : q->head + 1;
392   return r;
393 }
394
395 // remove the first occurance of a certain value from the queue
396 // return TRUE if the value is found
397 // FIXME: this is O(N)!
398 static int queue_remove(queue_t *q, void *v) {
399   int i = q->head;
400   int rv = FALSE;
401
402   while (i != q->tail) {
403     if (q->data[i] == v) {
404       q->tail = WRAP_DEC(q->tail, q->size);
405       q->data[i] = q->data[q->tail];
406       rv = TRUE;
407       break;
408     }
409     i = WRAP_INC(i, q->size);
410   }
411
412   return rv;
413 }
414
415 static int queue_isempty(queue_t *q) {
416   return q->head == q->tail;
417 }
418
419 #undef WRAP_LEN
420