mt-waitset: adding masking of channels so a thread won't handle two messages at the...
[barrelfish] / lib / barrelfish / threads.c
1 /**
2  * \file
3  * \brief Threads implementation.
4  */
5
6 /*
7  * Copyright (c) 2007, 2008, 2009, 2010, 2011, 2012, ETH Zurich.
8  * All rights reserved.
9  *
10  * This file is distributed under the terms in the attached LICENSE file.
11  * If you do not find this file, copies can be found by writing to:
12  * ETH Zurich D-INFK, Haldeneggsteig 4, CH-8092 Zurich. Attn: Systems Group.
13  */
14
15 #include <stdlib.h>
16 #include <stdio.h>
17 #include <string.h>
18 #include <barrelfish/barrelfish.h>
19 #include <barrelfish/dispatch.h>
20 #include <barrelfish/dispatcher_arch.h>
21 #include <barrelfish/debug.h>
22 #include <barrelfish/slab.h>
23 #include <barrelfish/caddr.h>
24 #include <barrelfish/curdispatcher_arch.h>
25 #include <barrelfish/vspace_mmu_aware.h>
26 #include <barrelfish_kpi/cpu_arch.h>
27 #include <barrelfish_kpi/domain_params.h>
28 #include <arch/registers.h>
29 #include <trace/trace.h>
30
31 #include <trace_definitions/trace_defs.h>
32
33 #include "arch/threads.h"
34 #include "threads_priv.h"
35 #include "init.h"
36
37 #if defined(__x86_64__)
38 #  include "arch/ldt.h"
39 #endif
40
41 #ifdef FPU_LAZY_CONTEXT_SWITCH
42 #  include <arch/fpu.h>
43 #endif
44
45 /// Maximum number of threads in a domain, used to size VM region for thread structures
46 // there is no point having MAX_THREADS > LDT_NENTRIES on x86 (see ldt.c)
47 #define MAX_THREADS 256
48
49 /// 16-byte alignment required for x86-64
50 // FIXME: this should be in an arch header
51 #define STACK_ALIGNMENT (sizeof(uint64_t) * 2)
52
53 /// Static stack and storage for a bootstrap/cleanup thread
54 // XXX: 16-byte aligned for x86-64
55 static uintptr_t staticstack[THREADS_DEFAULT_STACK_BYTES / sizeof(uintptr_t)]
56 __attribute__((aligned(STACK_ALIGNMENT)));
57 static struct thread staticthread = {
58     .stack = staticstack,
59     .stack_top = (char *)staticstack + sizeof(staticstack)
60 };
61 static struct thread_mutex staticthread_lock = THREAD_MUTEX_INITIALIZER;
62
63 /// Storage metadata for thread structures (and TLS data)
64 static struct slab_allocator thread_slabs;
65 static struct vspace_mmu_aware thread_slabs_vm;
66
67 // XXX: mutex and spinlock protecting thread slabs in spanned domains
68 /* This ought to be just a mutex. However, thread_create() is called on the
69  * inter-disp message handler thread, and if it blocks in a mutex, there is no
70  * way to wake it up and we will deadlock. This is a quick-fix workaround:
71  *   The spinlock protects the data structure
72  *   The mutex avoids unneccessary spinning (it is acquired first when safe)
73  */
74 static spinlock_t thread_slabs_spinlock;
75 static struct thread_mutex thread_slabs_mutex = THREAD_MUTEX_INITIALIZER;
76
77 /// Base and size of the original ("pristine") thread-local storage init data
78 static void *tls_block_init_base;
79 static size_t tls_block_init_len;
80 static size_t tls_block_total_len;
81
82 /// Warning already issued about RSP usage.  (Prevent repeated warnings
83 /// from the same domain -- e.g., when using THC whose stacks appear
84 /// invalid here).
85 __attribute__((unused)) static bool stack_warned=0;
86
87 /// Wrapper function for most threads, runs given function then deletes itself
88 static void thread_entry(thread_func_t start_func, void *start_data)
89 {
90     assert((lvaddr_t)start_func >= BASE_PAGE_SIZE);
91     int retval = start_func(start_data);
92     thread_exit(retval);
93     assert(!"thread_exit returned");
94 }
95
96 /// int counter for assigning initial thread ids
97 static uintptr_t threadid = 0;
98
99 #ifndef NDEBUG
100 /// Debugging assertions on thread queues
101 static void check_queue(struct thread *queue)
102 {
103     if (queue == NULL) {
104         return;
105     }
106     struct thread *q = queue;
107     int i = 0;
108
109     do {
110         assert_disabled(q != NULL);
111
112         // check for NULL next and prev pointers
113         assert_disabled((lvaddr_t)q->next > BASE_PAGE_SIZE);
114         assert_disabled((lvaddr_t)q->prev > BASE_PAGE_SIZE);
115
116         // check that next and prev pointers are sane
117         assert_disabled(q->next->prev == q);
118         assert_disabled(q->prev->next == q);
119
120         // advance to next elem
121         q = q->next;
122         i++;
123         assert_disabled(i < MAX_THREADS);
124     } while (q != queue);
125 }
126 #else /* NDEBUG version */
127 static inline void check_queue(struct thread *queue) {}
128 #endif
129
130 /**
131  * \brief Enqueue a thread in the given queue
132  *
133  * For safety, should only happen while disabled.
134  */
135 void thread_enqueue(struct thread *thread, struct thread **queue)
136 {
137     assert_disabled(thread != NULL);
138     assert_disabled(queue != NULL);
139     check_queue(*queue);
140     if (*queue == NULL) {
141         *queue = thread->prev = thread->next = thread;
142     } else {
143         assert_disabled((*queue)->prev != NULL);
144         thread->prev = (*queue)->prev;
145         thread->next = *queue;
146         (*queue)->prev = thread;
147         assert_disabled(thread->prev != NULL);
148         thread->prev->next = thread;
149     }
150
151     check_queue(*queue);
152 }
153
154 /**
155  * \brief Dequeue the first thread on the given queue
156  *
157  * For safety, should only happen while disabled.
158  *
159  * \returns Pointer to thread that was dequeued
160  */
161 struct thread *thread_dequeue(struct thread **queue)
162 {
163     assert_disabled(queue != NULL);
164     struct thread *thread = *queue;
165     assert_disabled(thread != NULL);
166     check_queue(thread);
167     if (thread->prev == thread) {
168         assert_disabled(thread->next == thread);
169         *queue = NULL;
170     } else {
171         thread->prev->next = thread->next;
172         thread->next->prev = thread->prev;
173         *queue = thread->next;
174     }
175     check_queue(*queue);
176 #ifndef NDEBUG
177     thread->prev = thread->next = NULL;
178 #endif
179     return thread;
180 }
181
182 /**
183  * \brief Remove a specific thread from a queue
184  *
185  * Does not check that the thread is in the given queue, which it must be.
186  * For safety, should only happen while disabled.
187  */
188 void thread_remove_from_queue(struct thread **queue, struct thread *thread)
189 {
190     assert_disabled(queue != NULL);
191     assert_disabled(thread != NULL);
192     check_queue(*queue);
193     if (thread->prev == thread) {
194         assert_disabled(thread->next == thread);
195         assert_disabled(*queue == thread);
196         *queue = NULL;
197     } else {
198         thread->prev->next = thread->next;
199         thread->next->prev = thread->prev;
200         if (*queue == thread) {
201             *queue = thread->next;
202         }
203     }
204     check_queue(*queue);
205 #ifndef NDEBUG
206     thread->prev = thread->next = NULL;
207 #endif
208 }
209
210 /// Refill backing storage for thread region
211 static errval_t refill_thread_slabs(struct slab_allocator *slabs)
212 {
213     assert(slabs == &thread_slabs);
214
215     size_t size;
216     void *buf;
217     errval_t err;
218
219     size_t blocksize = sizeof(struct thread) + tls_block_total_len;
220     err = vspace_mmu_aware_map(&thread_slabs_vm, blocksize, &buf, &size);
221     if (err_is_fail(err)) {
222         return err_push(err, LIB_ERR_VSPACE_MMU_AWARE_MAP);
223     }
224
225     slab_grow(slabs, buf, size);
226
227     return SYS_ERR_OK;
228 }
229
230 /// Initialise the state of a new thread structure
231 static void thread_init(dispatcher_handle_t disp, struct thread *newthread)
232 {
233     newthread->self = newthread;
234 #ifndef NDEBUG
235     newthread->next = newthread->prev = NULL;
236 #endif
237     newthread->tls_dtv = NULL;
238     newthread->disp = disp;
239     newthread->coreid = get_dispatcher_generic(disp)->core_id;
240     newthread->userptr = NULL;
241     memset(newthread->userptrs, 0, sizeof(newthread->userptrs));
242     newthread->yield_epoch = 0;
243     newthread->wakeup_reason = NULL;
244     newthread->return_value = 0;
245     thread_cond_init(&newthread->exit_condition);
246     thread_mutex_init(&newthread->exit_lock);
247     newthread->state = THREAD_STATE_RUNNABLE;
248     newthread->detached = false;
249     newthread->joining = false;
250     newthread->in_exception = false;
251     newthread->used_fpu = false;
252     newthread->paused = false;
253     newthread->slab = NULL;
254     newthread->token = 0;
255     newthread->token_number = 1;
256
257     newthread->rpc_in_progress = false;
258     newthread->async_error = SYS_ERR_OK;
259     newthread->mask_channels = false;
260 }
261
262 /**
263  * \brief Handle FPU context switching
264  */
265 static void fpu_context_switch(struct dispatcher_generic *disp_gen,
266                                struct thread * next)
267 {
268 #ifdef FPU_LAZY_CONTEXT_SWITCH
269     if(disp_gen->fpu_thread == NULL) {
270         // FPU wasn't used -- no switching necessary
271         return;
272     }
273
274     // Switch FPU trap back on if we switch away from FPU thread
275     if(disp_gen->fpu_thread == disp_gen->current &&
276        disp_gen->fpu_thread != next) {
277         fpu_trap_on();
278     }
279 #endif
280 }
281
282 /**
283  * \brief Returns false if the stack pointer is out of bounds.
284  */
285 static bool thread_check_stack_bounds(struct thread *thread,
286                                       arch_registers_state_t *archregs) {
287     lvaddr_t sp = (lvaddr_t) registers_get_sp(archregs);
288     return sp > (lvaddr_t)thread->stack ||
289            sp <= (lvaddr_t)thread->stack_top;
290 }
291
292 /**
293  * \brief Schedule and run the next active thread, or yield the dispatcher.
294  *
295  * This may only be called from the dispatcher (on its stack and while
296  * disabled!).
297  *
298  * \param disp Dispatcher pointer
299  */
300 void thread_run_disabled(dispatcher_handle_t handle)
301 {
302     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
303     struct dispatcher_shared_generic *disp =
304         get_dispatcher_shared_generic(handle);
305     arch_registers_state_t *enabled_area =
306         dispatcher_get_enabled_save_area(handle);
307
308     if (disp_gen->current != NULL) {
309         assert_disabled(disp_gen->runq != NULL);
310
311         // check stack bounds
312         warn_disabled(&stack_warned,
313                       thread_check_stack_bounds(disp_gen->current, enabled_area));
314
315         struct thread *next = disp_gen->current->next;
316         assert_disabled(next != NULL);
317         if (next != disp_gen->current) {
318             fpu_context_switch(disp_gen, next);
319
320             // save previous thread's state
321             arch_registers_state_t *cur_regs = &disp_gen->current->regs;
322             memcpy(cur_regs, enabled_area, sizeof(arch_registers_state_t));
323             disp_gen->current = next;
324             disp_resume(handle, &next->regs);
325         } else {
326             // same thread as before
327             disp_resume(handle, enabled_area);
328         }
329     } else if (disp_gen->runq != NULL) {
330         fpu_context_switch(disp_gen, disp_gen->runq);
331         disp_gen->current = disp_gen->runq;
332         disp->haswork = true;
333         disp_resume(handle, &disp_gen->runq->regs);
334     } else {
335         // kernel gave us the CPU when we have nothing to do. block!
336         disp->haswork = havework_disabled(handle);
337         disp_gen->current = NULL;
338         disp_yield_disabled(handle);
339     }
340 }
341
342 /** Free all heap/slab-allocated state associated with a thread */
343 static void free_thread(struct thread *thread)
344 {
345 #if defined(__x86_64__) // XXX: gungy segment selector stuff
346     assert(thread->thread_seg_selector != 0);
347     uint16_t fs;
348     __asm("mov %%fs, %0" : "=r" (fs));
349     if (thread->thread_seg_selector == fs) {
350         assert(thread->disp == curdispatcher());
351         struct dispatcher_x86_64 *disp_priv = get_dispatcher_x86_64(thread->disp);
352         // we're freeing the current thread; make sure we reload a valid segment
353         // selector so that curdispatcher() keeps working!
354         __asm volatile("mov %%ax, %%fs"
355                        : /* No outputs */
356                        : "a" (disp_priv->disp_seg_selector));
357     }
358     ldt_free_segment(thread->thread_seg_selector);
359 #endif
360
361     free(thread->stack);
362     if (thread->tls_dtv != NULL) {
363         free(thread->tls_dtv);
364     }
365
366     thread_mutex_lock(&thread_slabs_mutex);
367     acquire_spinlock(&thread_slabs_spinlock);
368     slab_free(&thread_slabs, thread->slab); // frees thread itself
369     release_spinlock(&thread_slabs_spinlock);
370     thread_mutex_unlock(&thread_slabs_mutex);
371 }
372
373 /**
374  * \brief Creates a new thread that will not be runnable
375  *
376  * \param start_func Function to run on the new thread
377  * \param arg Argument to pass to function
378  * \param stacksize Size of stack, in bytes
379  *
380  * \returns Thread pointer on success, NULL on failure
381  */
382 struct thread *thread_create_unrunnable(thread_func_t start_func, void *arg,
383                                         size_t stacksize)
384 {
385     // allocate stack
386     assert((stacksize % sizeof(uintptr_t)) == 0);
387     void *stack = malloc(stacksize);
388     if (stack == NULL) {
389         return NULL;
390     }
391
392     // allocate space for TCB + initial TLS data
393     // no mutex as it may deadlock: see comment for thread_slabs_spinlock
394     // thread_mutex_lock(&thread_slabs_mutex);
395     acquire_spinlock(&thread_slabs_spinlock);
396     void *space = slab_alloc(&thread_slabs);
397     release_spinlock(&thread_slabs_spinlock);
398     // thread_mutex_unlock(&thread_slabs_mutex);
399     if (space == NULL) {
400         free(stack);
401         return NULL;
402     }
403
404     // split space into TLS data followed by TCB
405     // XXX: this layout is specific to the x86 ABIs! once other (saner)
406     // architectures support TLS, we'll need to break out the logic.
407     void *tls_data = space;
408     struct thread *newthread = (void *)((uintptr_t)space + tls_block_total_len);
409
410     // init thread
411     thread_init(curdispatcher(), newthread);
412     newthread->slab = space;
413
414     if (tls_block_total_len > 0) {
415         // populate initial TLS data from pristine copy
416         assert(tls_block_init_len <= tls_block_total_len);
417         memcpy(tls_data, tls_block_init_base, tls_block_init_len);
418
419         // zero-fill remainder
420         memset((char *)tls_data + tls_block_init_len, 0,
421                tls_block_total_len - tls_block_init_len);
422
423         // create a TLS thread vector
424         struct tls_dtv *dtv = malloc(sizeof(struct tls_dtv) + 1 * sizeof(void *));
425         assert(dtv != NULL);
426
427         dtv->gen = 0;
428         dtv->dtv[0] = tls_data;
429         newthread->tls_dtv = dtv;
430     }
431
432     // FIXME: make arch-specific
433 #if defined(__x86_64__) || defined(__k1om__)
434     // create segment for TCB
435     errval_t err = ldt_alloc_segment(newthread, &newthread->thread_seg_selector);
436     if (err_is_fail(err)) {
437         DEBUG_ERR(err, "error allocating LDT segment for new thread");
438         free_thread(newthread);
439         return NULL;
440     }
441 #endif
442
443     // init stack
444     newthread->stack = stack;
445     newthread->stack_top = (char *)stack + stacksize;
446
447     // waste space for alignment, if malloc gave us an unaligned stack
448     newthread->stack_top = (char *)newthread->stack_top
449         - (lvaddr_t)newthread->stack_top % STACK_ALIGNMENT;
450
451     // set thread's ID
452     newthread->id = threadid++;
453
454     // init registers
455     registers_set_initial(&newthread->regs, newthread, (lvaddr_t)thread_entry,
456                           (lvaddr_t)newthread->stack_top,
457                           (lvaddr_t)start_func, (lvaddr_t)arg, 0, 0);
458
459     return newthread;
460 }
461
462 /**
463  * \brief Creates a new thread, and makes it runnable
464  *
465  * \param start_func Function to run on the new thread
466  * \param arg Argument to pass to function
467  * \param stacksize Size of stack, in bytes
468  *
469  * \returns Thread pointer on success, NULL on failure
470  */
471 struct thread *thread_create_varstack(thread_func_t start_func, void *arg,
472                                       size_t stacksize)
473 {
474     struct thread *newthread = thread_create_unrunnable(start_func, arg, stacksize);
475     if (newthread) {
476         // enqueue on runq
477         dispatcher_handle_t handle = disp_disable();
478         struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
479         newthread->disp = handle;
480         thread_enqueue(newthread, &disp_gen->runq);
481         disp_enable(handle);
482     }
483     return newthread;
484 }
485
486 /**
487  * \brief Creates a new thread, and makes it runnable
488  *
489  * \param start_func Function to run on the new thread
490  * \param arg Argument to pass to function
491  *
492  * \returns Thread pointer on success, NULL on failure
493  */
494 struct thread *thread_create(thread_func_t start_func, void *arg)
495 {
496     return thread_create_varstack(start_func, arg, THREADS_DEFAULT_STACK_BYTES);
497 }
498
499 /**
500  * \brief Wait for termination of another thread
501  *
502  * \param thread        Pointer to thread to wait for
503  * \param retval        Pointer to variable to hold return value of thread, or NULL
504  *
505  * \returns SYS_ERR_OK on success, error code on error.
506  */
507 errval_t thread_join(struct thread *thread, int *retval)
508 {
509     assert(thread != NULL);
510     // this function should only be called for threads on same core
511     assert(thread->coreid == disp_get_core_id());
512
513     thread_mutex_lock(&thread->exit_lock);
514     if(thread->detached) {
515         // Thread is detached and thus not joinable
516         thread_mutex_unlock(&thread->exit_lock);
517         return LIB_ERR_THREAD_JOIN_DETACHED;
518     }
519
520     if(thread->joining) {
521         // Someone else already joins, that's an error
522         thread_mutex_unlock(&thread->exit_lock);
523         return LIB_ERR_THREAD_JOIN;
524     }
525
526     thread->joining = true;
527     if(thread->state != THREAD_STATE_EXITED) { // Possibly wait for thread exit
528         thread_cond_wait(&thread->exit_condition, &thread->exit_lock);
529     }
530
531     if(retval != NULL) {
532         *retval = thread->return_value;
533     }
534
535     thread_mutex_unlock(&thread->exit_lock);    // Not really needed
536     free_thread(thread);
537
538     return SYS_ERR_OK;
539 }
540
541 /**
542  * \brief Detach a thread. Free its state when it terminates.
543  *
544  * \param thread        Pointer to thread to detach
545  *
546  * \return SYS_ERR_OK on success.
547  */
548 errval_t thread_detach(struct thread *thread)
549 {
550     assert(thread != NULL);
551     thread_mutex_lock(&thread->exit_lock);
552
553     if(thread->joining) {
554         // Someone else already joins, that's an error
555         thread_mutex_unlock(&thread->exit_lock);
556         return LIB_ERR_THREAD_JOIN;
557     }
558
559     if(!thread->detached) {
560         thread->detached = true;
561     } else {
562         // Detaching more than once is an error
563         thread_mutex_unlock(&thread->exit_lock);
564         return LIB_ERR_THREAD_DETACHED;
565     }
566
567     if(thread->state == THREAD_STATE_EXITED) {
568         // Thread already exited before we detached, clean it up
569         free_thread(thread);
570         return SYS_ERR_OK;
571     }
572
573     thread_mutex_unlock(&thread->exit_lock);
574     return SYS_ERR_OK;
575 }
576
577 /**
578  * \brief Returns the thread pointer to the currently-running thread
579  */
580 struct thread *thread_self(void)
581 {
582     struct thread *me;
583 #if defined(__x86_64__) // XXX: AB's silly little arch-specific optimisation
584     __asm("movq %%fs:0, %0" : "=r" (me));
585 #else
586     // it's not necessary to disable, but might be once we do migration
587     bool was_enabled;
588     dispatcher_handle_t handle = disp_try_disable(&was_enabled);
589     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
590     me = disp_gen->current;
591     if (was_enabled)
592         disp_enable(handle);
593 #endif
594     return me;
595 }
596
597 struct thread *thread_self_disabled(void)
598 {
599     dispatcher_handle_t handle = curdispatcher();
600     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
601     return disp_gen->current;
602 }
603
604 uintptr_t thread_id(void)
605 {
606     return thread_self()->id;
607 }
608
609 uintptr_t thread_get_id(struct thread *t)
610 {
611     return t->id;
612 }
613
614 void thread_set_id(uintptr_t id)
615 {
616     struct thread *me = thread_self();
617     me->id = id;
618 }
619
620 uint32_t thread_set_token(struct waitset_chanstate *channel)
621 {
622     struct thread *me = thread_self();
623     // generate new token
624     uint32_t outgoing_token = (uint32_t)((me->id << 16) |
625          (me->coreid << 24) | ((me->token_number & 255) << 8)) | 1;
626     assert(me->token == 0);
627     me->token_number++;
628     if (!(me->token_number & 255))
629         me->token_number = 1;
630     me->token = outgoing_token & ~1;    // wait for this token
631     me->channel = channel;              // on that channel
632     return outgoing_token;
633 }
634
635 void thread_clear_token(struct waitset_chanstate *channel)
636 {
637     struct thread *me = thread_self();
638
639     me->token = 0;      // don't wait anymore
640     me->channel = NULL;
641 }
642
643 uint32_t thread_current_token(void)
644 {
645     return thread_self()->token;
646 }
647
648 void thread_set_outgoing_token(uint32_t token)
649 {
650     struct thread *me = thread_self();
651
652     assert(!me->outgoing_token);
653     me->outgoing_token = token;
654 }
655
656 void thread_get_outgoing_token(uint32_t *token)
657 {
658     struct thread *me = thread_self();
659     // if thread's outgoing token is set, get it
660     if (me->outgoing_token) {
661         *token = me->outgoing_token;
662         me->outgoing_token = 0;
663     }
664 }
665
666 void thread_set_rpc_in_progress(bool v)
667 {
668     thread_self()->rpc_in_progress = v;
669 }
670
671 bool thread_get_rpc_in_progress(void)
672 {
673     return thread_self()->rpc_in_progress;
674 }
675
676 void thread_set_async_error(errval_t e)
677 {
678     thread_self()->async_error = e;
679 }
680
681 errval_t thread_get_async_error(void)
682 {
683     return thread_self()->async_error;
684 }
685
686 void thread_set_mask_channels(bool m)
687 {
688     thread_self()->mask_channels = m;
689 }
690
691 bool thread_get_mask_channels(void)
692 {
693     return thread_self()->mask_channels;
694 }
695
696 /**
697  * \brief Yield the calling thread
698  *
699  * Switches to the next runnable thread in this dispatcher, or if none is
700  * available, yields the dispatcher.
701  */
702 void thread_yield(void)
703 {
704     dispatcher_handle_t handle = disp_disable();
705     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
706     struct dispatcher_shared_generic *disp =
707         get_dispatcher_shared_generic(handle);
708     arch_registers_state_t *enabled_area =
709         dispatcher_get_enabled_save_area(handle);
710
711     struct thread *me = disp_gen->current;
712     struct thread *next = me;
713     me->yield_epoch = disp_gen->timeslice;
714
715     do {
716         assert_disabled(next != NULL);
717         next = next->next;
718         if (next == me) {
719             break; // Everybody yielded this timeslice
720         }
721     } while(next->yield_epoch == disp_gen->timeslice);
722
723     poll_channels_disabled(handle);
724
725     if (next != me) {
726         fpu_context_switch(disp_gen, next);
727         disp_gen->current = next;
728         disp_switch(handle, &me->regs, &next->regs);
729     } else {
730         assert_disabled(disp_gen->runq != NULL);
731         assert_disabled(disp->haswork);
732         trace_event(TRACE_SUBSYS_THREADS, TRACE_EVENT_THREADS_C_DISP_SAVE, 3);
733         disp_save(handle, enabled_area, true, CPTR_NULL);
734     }
735 }
736
737 /**
738  * \brief Yield both the calling thread, and the dispatcher to another domain
739  *
740  * \param endpoint Endpoint cap to which we wish to yield, or #CAP_NULL
741  *                  for an undirected yield
742  *
743  * Yields the dispatcher, optionally to another specified dispatcher.
744  */
745 void thread_yield_dispatcher(struct capref endpoint)
746 {
747     dispatcher_handle_t handle = disp_disable();
748     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
749     struct dispatcher_shared_generic *disp =
750         get_dispatcher_shared_generic(handle);
751     arch_registers_state_t *enabled_area =
752         dispatcher_get_enabled_save_area(handle);
753
754     assert_disabled(disp_gen->runq != NULL);
755     assert_disabled(disp->haswork);
756
757     trace_event(TRACE_SUBSYS_THREADS, TRACE_EVENT_THREADS_C_DISP_SAVE, 1);
758     disp_save(handle, enabled_area, true, get_cap_addr(endpoint));
759 }
760
761 /// Function that runs on the static thread/stack to clean up a "real" (alloced) thread
762 static int cleanup_thread(void *arg)
763 {
764     struct thread *thread = arg;
765
766     // free old thread and its stack
767     if (thread != NULL) {
768         free_thread(thread);
769     }
770
771     // disable and release static thread
772     dispatcher_handle_t handle = disp_disable();
773     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
774     struct dispatcher_shared_generic *disp =
775         get_dispatcher_shared_generic(handle);
776     struct thread *me = disp_gen->current;
777     struct thread *ft =
778         thread_mutex_unlock_disabled(handle, &disp_gen->cleanupthread_lock);
779     assert(ft == NULL);
780
781     // run the next thread, if any
782     struct thread *next = me->next;
783     thread_remove_from_queue(&disp_gen->runq, me);
784     if (next != me) {
785         disp_gen->current = next;
786         disp_resume(handle, &next->regs);
787     } else {
788         disp_gen->current = NULL;
789         disp->haswork = havework_disabled(handle);
790         disp_yield_disabled(handle);
791     }
792
793     return 0;
794 }
795
796 /**
797  * \brief Terminate the calling thread
798  */
799 void thread_exit(int status)
800 {
801     struct thread *me = thread_self();
802
803     thread_mutex_lock(&me->exit_lock);
804
805     // if this is the static thread, we don't need to do anything but cleanup
806     if (me == &staticthread) {
807         assert(me->detached);
808         // disable and release static thread
809         dispatcher_handle_t handle = disp_disable();
810         struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
811         struct dispatcher_shared_generic *disp =
812             get_dispatcher_shared_generic(handle);
813         assert_disabled(me == &staticthread);
814         assert_disabled(me->stack == staticstack);
815         struct thread *ft =
816             thread_mutex_unlock_disabled(handle, &staticthread_lock);
817         assert(ft == NULL);
818
819 #ifdef FPU_LAZY_CONTEXT_SWITCH
820         // No more FPU usage from here on
821         if(disp_gen->fpu_thread == me) {
822             disp_gen->fpu_thread = NULL;
823             fpu_trap_on();
824         }
825 #endif
826
827         // run the next thread, if any
828         struct thread *next = me->next;
829         thread_remove_from_queue(&disp_gen->runq, me);
830         if (next != me) {
831             fpu_context_switch(disp_gen, next);
832             disp_gen->current = next;
833             disp_resume(handle, &next->regs);
834         } else {
835             disp_gen->current = NULL;
836             disp->haswork = havework_disabled(handle);
837             disp_yield_disabled(handle);
838         }
839     }
840
841     if(me->detached) {
842         // otherwise, we use a dispatcher-local thread to perform cleanup
843         struct dispatcher_generic *dg = get_dispatcher_generic(curdispatcher());
844         thread_mutex_lock(&dg->cleanupthread_lock);
845         if(dg->cleanupthread == NULL) {
846             dg->cleanupthread =
847                 thread_create_unrunnable(cleanup_thread, me,
848                                          THREADS_DEFAULT_STACK_BYTES);
849         }
850         thread_init(curdispatcher(), dg->cleanupthread);
851
852         registers_set_initial(&dg->cleanupthread->regs, dg->cleanupthread,
853                               (lvaddr_t)cleanup_thread,
854                               (lvaddr_t)dg->cleanupthread->stack_top, (lvaddr_t)me,
855                               0, 0, 0);
856
857         // Switch to it (on this dispatcher)
858         dispatcher_handle_t handle = disp_disable();
859         struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
860
861 #ifdef FPU_LAZY_CONTEXT_SWITCH
862         // No more FPU usage from here on
863         if(disp_gen->fpu_thread == me) {
864             disp_gen->fpu_thread = NULL;
865             fpu_trap_on();
866         }
867 #endif
868
869         thread_remove_from_queue(&disp_gen->runq, me);
870         thread_enqueue(dg->cleanupthread, &disp_gen->runq);
871         disp_gen->cleanupthread->disp = handle;
872         fpu_context_switch(disp_gen, dg->cleanupthread);
873         disp_gen->current = dg->cleanupthread;
874         disp_resume(handle, &dg->cleanupthread->regs);
875     } else {
876         // We're not detached -- wakeup joiner
877         me->return_value = status;
878         me->state = THREAD_STATE_EXITED;
879         thread_cond_signal(&me->exit_condition);
880
881         // Disable and unlock exit lock
882         dispatcher_handle_t handle = disp_disable();
883         struct thread *wakeup =
884             thread_mutex_unlock_disabled(handle, &me->exit_lock);
885         struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
886         struct dispatcher_shared_generic *disp =
887             get_dispatcher_shared_generic(handle);
888
889         assert_disabled(wakeup == NULL);
890
891 #ifdef FPU_LAZY_CONTEXT_SWITCH
892         // No more FPU usage from here on
893         if(disp_gen->fpu_thread == me) {
894             disp_gen->fpu_thread = NULL;
895             fpu_trap_on();
896         }
897 #endif
898
899         // run the next thread, if any
900         struct thread *next = me->next;
901         thread_remove_from_queue(&disp_gen->runq, me);
902         if (next != me) {
903             fpu_context_switch(disp_gen, next);
904             disp_gen->current = next;
905             disp_resume(handle, &next->regs);
906         } else {
907             disp_gen->current = NULL;
908             disp->haswork = havework_disabled(handle);
909             disp_yield_disabled(handle);
910         }
911     }
912
913     USER_PANIC("should never be reached");
914 }
915
916 /**
917  * \brief Block the caller, and optionally release a spinlock, while disabled
918  *
919  * The caller is unconditionally blocked, and placed into the given queue
920  * pending a call that will unblock it. After manipulating the queues, and
921  * before switching threds, the given spinlock, if specified, is unlocked.
922  * This function must only be called while disabled.
923  *
924  * This function is intended for use by multi-processor thread synchronisation
925  * functions.
926  *
927  * \param disp Dispatcher pointer
928  * \param queue (Optional) Queue of threads in which to place caller
929  * \param spinlock (Optional) pointer to spinlock
930  *
931  * \returns Argument passed to thread_unblock, when unblocked
932  */
933 void *thread_block_and_release_spinlock_disabled(dispatcher_handle_t handle,
934                                                  struct thread **queue,
935                                                  spinlock_t *spinlock)
936 {
937     struct dispatcher_shared_generic *disp =
938         get_dispatcher_shared_generic(handle);
939     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
940     struct thread *me = disp_gen->current;
941     struct thread *next = me->next;
942     assert_disabled(next != NULL);
943
944     assert_disabled(me->state == THREAD_STATE_RUNNABLE);
945     me->state = THREAD_STATE_BLOCKED;
946
947     thread_remove_from_queue(&disp_gen->runq, me);
948     if (queue != NULL) {
949         thread_enqueue(me, queue);
950     }
951
952     if (spinlock != NULL) {
953         release_spinlock(spinlock);
954     }
955
956     if (next != me) {
957         assert_disabled(disp_gen->runq != NULL);
958         fpu_context_switch(disp_gen, next);
959         disp_gen->current = next;
960         disp_switch(handle, &me->regs, &next->regs);
961     } else {
962         assert_disabled(disp_gen->runq == NULL);
963         disp_gen->current = NULL;
964         disp->haswork = havework_disabled(handle);
965         trace_event(TRACE_SUBSYS_THREADS, TRACE_EVENT_THREADS_C_DISP_SAVE, 2);
966         disp_save(handle, &me->regs, true, CPTR_NULL);
967     }
968
969     assert(me->disp == handle); // didn't migrate while asleep
970     return me->wakeup_reason;
971 }
972
973 /**
974  * \brief Block the calling thread, while disabled
975  *
976  * The caller is unconditionally blocked, and placed into the given queue
977  * pending a call that will unblock it.
978  * This function must only be called while disabled.
979  *
980  * \param disp Dispatcher pointer
981  * \param queue Queue of threads in which to place caller
982  *
983  * \returns Argument passed to thread_unblock, when unblocked
984  */
985 void *thread_block_disabled(dispatcher_handle_t disp, struct thread **queue)
986 {
987     return thread_block_and_release_spinlock_disabled(disp, queue, NULL);
988 }
989
990 /**
991  * \brief Block the calling thread, while enabled
992  *
993  * The caller is unconditionally blocked, and placed into the given queue
994  * pending a call that will unblock it.
995  * This function must only be called while enabled.
996  *
997  * \param queue Queue of threads in which to place caller
998  *
999  * \returns Argument passed to thread_unblock, when unblocked
1000  */
1001 void *thread_block(struct thread **queue)
1002 {
1003     return thread_block_disabled(disp_disable(), queue);
1004 }
1005
1006 /**
1007  * \brief Unblock a single thread from a given queue, while disabled
1008  *
1009  * A single thread is removed from the queue of blocked threads, and awoken.
1010  * This function must only be called while disabled.
1011  *
1012  * \param disp   Dispatcher pointer
1013  * \param queue  Queue of threads from which to unblock one
1014  * \param reason Value to be returned from thread_block()
1015  *
1016  * \returns Pointer to thread to be woken on a foreign dispatcher
1017  */
1018 struct thread *thread_unblock_one_disabled(dispatcher_handle_t handle,
1019                                            struct thread **queue,
1020                                            void *reason)
1021 {
1022     assert_disabled(queue != NULL);
1023
1024     // Any threads in queue?
1025     if (*queue == NULL) {
1026         return NULL;
1027     }
1028
1029     // Wakeup one waiting thread
1030     struct thread *wakeup = thread_dequeue(queue);
1031     wakeup->wakeup_reason = reason;
1032     assert_disabled(wakeup->state == THREAD_STATE_BLOCKED);
1033     wakeup->state = THREAD_STATE_RUNNABLE;
1034
1035     /* enqueue on run queue if it's "our" thread, and not paused */
1036     if (wakeup->disp == handle) {
1037         if (!wakeup->paused) {
1038             struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
1039             thread_enqueue(wakeup, &disp_gen->runq);
1040         }
1041         return NULL;
1042     } else {
1043         return wakeup;
1044     }
1045 }
1046
1047 /**
1048  * \brief Unblock a single thread from a given queue, while enabled
1049  *
1050  * A single thread is removed from the queue of blocked threads, and awoken.
1051  * This function must only be called while enabled.
1052  *
1053  * \param queue  Queue of threads from which to unblock one
1054  * \param reason Value to be returned from thread_block()
1055  *
1056  * \returns Pointer to thread to be woken on a foreign dispatcher
1057  */
1058 struct thread *thread_unblock_one(struct thread **queue, void *reason)
1059 {
1060     struct thread *thread;
1061
1062     dispatcher_handle_t handle = disp_disable();
1063     thread = thread_unblock_one_disabled(handle, queue, reason);
1064     disp_enable(handle);
1065     return thread;
1066 }
1067
1068 /**
1069  * \brief Unblock all threads on a given queue, while disabled
1070  *
1071  * All threads on the queue of blocked threads are awoken.
1072  * This function must only be called while disabled.
1073  *
1074  * \param disp   Dispatcher pointer
1075  * \param queue  Queue of threads to unblock
1076  * \param reason Value to be returned from thread_block()
1077  *
1078  * \returns Pointer to list of threads to be woken on a foreign dispatcher
1079  */
1080 struct thread *thread_unblock_all_disabled(dispatcher_handle_t handle,
1081                                            struct thread **queue, void *reason)
1082 {
1083     assert_disabled(queue != NULL);
1084     struct thread *wakeupq = NULL;
1085
1086     // Wakeup all waiting threads
1087     while (*queue != NULL) {
1088         struct thread *wakeup = thread_unblock_one_disabled(handle, queue, reason);
1089         if (wakeup != NULL) {
1090             wakeup->next = wakeupq;
1091             wakeupq = wakeup;
1092         }
1093     }
1094
1095     return wakeupq;
1096 }
1097
1098 extern int _main(int argc, const char *argv[]);
1099
1100 /// Thread created in new domain that runs main()
1101 static int main_thread(void *params)
1102 {
1103     struct spawn_domain_params *p = params;
1104     exit(_main(p->argc, p->argv));
1105     return EXIT_FAILURE;
1106 }
1107
1108 static bool init_domain_global; // XXX
1109
1110 /// Thread created on static stack in new domain that runs init code
1111 static int bootstrap_thread(struct spawn_domain_params *params)
1112 //int bootstrap_thread(struct spawn_domain_params *params);
1113 //int bootstrap_thread(struct spawn_domain_params *params)
1114 {
1115     errval_t err;
1116
1117     // Set libc function pointers
1118     barrelfish_libc_glue_init();
1119
1120     if (params == NULL) {
1121         printf("%s: error in creating a thread, NULL parameters given\n",
1122                 disp_name());
1123     }
1124     assert(params != NULL);
1125
1126     // Do we have TLS data?
1127     tls_block_init_base = params->tls_init_base;
1128     tls_block_init_len = params->tls_init_len;
1129     tls_block_total_len = params->tls_total_len;
1130
1131     // Initialize subsystems
1132     err = barrelfish_init_onthread(params);
1133     if (err_is_fail(err)) {
1134         DEBUG_ERR(err, "error during libbarrelfish init");
1135         exit(EXIT_FAILURE);
1136         assert(!"exit returned!");
1137     }
1138
1139     // Allocate storage region for real threads
1140     size_t blocksize = sizeof(struct thread) + tls_block_total_len;
1141     err = vspace_mmu_aware_init(&thread_slabs_vm, MAX_THREADS * blocksize);
1142     if (err_is_fail(err)) {
1143         USER_PANIC_ERR(err, "vspace_mmu_aware_init for thread region failed\n");
1144     }
1145     slab_init(&thread_slabs, blocksize, refill_thread_slabs);
1146
1147     if (init_domain_global) {
1148         // run main() on this thread, since we can't allocate
1149         if (tls_block_total_len > 0) {
1150             USER_PANIC("unsupported: use of TLS data in bootstrap domain\n");
1151         }
1152         main_thread(params);
1153     } else {
1154         // Start real thread to run main()
1155         struct thread *thread = thread_create(main_thread, params);
1156         assert(thread != NULL);
1157     }
1158
1159     return 0; // ignored
1160 }
1161
1162 /**
1163  * \brief Initialise thread system while still disabled
1164  *
1165  * This function initialises the thread system while the dispatcher is still
1166  * disabled, before enabling the dispatcher, running the general initialisation
1167  * code, and calling main().
1168  *
1169  * \param disp Dispatcher pointer
1170  * \param init_domain True if we are a bootstrap domain
1171  */
1172 void thread_init_disabled(dispatcher_handle_t handle, bool init_domain)
1173 {
1174     struct dispatcher_shared_generic *disp =
1175         get_dispatcher_shared_generic(handle);
1176     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
1177     arch_registers_state_t *enabled_area =
1178         dispatcher_get_enabled_save_area(handle);
1179
1180     init_domain_global = init_domain;
1181
1182     // Create the first thread manually
1183     struct thread *thread = &staticthread;
1184     staticthread_lock.locked = true; // XXX: safe while disabled
1185
1186     // waste space for alignment, if unaligned
1187     thread->stack_top = (char *)thread->stack_top
1188         - (lvaddr_t)thread->stack_top % STACK_ALIGNMENT;
1189
1190     // Initialise the first (static) thread
1191     thread_init(handle, thread);
1192     thread->detached = true;
1193
1194 #if defined(__x86_64__)
1195     // create segment for TCB
1196     errval_t err = ldt_alloc_segment_disabled(handle, thread,
1197                                               &thread->thread_seg_selector);
1198     if (err_is_fail(err)) {
1199         USER_PANIC_ERR(err, "error allocating LDT segment for first thread");
1200     }
1201 #endif
1202
1203     uintptr_t param;
1204     registers_get_param(enabled_area, &param);
1205
1206     registers_set_initial(&thread->regs, thread, (lvaddr_t)thread_entry,
1207                           /* TODO: pass stack base and limit, choose in arch
1208                            * code (possibly setting up other hints on stack) */
1209                           (lvaddr_t)thread->stack_top,
1210                           (lvaddr_t)bootstrap_thread, param, 0, 0);
1211
1212     // Switch to it (always on this dispatcher)
1213     thread->disp = handle;
1214     thread_enqueue(thread, &disp_gen->runq);
1215     disp_gen->current = thread;
1216     disp->haswork = true;
1217     disp_resume(handle, &thread->regs);
1218 }
1219
1220 /**
1221  * \brief Called on the remote core when spanning a domain across cores
1222  *
1223  * Runs the provided thread after enqueuing it and enabling the dispatcher
1224  */
1225 void thread_init_remote(dispatcher_handle_t handle, struct thread *thread)
1226 {
1227     struct dispatcher_shared_generic *disp =
1228         get_dispatcher_shared_generic(handle);
1229     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
1230     thread_enqueue(thread, &disp_gen->runq);
1231     disp_gen->current = thread;
1232     disp->haswork = true;
1233     disp_resume(handle, &thread->regs);
1234 }
1235
1236 /**
1237  * \brief Prepare to span the current domain
1238  *
1239  * This is a kludge. It is called from domain.c when creating a new dispatcher,
1240  * and is responsible for pre-allocating all the storage that might be needed
1241  * for thread metadata in the slab allocator. It can go away once we sanely
1242  * manage the vspace across multiple dispatchers in a domain.
1243  */
1244 void threads_prepare_to_span(dispatcher_handle_t newdh)
1245 {
1246     static bool called;
1247
1248     if (!called) {
1249         called = true;
1250
1251         thread_mutex_lock(&thread_slabs_mutex);
1252         acquire_spinlock(&thread_slabs_spinlock);
1253
1254         while (slab_freecount(&thread_slabs) < MAX_THREADS - 1) {
1255             struct capref frame;
1256             size_t size;
1257             void *buf;
1258             errval_t err;
1259
1260             size_t blocksize = sizeof(struct thread) + tls_block_total_len;
1261             err = vspace_mmu_aware_map(&thread_slabs_vm, blocksize,
1262                                        &buf, &size);
1263             if (err_is_fail(err)) {
1264                 slot_free(frame);
1265                 if (err_no(err) == LIB_ERR_VSPACE_MMU_AWARE_NO_SPACE) {
1266                     // we've wasted space with fragmentation
1267                     // cross our fingers and hope for the best...
1268                     break;
1269                 }
1270                 USER_PANIC_ERR(err, "in vspace_mmu_aware_map while prefilling "
1271                                "thread slabs\n");
1272             }
1273
1274             slab_grow(&thread_slabs, buf, size);
1275         }
1276
1277         release_spinlock(&thread_slabs_spinlock);
1278         thread_mutex_unlock(&thread_slabs_mutex);
1279     }
1280 }
1281
1282 /**
1283  * \brief Pause (suspend execution of) the given thread, and optionally capture its register state
1284  *
1285  * The thread will not be run, until a subsequent call to thread_resume()
1286  */
1287 void thread_pause_and_capture_state(struct thread *thread,
1288                                     arch_registers_state_t **ret_regs,
1289                                     arch_registers_fpu_state_t **ret_fpuregs)
1290 {
1291     assert(thread != NULL);
1292     dispatcher_handle_t dh = disp_disable();
1293     struct dispatcher_generic *disp = get_dispatcher_generic(dh);
1294     if (thread->disp == dh) {
1295         if (!thread->paused) {
1296             thread->paused = true;
1297             if (thread == disp->current) { // doesn't make much sense...
1298                 sys_print("Warning: pausing current thread!\n",100);
1299                 assert_disabled(thread->state == THREAD_STATE_RUNNABLE);
1300                 thread_block_disabled(dh, NULL);
1301             } else if (thread->state == THREAD_STATE_RUNNABLE) {
1302                 thread_remove_from_queue(&disp->runq, thread);
1303             }
1304         }
1305         if (ret_regs != NULL) {
1306             *ret_regs = &thread->regs;
1307         }
1308         if (ret_fpuregs != NULL) {
1309             if (thread->used_fpu) {
1310                 // FIXME: this may not be the right FPU state?
1311                 *ret_fpuregs = &thread->fpu_state;
1312             } else {
1313                 *ret_fpuregs = NULL;
1314             }
1315         }
1316     } else {
1317         USER_PANIC("NYI: remote dispatcher thread_pause()");
1318     }
1319     disp_enable(dh);
1320 }
1321
1322 /**
1323  * \brief Pause (suspend execution of) the given thread
1324  *
1325  * The thread will not be run, until a subsequent call to thread_resume()
1326  */
1327 void thread_pause(struct thread *thread)
1328 {
1329     thread_pause_and_capture_state(thread, NULL, NULL);
1330 }
1331
1332 /**
1333  * \brief Resume execution of a thread previously suspended by thread_pause()
1334  */
1335 void thread_resume(struct thread *thread)
1336 {
1337     assert(thread != NULL);
1338     dispatcher_handle_t dh = disp_disable();
1339     struct dispatcher_generic *disp = get_dispatcher_generic(dh);
1340     if (thread->disp == dh) {
1341         if (thread->paused) {
1342             thread->paused = false;
1343             if (thread->state == THREAD_STATE_RUNNABLE) {
1344                 thread_enqueue(thread, &disp->runq);
1345             }
1346         }
1347     } else {
1348         USER_PANIC("NYI: remote dispatcher thread_resume()");
1349     }
1350     disp_enable(dh);
1351 }
1352
1353 /**
1354  * \brief Set old-style thread-local storage pointer.
1355  * \param p   User's pointer
1356  */
1357 void thread_set_tls(void *p)
1358 {
1359     struct thread *me = thread_self();
1360     me->userptr = p;
1361 }
1362
1363 void thread_set_tls_key(int key, void *p)
1364 {
1365     struct thread *me = thread_self();
1366     me->userptrs[key] = p;
1367 }
1368
1369 /**
1370  * \brief Return old-style thread-local storage pointer.
1371  * \return User's pointer, previously passed to thread_set_tls()
1372  */
1373 void *thread_get_tls(void)
1374 {
1375     struct thread *me = thread_self();
1376     return me->userptr;
1377 }
1378
1379 void *thread_get_tls_key(int key)
1380 {
1381     struct thread *me = thread_self();
1382     return me->userptrs[key];
1383 }
1384
1385 /**
1386  * \brief Set the exception handler function for the current thread.
1387  *        Optionally also change its stack, and return the old values.
1388  *
1389  * \param newhandler New exception handler. Pass NULL to disable an existing handler.
1390  * \param oldhandler If non-NULL, returns previous exception handler
1391  * \param new_stack_base If non-NULL, sets a new exception handler stack (base)
1392  * \param new_stack_top  If non-NULL, sets a new exception handler stack (top)
1393  * \param old_stack_base If non-NULL, returns previous stack base
1394  * \param old_stack_top If non-NULL, returns previous stack top
1395  */
1396 errval_t thread_set_exception_handler(exception_handler_fn newhandler,
1397                                       exception_handler_fn *oldhandler,
1398                                       void *new_stack_base, void *new_stack_top,
1399                                       void **old_stack_base, void **old_stack_top)
1400 {
1401     struct thread *me = thread_self();
1402
1403     if (oldhandler != NULL) {
1404         *oldhandler = me->exception_handler;
1405     }
1406
1407     if (old_stack_base != NULL) {
1408         *old_stack_base = me->exception_stack;
1409     }
1410
1411     if (old_stack_top != NULL) {
1412         *old_stack_top = me->exception_stack_top;
1413     }
1414
1415     me->exception_handler = newhandler;
1416
1417     if (new_stack_base != NULL && new_stack_top != NULL) {
1418         me->exception_stack = new_stack_base;
1419         me->exception_stack_top = new_stack_top;
1420     }
1421
1422     return SYS_ERR_OK;
1423 }
1424
1425 static void exception_handler_wrapper(arch_registers_state_t *cpuframe,
1426                                       arch_registers_fpu_state_t *fpuframe,
1427                                       uintptr_t hack_arg, void *addr)
1428 {
1429     struct thread *me = thread_self();
1430
1431     assert(me->in_exception);
1432     assert(me->exception_handler != NULL);
1433
1434     // XXX: unpack hack arg
1435     enum exception_type type = hack_arg >> 16;
1436     int subtype = hack_arg & 0xffff;
1437
1438     // run handler
1439     me->exception_handler(type, subtype, addr, cpuframe, fpuframe);
1440
1441     // resume state
1442     dispatcher_handle_t dh = disp_disable();
1443     struct dispatcher_generic *disp_gen = get_dispatcher_generic(dh);
1444     //memcpy(&me->regs, cpuframe, sizeof(arch_registers_state_t));
1445
1446 #ifdef FPU_LAZY_CONTEXT_SWITCH
1447     if (fpuframe != NULL) {
1448         assert_disabled(me->used_fpu);
1449         arch_registers_fpu_state_t *dest;
1450         if (disp_gen->fpu_thread == me) {
1451             dest = dispatcher_get_enabled_fpu_save_area(dh);
1452         } else {
1453             dest = &me->fpu_state;
1454         }
1455         fpu_copy(dest, fpuframe);
1456         fpu_trap_on();
1457     }
1458 #endif
1459
1460     assert_disabled(me->in_exception);
1461     me->in_exception = false;
1462
1463     assert_disabled(disp_gen->current == me);
1464     disp_resume(dh, cpuframe);
1465 }
1466
1467 #if 0
1468 void thread_debug_regs(struct thread *t);
1469 void thread_debug_regs(struct thread *t)
1470 {
1471   printf("%d: RIP = %lx, RSP = %lx\n", disp_get_domain_id(),
1472          t->regs.rip, t->regs.rsp);
1473   uint64_t *stack = (uint64_t *)t->regs.rsp;
1474   printf("%d: ", disp_get_domain_id());
1475   for(int i = 0; i < 30; i++) {
1476     printf("%lx ", stack[i]);
1477   }
1478   printf("\n");
1479 }
1480 #endif
1481
1482 /**
1483  * \brief Deliver an exception to the current thread, and resume.
1484  *
1485  * This may only be called from the dispatcher (on its stack and while
1486  * disabled!).
1487  *
1488  * \param handle Dispatcher handle
1489  * \param type   Exception type
1490  * \param subtype Exception subtype
1491  * \param addr   Exception address
1492  * \param regs   CPU register state at time of exception
1493  */
1494 void thread_deliver_exception_disabled(dispatcher_handle_t handle,
1495                                        enum exception_type type, int subtype,
1496                                        void *addr, arch_registers_state_t *regs)
1497 {
1498     struct dispatcher_generic *disp_gen = get_dispatcher_generic(handle);
1499     struct thread *thread = disp_gen->current;
1500     assert_disabled(thread != NULL);
1501     assert_disabled(disp_gen->runq != NULL);
1502
1503     // can we deliver the exception?
1504     if (thread->exception_handler == NULL || thread->exception_stack_top == NULL
1505         || thread->in_exception) {
1506         if (thread->in_exception) {
1507             sys_print("Can't deliver exception to thread: already in handler\n",
1508                       100);
1509         } else {
1510             sys_print("Can't deliver exception to thread: handler not set\n",
1511                       100);
1512         }
1513
1514         // warn on stack overflow.
1515         lvaddr_t sp = (lvaddr_t) registers_get_sp(regs);
1516         if (sp < (lvaddr_t)thread->stack ||
1517             sp > (lvaddr_t)thread->stack_top) {
1518             char str[256];
1519             snprintf(str, sizeof(str), "Error: stack bounds exceeded: sp = 0x%"
1520                      PRIxPTR " but [bottom, top] = [0x%" PRIxPTR ", 0x%"
1521                      PRIxPTR "]\n", (lvaddr_t) sp, (lvaddr_t) thread->stack,
1522                      (lvaddr_t) thread->stack_top);
1523             sys_print(str, sizeof(str));
1524         }
1525
1526         // TODO: actually delete the thread!
1527         disp_gen->current = NULL;
1528         thread_remove_from_queue(&disp_gen->runq, thread);
1529         return;
1530     }
1531
1532     thread->in_exception = true;
1533
1534     lvaddr_t stack_top = (lvaddr_t)thread->exception_stack_top;
1535
1536     // save thread's state at time of fault on top of exception stack
1537     stack_top -= sizeof(arch_registers_state_t);
1538     arch_registers_state_t *cpuframe = (void *)stack_top;
1539     memcpy(cpuframe, regs, sizeof(arch_registers_state_t));
1540
1541     arch_registers_fpu_state_t *fpuframe = NULL;
1542 #ifdef FPU_LAZY_CONTEXT_SWITCH
1543     if (thread->used_fpu) {
1544         stack_top -= sizeof(arch_registers_fpu_state_t);
1545         fpuframe = (void *)stack_top;
1546         fpu_copy(fpuframe, &thread->fpu_state);
1547     }
1548 #endif
1549
1550     // align stack
1551     stack_top -= stack_top % STACK_ALIGNMENT;
1552
1553     // XXX: sanity-check to ensure we have a sensible amount of exception stack left
1554     assert_disabled(stack_top > (lvaddr_t)thread->exception_stack + 8192);
1555
1556     // XXX: pack two small ints together to fit into a single register
1557     uintptr_t hack_arg = (uintptr_t)type << 16 | (subtype & 0xffff);
1558
1559     registers_set_initial(&thread->regs, thread,
1560                           (lvaddr_t)exception_handler_wrapper,
1561                           stack_top, (lvaddr_t)cpuframe, (lvaddr_t)fpuframe,
1562                           hack_arg, (lvaddr_t)addr);
1563
1564     disp_resume(handle, &thread->regs);
1565 }