Makes "measure kill" work with the appserver
[akaros.git] / user / parlib / pthread.c
1 #include <ros/arch/trapframe.h>
2 #include <pthread.h>
3 #include <vcore.h>
4 #include <mcs.h>
5 #include <stdlib.h>
6 #include <string.h>
7 #include <assert.h>
8 #include <rstdio.h>
9 #include <errno.h>
10 #include <parlib.h>
11 #include <ros/notification.h>
12 #include <arch/atomic.h>
13 #include <arch/arch.h>
14 #include <sys/queue.h>
15 #include <sys/mman.h>
16 #include <assert.h>
17
18 struct pthread_queue ready_queue = TAILQ_HEAD_INITIALIZER(ready_queue);
19 struct pthread_queue active_queue = TAILQ_HEAD_INITIALIZER(active_queue);
20 mcs_lock_t queue_lock = MCS_LOCK_INIT;
21 pthread_once_t init_once = PTHREAD_ONCE_INIT;
22 int threads_ready = 0;
23 int threads_active = 0;
24
25 /* Helper / local functions */
26 static int get_next_pid(void);
27 static inline void spin_to_sleep(unsigned int spins, unsigned int *spun);
28
29 __thread struct pthread_tcb *current_thread = 0;
30
31 void _pthread_init()
32 {
33         if (vcore_init())
34                 printf("vcore_init() failed, we're fucked!\n");
35         
36         assert(vcore_id() == 0);
37
38         /* tell the kernel where and how we want to receive notifications */
39         struct notif_method *nm;
40         for (int i = 0; i < MAX_NR_NOTIF; i++) {
41                 nm = &__procdata.notif_methods[i];
42                 nm->flags |= NOTIF_WANTED | NOTIF_MSG | NOTIF_IPI;
43                 nm->vcoreid = i % 2; // vcore0 or 1, keepin' it fresh.
44         }
45
46         /* Create a pthread_tcb for the main thread */
47         pthread_t t = (pthread_t)calloc(sizeof(struct pthread_tcb), 1);
48         t->id = get_next_pid();
49         assert(t->id == 0);
50         /* Put the new pthread on the active queue */
51         mcs_lock_lock(&queue_lock);
52         threads_active++;
53         TAILQ_INSERT_TAIL(&active_queue, t, next);
54         mcs_lock_unlock(&queue_lock);
55         
56         /* Save a pointer to the newly created threads tls region into its tcb */
57         t->tls_desc = get_tls_desc(0);
58         /* Save a pointer to the pthread in its own TLS */
59         current_thread = t;
60
61         /* Change temporarily to vcore0s tls region so we can save the newly created
62          * tcb into its current_thread variable and then restore it.  One minor
63          * issue is that vcore0's transition-TLS isn't TLS_INITed yet.  Until it is
64          * (right before vcore_entry(), don't try and take the address of any of
65          * its TLS vars. */
66         extern void** vcore_thread_control_blocks;
67         set_tls_desc(vcore_thread_control_blocks[0], 0);
68         current_thread = t;
69         set_tls_desc(t->tls_desc, 0);
70
71         // TODO: consider replacing this when we have an interface allowing
72         // requesting absolute num vcores, and moving it to pthread_create and
73         // asking for 2
74         vcore_request(1);
75 }
76
77 void __attribute__((noreturn)) vcore_entry()
78 {
79         uint32_t vcoreid = vcore_id();
80
81         struct preempt_data *vcpd = &__procdata.vcore_preempt_data[vcoreid];
82         struct vcore *vc = &__procinfo.vcoremap[vcoreid];
83
84         /* Should always have notifications disabled when coming in here. */
85         assert(vcpd->notif_enabled == FALSE);
86
87         /* Put this in the loop that deals with notifications.  It will return if
88          * there is no preempt pending. */ 
89         // TODO: prob make a handle_notif() function
90         if (vc->preempt_pending)
91                 sys_yield(TRUE);
92         // TODO: consider making this restart path work for restarting as well as
93         // freshly starting
94         if (current_thread) {
95                 vcpd->notif_pending = 0;
96                 /* Do one last check for notifs after clearing pending */
97                 // TODO: call the handle_notif() here (first)
98
99                 set_tls_desc(current_thread->tls_desc, vcoreid);
100                 /* Pop the user trap frame */
101                 pop_ros_tf(&vcpd->notif_tf, vcoreid);
102                 assert(0);
103         }
104
105         /* no one currently running, so lets get someone from the ready queue */
106         struct pthread_tcb *new_thread = NULL;
107 #ifdef __CONFIG_OSDI__
108         // Added so that we will return back to here if there is no new thread
109         // instead of at the top of this function.  Related to the fact that
110         // the kernel level scheduler can't yet handle scheduling manycore 
111         // processed yet when there are no more jobs left (i.e. sys_yield() will
112         // return instead of actually yielding...).
113         while(!new_thread) {
114 #endif
115                 mcs_lock_lock(&queue_lock);
116                 new_thread = TAILQ_FIRST(&ready_queue);
117                 if (new_thread) {
118                         TAILQ_REMOVE(&ready_queue, new_thread, next);
119                         TAILQ_INSERT_TAIL(&active_queue, new_thread, next);
120                         threads_active++;
121                         threads_ready--;
122                 }
123                 mcs_lock_unlock(&queue_lock);
124                 if (!new_thread) {
125                         printd("[P] No threads, vcore %d is yielding\n", vcoreid);
126                         sys_yield(0);
127                 }
128 #ifdef __CONFIG_OSDI__
129         }
130 #endif
131         /* Save a ptr to the pthread running in the transition context's TLS */
132         current_thread = new_thread;
133         printd("[P] Vcore %d is starting pthread %d\n", vcoreid, new_thread->id);
134
135         vcpd->notif_pending = 0;
136         /* Do one last check for notifs after clearing pending */
137         // TODO: call the handle_notif() here (first)
138         set_tls_desc(new_thread->tls_desc, vcoreid);
139
140         /* Load silly state (Floating point) too.  For real */
141         // TODO: (HSS)
142
143         /* Pop the user trap frame */
144         pop_ros_tf(&new_thread->utf, vcoreid);
145         assert(0);
146 }
147
148 int pthread_attr_init(pthread_attr_t *a)
149 {
150   return 0;
151 }
152
153 int pthread_attr_destroy(pthread_attr_t *a)
154 {
155   return 0;
156 }
157
158 static void __pthread_free_tls(struct pthread_tcb *pt)
159 {
160         extern void _dl_deallocate_tls (void *tcb, bool dealloc_tcb) internal_function;
161
162         assert(pt->tls_desc);
163         _dl_deallocate_tls(pt->tls_desc, TRUE);
164         pt->tls_desc = NULL;
165 }
166
167 static int __pthread_allocate_tls(struct pthread_tcb *pt)
168 {
169         extern void *_dl_allocate_tls (void *mem) internal_function;
170
171         assert(!pt->tls_desc);
172         pt->tls_desc = _dl_allocate_tls(NULL);
173         if (!pt->tls_desc) {
174                 errno = ENOMEM;
175                 return -1;
176         }
177         return 0;
178 }
179
180 // TODO: how big do we want these?  ideally, we want to be able to guard and map
181 // more space if we go too far.
182 #define PTHREAD_STACK_PAGES 4
183 #define PTHREAD_STACK_SIZE (PTHREAD_STACK_PAGES*PGSIZE)
184
185 static void __pthread_free_stack(struct pthread_tcb *pt)
186 {
187         assert(!munmap(pt->stacktop - PTHREAD_STACK_SIZE, PTHREAD_STACK_SIZE));
188 }
189
190 static int __pthread_allocate_stack(struct pthread_tcb *pt)
191 {
192         void* stackbot = mmap(0, PTHREAD_STACK_SIZE,
193                               PROT_READ|PROT_WRITE|PROT_EXEC,
194                               MAP_POPULATE|MAP_ANONYMOUS, -1, 0);
195         if (stackbot == MAP_FAILED)
196                 return -1; // errno set by mmap
197         pt->stacktop = stackbot + PTHREAD_STACK_SIZE;
198         return 0;
199 }
200
201 void __pthread_run(void)
202 {
203         struct pthread_tcb *me = current_thread;
204         pthread_exit(me->start_routine(me->arg));
205 }
206
207 // Warning, this will reuse numbers eventually
208 static int get_next_pid(void)
209 {
210         static uint32_t next_pid = 0;
211         return next_pid++;
212 }
213
214 int pthread_create(pthread_t* thread, const pthread_attr_t* attr,
215                    void *(*start_routine)(void *), void* arg)
216 {
217         /* After this init, we are an MCP and the caller is a pthread */
218         pthread_once(&init_once,&_pthread_init);
219
220         struct pthread_tcb *t = pthread_self();
221         assert(t);
222         /* Don't migrate this thread to anothe vcore, since it depends on being on
223          * the same vcore throughout. */
224         t->dont_migrate = TRUE;
225         uint32_t vcoreid = vcore_id();
226         *thread = (pthread_t)calloc(sizeof(struct pthread_tcb), 1);
227         (*thread)->start_routine = start_routine;
228         (*thread)->arg = arg;
229         (*thread)->id = get_next_pid();
230         if (__pthread_allocate_stack(*thread) ||  __pthread_allocate_tls(*thread))
231                 printf("We're fucked\n");
232         /* Save the ptr to the new pthread in that pthread's TLS */
233         set_tls_desc((*thread)->tls_desc, vcoreid);
234         current_thread = *thread;
235         set_tls_desc(t->tls_desc, vcoreid);
236         /* Set the u_tf to start up in __pthread_run, which will call the real
237          * start_routine and pass it the arg. */
238         init_user_tf(&(*thread)->utf, (uint32_t)__pthread_run, 
239                  (uint32_t)((*thread)->stacktop));
240         /* Insert the newly created thread into the ready queue of threads.
241          * It will be removed from this queue later when vcore_entry() comes up */
242         mcs_lock_lock(&queue_lock);
243         TAILQ_INSERT_TAIL(&ready_queue, *thread, next);
244         threads_ready++;
245         mcs_lock_unlock(&queue_lock);
246         /* Okay to migrate now. */
247         t->dont_migrate = FALSE;
248         /* Attempt to request a new core, may or may not get it... */
249         vcore_request(1);
250         return 0;
251 }
252
253 int pthread_join(pthread_t thread, void** retval)
254 {
255         /* Not sure if this is the right semantics.  There is a race if we deref
256          * thread and he is already freed (which would have happened if he was
257          * detached. */
258         if (thread->detached) {
259                 printf("[pthread] trying to join on a detached pthread");
260                 return -1;
261         }
262         while (!thread->finished)
263                 pthread_yield();
264         if (retval)
265                 *retval = thread->retval;
266         free(thread);
267         return 0;
268 }
269
270 static void __attribute__((noinline, noreturn)) 
271 __pthread_yield(struct pthread_tcb *t)
272 {
273         /* TODO: want to set this to FALSE once we no longer depend on being on this
274          * vcore.  Though if we are using TLS, we are depending on the vcore.  Since
275          * notifs are disabled and we are in a transition context, we probably
276          * shouldn't be moved anyway.  It does mean that a pthread could get jammed.
277          * If we do this after putting it on the active list, we'll have a race on
278          * dont_migrate. */
279         t->dont_migrate = FALSE;
280         /* Take from the active list, and put on the ready list (tail).  Don't do
281          * this until we are done completely with the thread, since it can be
282          * restarted somewhere else. */
283         mcs_lock_lock(&queue_lock);
284         threads_active--;
285         TAILQ_REMOVE(&active_queue, t, next);
286         threads_ready++;
287         TAILQ_INSERT_TAIL(&ready_queue, t, next);
288         mcs_lock_unlock(&queue_lock);
289         /* Leave the current vcore completely */
290         current_thread = NULL; // this might be okay, even with a migration
291         /* Go back to the entry point, where we can handle notifications or
292          * reschedule someone. */
293         vcore_entry();
294 }
295
296 int pthread_yield(void)
297 {
298         struct pthread_tcb *t = pthread_self();
299         volatile bool yielding = TRUE; /* signal to short circuit when restarting */
300
301         /* TODO: (HSS) Save silly state */
302         // save_fp_state(&t->as);
303
304         /* Don't migrate this thread to another vcore, since it depends on being on
305          * the same vcore throughout (once it disables notifs). */
306         t->dont_migrate = TRUE;
307         uint32_t vcoreid = vcore_id();
308         printd("[P] Pthread id %d is yielding on vcore %d\n", t->id, vcoreid);
309         struct preempt_data *vcpd = &__procdata.vcore_preempt_data[vcoreid];
310         /* once we do this, we might miss a notif_pending, so we need to enter vcore
311          * entry later.  Need to disable notifs so we don't get in weird loops with
312          * save_ros_tf() and pop_ros_tf(). */
313         vcpd->notif_enabled = FALSE;
314         /* take the current state and save it into t->utf when this pthread
315          * restarts, it will continue from right after this, see yielding is false,
316          * and short ciruit the function. */
317         save_ros_tf(&t->utf);
318         if (!yielding)
319                 goto yield_return_path;
320         yielding = FALSE; /* for when it starts back up */
321         /* Change to the transition context (both TLS and stack). */
322         extern void** vcore_thread_control_blocks;
323         set_tls_desc(vcore_thread_control_blocks[vcoreid], vcoreid);
324         assert(current_thread == t);    
325         /* After this, make sure you don't use local variables.  Note the warning in
326          * pthread_exit() */
327         set_stack_pointer((void*)vcpd->transition_stack);
328         /* Finish exiting in another function. */
329         __pthread_yield(current_thread);
330         /* Should never get here */
331         assert(0);
332         /* Will jump here when the pthread's trapframe is restarted/popped. */
333 yield_return_path:
334         printd("[P] pthread %d returning from a yield!\n", t->id);
335         return 0;
336 }
337
338 int pthread_mutexattr_init(pthread_mutexattr_t* attr)
339 {
340   attr->type = PTHREAD_MUTEX_DEFAULT;
341   return 0;
342 }
343
344 int pthread_mutexattr_destroy(pthread_mutexattr_t* attr)
345 {
346   return 0;
347 }
348
349
350 int pthread_attr_setdetachstate(pthread_attr_t *__attr,int __detachstate) {
351         *__attr = __detachstate;
352         // TODO: the right thing
353         return 0;
354 }
355
356 int pthread_mutexattr_gettype(const pthread_mutexattr_t* attr, int* type)
357 {
358   *type = attr ? attr->type : PTHREAD_MUTEX_DEFAULT;
359   return 0;
360 }
361
362 int pthread_mutexattr_settype(pthread_mutexattr_t* attr, int type)
363 {
364   if(type != PTHREAD_MUTEX_NORMAL)
365     return EINVAL;
366   attr->type = type;
367   return 0;
368 }
369
370 int pthread_mutex_init(pthread_mutex_t* m, const pthread_mutexattr_t* attr)
371 {
372   m->attr = attr;
373   m->lock = 0;
374   return 0;
375 }
376
377 /* Set *spun to 0 when calling this the first time.  It will yield after 'spins'
378  * calls.  Use this for adaptive mutexes and such. */
379 static inline void spin_to_sleep(unsigned int spins, unsigned int *spun)
380 {
381         if ((*spun)++ == spins) {
382                 pthread_yield();
383                 *spun = 0;
384         }
385 }
386
387 int pthread_mutex_lock(pthread_mutex_t* m)
388 {
389         unsigned int spinner = 0;
390         while(pthread_mutex_trylock(m))
391                 while(*(volatile size_t*)&m->lock) {
392                         cpu_relax();
393                         spin_to_sleep(PTHREAD_MUTEX_SPINS, &spinner);
394                 }
395         return 0;
396 }
397
398 int pthread_mutex_trylock(pthread_mutex_t* m)
399 {
400   return atomic_swap(&m->lock,1) == 0 ? 0 : EBUSY;
401 }
402
403 int pthread_mutex_unlock(pthread_mutex_t* m)
404 {
405   m->lock = 0;
406   return 0;
407 }
408
409 int pthread_mutex_destroy(pthread_mutex_t* m)
410 {
411   return 0;
412 }
413
414 int pthread_cond_init(pthread_cond_t *c, const pthread_condattr_t *a)
415 {
416   c->attr = a;
417   memset(c->waiters,0,sizeof(c->waiters));
418   return 0;
419 }
420
421 int pthread_cond_destroy(pthread_cond_t *c)
422 {
423   return 0;
424 }
425
426 int pthread_cond_broadcast(pthread_cond_t *c)
427 {
428   memset(c->waiters,0,sizeof(c->waiters));
429   return 0;
430 }
431
432 int pthread_cond_signal(pthread_cond_t *c)
433 {
434   int i;
435   for(i = 0; i < max_vcores(); i++)
436   {
437     if(c->waiters[i])
438     {
439       c->waiters[i] = 0;
440       break;
441     }
442   }
443   return 0;
444 }
445
446 int pthread_cond_wait(pthread_cond_t *c, pthread_mutex_t *m)
447 {
448   c->waiters[vcore_id()] = 1;
449   pthread_mutex_unlock(m);
450
451   volatile int* poll = &c->waiters[vcore_id()];
452   while(*poll);
453
454   pthread_mutex_lock(m);
455
456   return 0;
457 }
458
459 int pthread_condattr_init(pthread_condattr_t *a)
460 {
461   a = PTHREAD_PROCESS_PRIVATE;
462   return 0;
463 }
464
465 int pthread_condattr_destroy(pthread_condattr_t *a)
466 {
467   return 0;
468 }
469
470 int pthread_condattr_setpshared(pthread_condattr_t *a, int s)
471 {
472   a->pshared = s;
473   return 0;
474 }
475
476 int pthread_condattr_getpshared(pthread_condattr_t *a, int *s)
477 {
478   *s = a->pshared;
479   return 0;
480 }
481
482 pthread_t pthread_self()
483 {
484   return current_thread;
485 }
486
487 int pthread_equal(pthread_t t1, pthread_t t2)
488 {
489   return t1 == t2;
490 }
491
492 /* Need to have this as a separate, non-inlined function since we clobber the
493  * stack pointer before calling it, and don't want the compiler to play games
494  * with my hart. */
495 static void __attribute__((noinline, noreturn)) 
496 __pthread_exit(struct pthread_tcb *t)
497 {
498         __pthread_free_tls(t);
499         __pthread_free_stack(t);
500         if (t->detached)
501                 free(t);
502         /* Once we do this, our joiner can free us.  He won't free us if we're
503          * detached, but there is still a potential race there (since he's accessing
504          * someone who is freed. */
505         t->finished = 1;
506         current_thread = NULL;
507         /* Go back to the entry point, where we can handle notifications or
508          * reschedule someone. */
509         vcore_entry();
510 }
511
512 /* This function cannot be migrated to a different vcore by the userspace
513  * scheduler.  Will need to sort that shit out.  */
514 void pthread_exit(void* ret)
515 {
516         struct pthread_tcb *t = pthread_self();
517         /* Don't migrate this thread to anothe vcore, since it depends on being on
518          * the same vcore throughout. */
519         t->dont_migrate = TRUE; // won't set this to false later, since he is dying
520
521         uint32_t vcoreid = vcore_id();
522         struct preempt_data *vcpd = &__procdata.vcore_preempt_data[vcoreid];
523
524         t->retval = ret;
525         mcs_lock_lock(&queue_lock);
526         threads_active--;
527         TAILQ_REMOVE(&active_queue, t, next);
528         mcs_lock_unlock(&queue_lock);
529
530         printd("[P] Pthread id %d is exiting on vcore %d\n", t->id, vcoreid);
531         
532         /* once we do this, we might miss a notif_pending, so we need to enter vcore
533          * entry later. */
534         vcpd->notif_enabled = FALSE;
535
536         /* Change to the transition context (both TLS and stack). */
537         extern void** vcore_thread_control_blocks;
538         set_tls_desc(vcore_thread_control_blocks[vcoreid], vcoreid);
539         assert(current_thread == t);    
540         /* After this, make sure you don't use local variables.  Also, make sure the
541          * compiler doesn't use them without telling you (TODO).
542          *
543          * In each arch's set_stack_pointer, make sure you subtract off as much room
544          * as you need to any local vars that might be pushed before calling the
545          * next function, or for whatever other reason the compiler/hardware might
546          * walk up the stack a bit when calling a noreturn function. */
547         set_stack_pointer((void*)vcpd->transition_stack);
548         /* Finish exiting in another function.  Ugh. */
549         __pthread_exit(current_thread);
550 }
551
552 int pthread_once(pthread_once_t* once_control, void (*init_routine)(void))
553 {
554   if(atomic_swap(once_control,1) == 0)
555     init_routine();
556   return 0;
557 }
558
559 int pthread_barrier_init(pthread_barrier_t* b, const pthread_barrierattr_t* a, int count)
560 {
561   memset(b->local_sense,0,sizeof(b->local_sense));
562
563   b->sense = 0;
564   b->nprocs = b->count = count;
565   pthread_mutex_init(&b->pmutex, 0);
566   return 0;
567 }
568
569 int pthread_barrier_wait(pthread_barrier_t* b)
570 {
571   unsigned int spinner = 0;
572   struct pthread_tcb *t = pthread_self();
573
574   int id = t->id;
575   int ls = b->local_sense[32*id] = 1 - b->local_sense[32*id];
576
577   pthread_mutex_lock(&b->pmutex);
578   int count = --b->count;
579   pthread_mutex_unlock(&b->pmutex);
580
581   if(count == 0)
582   {
583     b->count = b->nprocs;
584     b->sense = ls;
585     return PTHREAD_BARRIER_SERIAL_THREAD;
586   }
587   else
588   {
589     while(b->sense != ls) {
590       cpu_relax();
591       spin_to_sleep(PTHREAD_BARRIER_SPINS, &spinner);
592     }
593     return 0;
594   }
595 }
596
597 int pthread_barrier_destroy(pthread_barrier_t* b)
598 {
599   pthread_mutex_destroy(&b->pmutex);
600   return 0;
601 }