libthreads.c

   1 #include <string.h>
   2 #include <stdlib.h>
   3 #include <ucontext.h>
   4 #include <stdio.h>
   5
   6 //#define CONFIG_DEBUG
   7
   8 #ifdef CONFIG_DEBUG
   9 #define DBG() do { printf("Here: %s, L%d\n", __func__, __LINE__); } while (0)
  10 #define DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__)
  11 #else
  12 #define DBG()
  13 #define DEBUG(fmt, ...)
  14 #endif
  15
  16 #define STACK_SIZE (1024 * 1024)
  17
  18 struct thread {
  19         void (*start_routine);
  20         void *arg;
  21         ucontext_t context;
  22         void *stack;
  23         int index;
  24 };
  25
  26 static struct thread *current;
  27 static ucontext_t *cleanup;
  28
  29 static void *stack_allocate(size_t size)
  30 {
  31         return malloc(size);
  32 }
  33
  34 int thread_create(struct thread *t, void (*start_routine), void *arg)
  35 {
  36         static int created;
  37         ucontext_t local;
  38
  39         DBG();
  40
  41         t->index = created++;
  42         DEBUG("create thread %d\n", t->index);
  43
  44         t->start_routine = start_routine;
  45         t->arg = arg;
  46
  47         /* Initialize state */
  48         getcontext(&t->context);
  49         t->stack = stack_allocate(STACK_SIZE);
  50         t->context.uc_stack.ss_sp = t->stack;
  51         t->context.uc_stack.ss_size = STACK_SIZE;
  52         t->context.uc_stack.ss_flags = 0;
  53         if (current)
  54                 t->context.uc_link = &current->context;
  55         else
  56                 t->context.uc_link = cleanup;
  57         makecontext(&t->context, t->start_routine, 1, t->arg);
  58
  59         return 0;
  60 }
  61
  62 void thread_start(struct thread *t)
  63 {
  64         DBG();
  65
  66         if (current) {
  67                 struct thread *old = current;
  68                 current = t;
  69                 swapcontext(&old->context, &current->context);
  70         } else {
  71                 current = t;
  72                 swapcontext(cleanup, &current->context);
  73         }
  74         DBG();
  75 }
  76
  77 void a(int *idx)
  78 {
  79         int i;
  80
  81         for (i = 0; i < 10; i++)
  82                 printf("Thread %d, loop %d\n", *idx, i);
  83 }
  84
  85 void user_main()
  86 {
  87         struct thread t1, t2;
  88         int i = 1, j = 2;
  89
  90         thread_create(&t1, &a, &i);
  91         thread_create(&t2, &a, &j);
  92
  93         printf("user_main() is going to start 2 threads\n");
  94         thread_start(&t1);
  95         thread_start(&t2);
  96         printf("user_main() is finished\n");
  97 }
  98
  99 int main()
 100 {
 101         struct thread t;
 102         ucontext_t main_context;
 103
 104         cleanup = &main_context;
 105
 106         thread_create(&t, &user_main, NULL);
 107
 108         thread_start(&t);
 109
 110         DBG();
 111
 112         DEBUG("Exiting\n");
 113         return 0;
 114 }