libthreads.c

   1 #include <string.h>
   2 #include <stdlib.h>
   3
   4 //#define CONFIG_DEBUG
   5
   6 #include "libthreads.h"
   7
   8 #define STACK_SIZE (1024 * 1024)
   9
  10 static struct thread *current, *main_thread;
  11
  12 static void *stack_allocate(size_t size)
  13 {
  14         return malloc(size);
  15 }
  16
  17 static int create_context(struct thread *t)
  18 {
  19         int ret;
  20
  21         memset(&t->context, 0, sizeof(t->context));
  22         ret = getcontext(&t->context);
  23         if (ret)
  24                 return ret;
  25
  26         /* t->start_routine == NULL means this is our initial context */
  27         if (!t->start_routine)
  28                 return 0;
  29
  30         /* Initialize new managed context */
  31         t->stack = stack_allocate(STACK_SIZE);
  32         t->context.uc_stack.ss_sp = t->stack;
  33         t->context.uc_stack.ss_size = STACK_SIZE;
  34         t->context.uc_stack.ss_flags = 0;
  35         t->context.uc_link = &main_thread->context;
  36         makecontext(&t->context, t->start_routine, 1, t->arg);
  37
  38         return 0;
  39 }
  40
  41 static int create_initial_thread(struct thread *t)
  42 {
  43         memset(t, 0, sizeof(*t));
  44         return create_context(t);
  45 }
  46
  47 static int thread_swap(struct thread *old, struct thread *new)
  48 {
  49         return swapcontext(&old->context, &new->context);
  50 }
  51
  52 static int thread_yield()
  53 {
  54         struct thread *old, *next;
  55
  56         DBG();
  57         if (current) {
  58                 old = current;
  59                 schedule_add_thread(old);
  60         } else {
  61                 old = main_thread;
  62         }
  63         schedule_choose_next(&next);
  64         current = next;
  65         return thread_swap(old, next);
  66 }
  67
  68 static int master_thread_yield()
  69 {
  70         struct thread *next;
  71
  72         DBG();
  73
  74         if (current) {
  75                 DEBUG("completed thread %d\n", current->index);
  76                 current->completed = 1;
  77         }
  78         schedule_choose_next(&next);
  79         if (next && !next->completed) {
  80                 current = next;
  81                 return thread_swap(main_thread, next);
  82         }
  83         return 1;
  84 }
  85
  86 int thread_create(struct thread *t, void (*start_routine), void *arg)
  87 {
  88         static int created = 1;
  89
  90         DBG();
  91
  92         memset(t, 0, sizeof(*t));
  93         t->index = created++;
  94         DEBUG("create thread %d\n", t->index);
  95
  96         t->start_routine = start_routine;
  97         t->arg = arg;
  98
  99         /* Initialize state */
 100         return create_context(t);
 101 }
 102
 103 void thread_start(struct thread *t)
 104 {
 105         DBG();
 106
 107         schedule_add_thread(t);
 108 }
 109
 110 void thread_join(struct thread *t)
 111 {
 112         while (!t->completed)
 113                 thread_yield();
 114 }
 115
 116 void a(int *idx)
 117 {
 118         int i;
 119
 120         for (i = 0; i < 10; i++) {
 121                 printf("Thread %d, loop %d\n", *idx, i);
 122                 if (i % 2)
 123                         thread_yield();
 124         }
 125 }
 126
 127 void user_main()
 128 {
 129         struct thread t1, t2;
 130         int i = 2, j = 3;
 131
 132         thread_create(&t1, &a, &i);
 133         thread_create(&t2, &a, &j);
 134
 135         printf("%s() is going to start 1 thread\n", __func__);
 136         thread_start(&t1);
 137         thread_start(&t2);
 138
 139         thread_join(&t1);
 140         thread_join(&t2);
 141         printf("%s() is finished\n", __func__);
 142 }
 143
 144 int main()
 145 {
 146         struct thread user_thread;
 147
 148         main_thread = malloc(sizeof(struct thread));
 149         create_initial_thread(main_thread);
 150
 151         thread_create(&user_thread, &user_main, NULL);
 152         thread_start(&user_thread);
 153
 154         /* Wait for all threads to complete */
 155         while (master_thread_yield() == 0);
 156
 157         DEBUG("Exiting\n");
 158         return 0;
 159 }