diff options
Diffstat (limited to 'firmware/asm/thread-unix.c')
-rw-r--r-- | firmware/asm/thread-unix.c | 30 |
1 files changed, 26 insertions, 4 deletions
diff --git a/firmware/asm/thread-unix.c b/firmware/asm/thread-unix.c index 3c5e7c96ee..8538515a76 100644 --- a/firmware/asm/thread-unix.c +++ b/firmware/asm/thread-unix.c | |||
@@ -41,12 +41,31 @@ static pthread_t main_thread; | |||
41 | static struct ctx { | 41 | static struct ctx { |
42 | jmp_buf thread_buf; | 42 | jmp_buf thread_buf; |
43 | } thread_bufs[MAXTHREADS]; | 43 | } thread_bufs[MAXTHREADS]; |
44 | static threadbit_t free_thread_bufs; | ||
44 | static struct ctx* thread_context, *target_context; | 45 | static struct ctx* thread_context, *target_context; |
45 | static int curr_uc; | ||
46 | 46 | ||
47 | static void trampoline(int sig); | 47 | static void trampoline(int sig); |
48 | static void bootstrap_context(void) __attribute__((noinline)); | 48 | static void bootstrap_context(void) __attribute__((noinline)); |
49 | 49 | ||
50 | static void init_thread_bufs(void) | ||
51 | { | ||
52 | for (unsigned int bufidx = 0; bufidx < MAXTHREADS; bufidx++) | ||
53 | threadbit_set_bit(&free_thread_bufs, bufidx); | ||
54 | } | ||
55 | |||
56 | static struct ctx * alloc_thread_buf(void) | ||
57 | { | ||
58 | unsigned int bufidx = threadbit_ffs(&free_thread_bufs); | ||
59 | threadbit_clear_bit(&free_thread_bufs, bufidx); | ||
60 | return &thread_bufs[bufidx]; | ||
61 | } | ||
62 | |||
63 | static void free_thread_buf(struct ctx *threadbuf) | ||
64 | { | ||
65 | unsigned int bufidx = threadbuf - thread_bufs; | ||
66 | threadbit_set_bit(&free_thread_bufs, bufidx); | ||
67 | } | ||
68 | |||
50 | /* The *_context functions are heavily based on Gnu pth | 69 | /* The *_context functions are heavily based on Gnu pth |
51 | * http://www.gnu.org/software/pth/ | 70 | * http://www.gnu.org/software/pth/ |
52 | * | 71 | * |
@@ -228,6 +247,7 @@ void bootstrap_context(void) | |||
228 | */ | 247 | */ |
229 | thread_entry(); | 248 | thread_entry(); |
230 | DEBUGF("thread left\n"); | 249 | DEBUGF("thread left\n"); |
250 | free_thread_buf(t); | ||
231 | thread_exit(); | 251 | thread_exit(); |
232 | } | 252 | } |
233 | 253 | ||
@@ -238,7 +258,7 @@ static inline void set_context(struct ctx *c) | |||
238 | 258 | ||
239 | static inline void swap_context(struct ctx *old, struct ctx *new) | 259 | static inline void swap_context(struct ctx *old, struct ctx *new) |
240 | { | 260 | { |
241 | if (setjmp(old->thread_buf) == 0) | 261 | if (!old || setjmp(old->thread_buf) == 0) |
242 | longjmp(new->thread_buf, 1); | 262 | longjmp(new->thread_buf, 1); |
243 | } | 263 | } |
244 | 264 | ||
@@ -256,7 +276,8 @@ static void init_main_thread(void *addr) | |||
256 | /* get a context for the main thread so that we can jump to it from | 276 | /* get a context for the main thread so that we can jump to it from |
257 | * other threads */ | 277 | * other threads */ |
258 | struct regs *context = (struct regs*)addr; | 278 | struct regs *context = (struct regs*)addr; |
259 | context->uc = &thread_bufs[curr_uc++]; | 279 | init_thread_bufs(); |
280 | context->uc = alloc_thread_buf(); | ||
260 | get_context(context->uc); | 281 | get_context(context->uc); |
261 | } | 282 | } |
262 | 283 | ||
@@ -273,7 +294,7 @@ static void init_main_thread(void *addr) | |||
273 | static void setup_thread(struct regs *context) | 294 | static void setup_thread(struct regs *context) |
274 | { | 295 | { |
275 | void (*fn)(void) = context->start; | 296 | void (*fn)(void) = context->start; |
276 | context->uc = &thread_bufs[curr_uc++]; | 297 | context->uc = alloc_thread_buf(); |
277 | while (!make_context(context->uc, fn, (char*)context->stack, context->stack_size)) | 298 | while (!make_context(context->uc, fn, (char*)context->stack, context->stack_size)) |
278 | DEBUGF("Thread creation failed. Retrying"); | 299 | DEBUGF("Thread creation failed. Retrying"); |
279 | } | 300 | } |
@@ -304,4 +325,5 @@ static inline void load_context(const void* addr) | |||
304 | r->start = NULL; | 325 | r->start = NULL; |
305 | } | 326 | } |
306 | swap_context(target_context, r->uc); | 327 | swap_context(target_context, r->uc); |
328 | target_context = NULL; | ||
307 | } | 329 | } |