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 started;
  24         int index;
  25 };
  26
  27 static struct thread *current;
  28 static ucontext_t *cleanup;
  29
  30 static void *stack_allocate(size_t size)
  31 {
  32         return malloc(size);
  33 }
  34
  35 int thread_create(struct thread *t, void (*start_routine), void *arg)
  36 {
  37         static int created;
  38         ucontext_t local;
  39
  40         DBG();
  41
  42         t->index = created++;
  43         DEBUG("create thread %d\n", t->index);
  44
  45         t->start_routine = start_routine;
  46         t->arg = arg;
  47         t->started = 0;
  48
  49         /* Initialize state */
  50         getcontext(&t->context);
  51         t->stack = stack_allocate(STACK_SIZE);
  52         t->context.uc_stack.ss_sp = t->stack;
  53         t->context.uc_stack.ss_size = STACK_SIZE;
  54         t->context.uc_stack.ss_flags = 0;
  55         if (current)
  56                 t->context.uc_link = &current->context;
  57         else
  58                 t->context.uc_link = cleanup;
  59         makecontext(&t->context, t->start_routine, 1, t->arg);
  60
  61         return 0;
  62 }
  63
  64 void thread_start(struct thread *t)
  65 {
  66         DBG();
  67
  68         t->started = 1;
  69
  70         if (current) {
  71                 struct thread *old = current;
  72                 current = t;
  73                 swapcontext(&old->context, &current->context);
  74         } else {
  75                 current = t;
  76                 swapcontext(cleanup, &current->context);
  77         }
  78         DBG();
  79 }
  80
  81 void a(int *idx)
  82 {
  83         int i;
  84
  85         for (i = 0; i < 10; i++)
  86                 printf("Thread %d, loop %d\n", *idx, i);
  87 }
  88
  89 void user_main()
  90 {
  91         struct thread t1, t2;
  92         int i = 1, j = 2;
  93
  94         thread_create(&t1, &a, &i);
  95         thread_create(&t2, &a, &j);
  96
  97         printf("user_main() is going to start 2 threads\n");
  98         thread_start(&t1);
  99         thread_start(&t2);
 100         printf("user_main() is finished\n");
 101 }
 102
 103 int main()
 104 {
 105         struct thread t;
 106         ucontext_t main_context;
 107         int pass = 0;
 108
 109         cleanup = &main_context;
 110
 111         thread_create(&t, &user_main, NULL);
 112
 113         getcontext(&main_context);
 114         if (!pass++)
 115                 thread_start(&t);
 116
 117         DBG();
 118
 119         DEBUG("Exiting?\n");
 120         return 0;
 121 }