Add support for TLS if needed

[fw/altos] / src / kernel / ao_task.c

diff --git a/src/kernel/ao_task.c b/src/kernel/ao_task.c
index dc5c19132caeb121a21b5bbe0698da713a68c9d8..15cd2a556b68a5176732eaf3329e9eacf2490c83 100644 (file)
--- a/src/kernel/ao_task.c
+++ b/src/kernel/ao_task.c
@@ -24,6 +24,7 @@
 #if HAS_STACK_GUARD
 #include <ao_mpu.h>
 #endif
+#include <picotls.h>
 
 #define DEBUG  0
 
@@ -67,7 +68,7 @@ static inline void ao_check_stack(void) {
 
 static struct ao_list  run_queue;
 static struct ao_list  alarm_queue;
-static struct ao_list  sleep_queue[SLEEP_HASH_SIZE];
+static struct ao_list  ao_sleep_queue[SLEEP_HASH_SIZE];
 
 static void
 ao_task_to_run_queue(struct ao_task *task)
@@ -80,7 +81,7 @@ ao_task_to_run_queue(struct ao_task *task)
 static struct ao_list *
 ao_task_sleep_queue(void *wchan)
 {
-       return &sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE];
+       return &ao_sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE];
 }
 
 static void
@@ -195,7 +196,7 @@ ao_task_init(void)
        ao_list_init(&alarm_queue);
        ao_task_alarm_tick = 0;
        for (i = 0; i < SLEEP_HASH_SIZE; i++)
-               ao_list_init(&sleep_queue[i]);
+               ao_list_init(&ao_sleep_queue[i]);
 }
 
 #if DEBUG
@@ -289,8 +290,23 @@ ao_task_validate(void)
 
 #endif /* HAS_TASK_QUEUE */
 
+static inline void *
+ao_stack_top(struct ao_task *task)
+{
+       uint8_t *top = &task->stack8[AO_STACK_SIZE];
+
+       /* Subtract off the TLS space, but keep the resulting
+        * stack 8-byte aligned
+        */
+#if USE_TLS
+       return top - ((_tls_size() + 7) & ~3);
+#else
+       return top;
+#endif
+}
+
 void
-ao_add_task(struct ao_task * task, void (*start)(void), const char *name) 
+ao_add_task(struct ao_task * task, void (*task_func)(void), const char *name) 
 {
        uint8_t task_id;
        uint8_t t;
@@ -310,7 +326,11 @@ ao_add_task(struct ao_task * task, void (*start)(void), const char *name)
         * Construct a stack frame so that it will 'return'
         * to the start of the task
         */
-       ao_arch_init_stack(task, start);
+       uint32_t *sp = ao_stack_top(task);
+#if USE_TLS
+       _init_tls(sp);
+#endif
+       ao_arch_init_stack(task, sp, task_func);
        ao_arch_critical(
 #if HAS_TASK_QUEUE
                ao_task_init_queue(task);
@@ -413,6 +433,9 @@ ao_yield(void) ao_arch_naked_define
 #endif
 #if AO_CHECK_STACK
        in_yield = 0;
+#endif
+#if USE_TLS
+       _set_tls(ao_stack_top(ao_cur_task));
 #endif
        ao_arch_restore_stack();
 }