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