10 files changed, 359 insertions, 3 deletions

diff --git a/include/bh/bh.h b/include/bh/bh.h
index 277728e..a56e169 100644
--- a/include/bh/bh.h
+++ b/include/bh/bh.h
@@ -27,6 +27,7 @@
 #define BH_NOT_FOUND    0x0006
 #define BH_TIMEOUT      0x0007
 
+typedef void (*bh_generic_cb_t)(void *data);
 typedef int (*bh_equal_cb_t)(const void *, const void *);
 typedef size_t (*bh_hash_cb_t)(const void *);
 
diff --git a/include/bh/internal/thread.h b/include/bh/internal/thread.h
index 34e2f76..8ef9487 100644
--- a/include/bh/internal/thread.h
+++ b/include/bh/internal/thread.h
@@ -6,6 +6,8 @@
 
 #define BH_THREAD_DONE      0x0100
 
+#define BH_MAX_TLS          32
+
 #if defined(BH_USE_PTHREAD)
 #include "thread_posix.h"
 #elif defined(BH_USE_WINTHREAD)
@@ -33,6 +35,18 @@ struct bh_thread_pool_s
     int shutdown;
 };
 
+typedef struct bh_tls_info_s
+{
+    size_t counter;
+    bh_spinlock_t lock;
+    bh_generic_cb_t cleanup[BH_MAX_TLS];
+} bh_tls_info_t;
+
+typedef struct bh_tls_s
+{
+    void *data[BH_MAX_TLS];
+} bh_tls_t;
+
 void bh_task_init(bh_task_t *task,
                   void (*func)(void *),
                   void *data,
@@ -42,4 +56,14 @@ void bh_task_destroy(bh_task_t *task);
 
 void bh_thread_pool_worker(void *arg);
 
+void bh_spinlock_init(bh_spinlock_t *lock);
+
+void bh_spinlock_destroy(bh_spinlock_t *lock);
+
+bh_tls_info_t *bh_tls_info(void);
+
+bh_tls_t *bh_tls_fetch(void);
+
+void bh_tls_cleanup(void);
+
 #endif /* BH_INTERNAL_THREAD_H */
diff --git a/include/bh/internal/thread_null.h b/include/bh/internal/thread_null.h
index 035d585..bcf476f 100644
--- a/include/bh/internal/thread_null.h
+++ b/include/bh/internal/thread_null.h
@@ -18,6 +18,11 @@ struct bh_cond_s
     void *handle;
 };
 
+struct bh_spinlock_s
+{
+    int handle;
+};
+
 int bh_thread_init(bh_thread_t *thread,
                    bh_task_t *task);
 
@@ -30,4 +35,4 @@ void bh_cond_destroy(bh_cond_t *cond);
 int bh_thread_pool_init(bh_thread_pool_t *pool,
                         size_t size);
 
-void bh_thread_pool_destroy(bh_thread_pool_t *pool);
+void bh_thread_pool_destroy(bh_thread_pool_t *pool);
\ No newline at end of file
diff --git a/include/bh/internal/thread_posix.h b/include/bh/internal/thread_posix.h
index f02ce3a..0226efd 100644
--- a/include/bh/internal/thread_posix.h
+++ b/include/bh/internal/thread_posix.h
@@ -22,6 +22,11 @@ struct bh_cond_s
     pthread_cond_t handle;
 };
 
+struct bh_spinlock_s
+{
+    int handle;
+};
+
 int bh_thread_init(bh_thread_t *thread,
                    bh_task_t *task);
 
diff --git a/include/bh/internal/thread_win.h b/include/bh/internal/thread_win.h
index 46365c8..d9d1f7e 100644
--- a/include/bh/internal/thread_win.h
+++ b/include/bh/internal/thread_win.h
@@ -39,6 +39,11 @@ struct bh_cond_s
 };
 #endif
 
+struct bh_spinlock_s
+{
+    LONG handle;
+};
+
 int bh_thread_init_base(bh_thread_t *thread,
                         bh_task_t *task,
                         bh_thread_begin_cb_t begin,
diff --git a/include/bh/thread.h b/include/bh/thread.h
index 22bdf30..3360c42 100644
--- a/include/bh/thread.h
+++ b/include/bh/thread.h
@@ -12,6 +12,7 @@ typedef struct bh_semaphore_s bh_semaphore_t;
 typedef struct bh_cond_s bh_cond_t;
 typedef struct bh_task_s bh_task_t;
 typedef struct bh_thread_pool_s bh_thread_pool_t;
+typedef struct bh_spinlock_s bh_spinlock_t;
 
 #if defined(BH_USE_PTHREAD)
 bh_thread_t *bh_thread_new(bh_task_t *task);
@@ -108,4 +109,20 @@ int bh_thread_pool_wait(bh_thread_pool_t *pool);
 
 void bh_thread_pool_free(bh_thread_pool_t *pool);
 
+bh_spinlock_t *bh_spinlock_new(void);
+
+void bh_spinlock_free(bh_spinlock_t *lock);
+
+int bh_spinlock_lock(bh_spinlock_t *lock);
+
+int bh_spinlock_unlock(bh_spinlock_t *lock);
+
+int bh_spinlock_try_lock(bh_spinlock_t *lock);
+
+int bh_tls_alloc(bh_generic_cb_t cleanup);
+
+void *bh_tls_get(int slot);
+
+void bh_tls_set(int slot, void* data);
+
 #endif /* BH_THREAD_H */
diff --git a/src/thread.c b/src/thread.c
index c3e536c..92edf5a 100644
--- a/src/thread.c
+++ b/src/thread.c
@@ -373,6 +373,74 @@ void bh_thread_pool_worker(void *arg)
     }
 }
 
+
+bh_spinlock_t *bh_spinlock_new(void)
+{
+    bh_spinlock_t *result;
+
+    result = malloc(sizeof(*result));
+    if (result)
+        bh_spinlock_init(result);
+
+    return result;
+}
+
+void bh_spinlock_free(bh_spinlock_t *lock)
+{
+    bh_spinlock_destroy(lock);
+    free(lock);
+}
+
+int bh_tls_alloc(bh_generic_cb_t cleanup)
+{
+    bh_tls_info_t *info;
+    int result;
+
+    /* Fetch TLS managment info */
+    info = bh_tls_info();
+    result = BH_ERROR;
+
+    /* Get new counter value and setup cleanup function */
+    bh_spinlock_lock(&info->lock);
+    if (info->counter < BH_MAX_TLS)
+    {
+        result = info->counter++;
+        info->cleanup[result] = cleanup;
+    }
+    bh_spinlock_unlock(&info->lock);
+
+    /* Return slot id */
+    return result;
+}
+
+void *bh_tls_get(int slot)
+{
+    return bh_tls_fetch()->data[slot];
+}
+
+void bh_tls_set(int slot, void *data)
+{
+    bh_tls_fetch()->data[slot] = data;
+}
+
+void bh_tls_cleanup(void)
+{
+    bh_tls_info_t *info;
+    bh_tls_t *tls;
+    size_t i;
+
+    /* Fetch TLS managment info and TLS itself */
+    info = bh_tls_info();
+    tls = bh_tls_fetch();
+
+    /* Call cleanup for every slot */
+    for (i = 0; i < info->counter; i++)
+        if (info->cleanup[i])
+            info->cleanup[i](tls->data[i]);
+
+    /* Free tls */
+    free(tls);
+}
 /**
  * \}
  */
\ No newline at end of file
diff --git a/src/thread_null.c b/src/thread_null.c
index 25d8a88..212174a 100644
--- a/src/thread_null.c
+++ b/src/thread_null.c
@@ -367,6 +367,45 @@ int bh_thread_pool_init(bh_thread_pool_t *pool,
     return BH_NO_IMPL;
 }
 
+void bh_spinlock_init(bh_spinlock_t *lock)
+{
+    lock->handle = 0;
+}
+
+void bh_spinlock_destroy(bh_spinlock_t *lock)
+{
+    (void)lock;
+}
+
+int bh_spinlock_lock(bh_spinlock_t *lock)
+{
+    return BH_NO_IMPL;
+}
+
+int bh_spinlock_unlock(bh_spinlock_t *lock)
+{
+    return BH_NO_IMPL;
+}
+
+int bh_spinlock_try_lock(bh_spinlock_t *lock)
+{
+    return BH_NO_IMPL;
+}
+
+bh_tls_info_t *bh_tls_info(void)
+{
+    static bh_tls_info_t info;
+
+    return &info;
+}
+
+bh_tls_t *bh_tls_fetch(void)
+{
+    static bh_tls_t tls;
+
+    return &tls;
+}
+
 /**
  * \}
  */
\ No newline at end of file
diff --git a/src/thread_posix.c b/src/thread_posix.c
index 2898644..84e8e09 100644
--- a/src/thread_posix.c
+++ b/src/thread_posix.c
@@ -4,9 +4,15 @@
 
 static void *bh_thread_run(void *arg)
 {
+    bh_tls_t *tls;
     bh_task_t *task;
+
+    /* Fetch thread data */
     task = (bh_task_t *)arg;
 
+    /* Setup TLS */
+    tls = bh_tls_fetch();
+
     /* Do the task, mark as done, and if required free it */
     task->func(task->data);
     task->flags |= BH_THREAD_DONE;
@@ -14,6 +20,9 @@ static void *bh_thread_run(void *arg)
     if (task->flags & BH_THREAD_CLEANUP)
         bh_task_free(task);
 
+    /* Destroy TLS */
+    bh_tls_cleanup();
+
     return NULL;
 }
 
@@ -283,3 +292,100 @@ bh_thread_pool_t *bh_thread_pool_new(size_t size)
 
     return result;
 }
+
+void bh_spinlock_init(bh_spinlock_t *lock)
+{
+    lock->handle = 0;
+}
+
+void bh_spinlock_destroy(bh_spinlock_t *lock)
+{
+    (void)lock;
+}
+
+int bh_spinlock_lock(bh_spinlock_t *lock)
+{
+    /* TODO: Check on other OS and compilers */
+#if defined(__clang__) || defined(__GNUC__)
+    while (__sync_lock_test_and_set(&lock->handle, 1));
+    return BH_OK;
+#else
+#error "Spinlocks are not supported"
+    return BH_NO_IMPL;
+#endif
+}
+
+int bh_spinlock_unlock(bh_spinlock_t *lock)
+{
+    /* TODO: Check on other OS and compilers */
+#if defined(__clang__) || defined(__GNUC__)
+    if (__sync_lock_test_and_set(&lock->handle, 0))
+        return BH_OK;
+    return BH_ERROR;
+#else
+#error "Spinlocks are not supported"
+    return BH_NO_IMPL;
+#endif
+}
+
+int bh_spinlock_try_lock(bh_spinlock_t *lock)
+{
+    /* TODO: Check on other OSs and compilers */
+#if defined(__clang__) || defined(__GNUC__)
+    if (__sync_lock_test_and_set(&lock->handle, 1))
+        return BH_ERROR;
+    return BH_OK;
+#else
+#error "Spinlocks are not supported"
+    return BH_NO_IMPL;
+#endif
+}
+
+bh_tls_info_t *bh_tls_info(void)
+{
+    static bh_tls_info_t info = { 0, 0 };
+
+    return &info;
+}
+
+bh_tls_t *bh_tls_fetch(void)
+{
+    static bh_spinlock_t tls_lock = { 0 };
+    static int tls_ready = 0;
+    static pthread_key_t tls_key;
+
+    bh_tls_t *tls;
+
+    /* Protect pthread key by spinlock */
+    bh_spinlock_lock(&tls_lock);
+
+    /* Allocate pthread key if it's not allocated */
+    if (tls_ready == 0)
+    {
+        pthread_key_create(&tls_key, NULL);
+        tls_ready = 1;
+    }
+
+    /* Unlock spinlock */
+    bh_spinlock_unlock(&tls_lock);
+
+    /* Get TLS pointer and setup TLS if neccesery */
+    tls = (bh_tls_t *)pthread_getspecific(tls_key);
+    if (tls == NULL)
+    {
+        /* Allocate space for TLS */
+        tls = malloc(sizeof(*tls));
+        if (!tls)
+            abort();
+
+        /* Zero out TLS */
+        memset(tls, 0, sizeof(*tls));
+
+        /* Set TLS value */
+        if (pthread_setspecific(tls_key, tls))
+            abort();
+    }
+
+    /* Return TLS */
+    return tls;
+}
\ No newline at end of file
diff --git a/src/thread_win.c b/src/thread_win.c
index bb14774..92a5bb4 100644
--- a/src/thread_win.c
+++ b/src/thread_win.c
@@ -2,12 +2,16 @@
 
 static unsigned __stdcall bh_thread_run(void *arg)
 {
+    bh_tls_t *tls;
     bh_thread_data_t data;
 
     /* Fetch thread data, store it on stack and free from heap */
     data = *(bh_thread_data_t *)arg;
     free(arg);
 
+    /* Setup TLS */
+    tls = bh_tls_fetch();
+
     /* Do the task, mark as done, and if required free it */
     data.task->func(data.task->data);
     data.task->flags |= BH_THREAD_DONE;
@@ -15,8 +19,12 @@ static unsigned __stdcall bh_thread_run(void *arg)
     if (data.task->flags & BH_THREAD_CLEANUP)
         bh_task_free(data.task);
 
-    /* Call thread specific end function (deallocate TLS) */
+    /* Destroy our TLS */
+    bh_tls_cleanup();
+
+    /* Call thread specific end function (deallocate libc TLS) */
     data.end(0);
+
     return 0;
 }
 
@@ -58,7 +66,7 @@ bh_thread_t *bh_thread_new_base(bh_task_t *task,
 
     /* Allocate thread object */
     result = malloc(sizeof(*result));
-    if (result && !bh_thread_init_base(result, task, begin, end))
+    if (result && bh_thread_init_base(result, task, begin, end))
     {
         free(result);
         result = NULL;
@@ -439,3 +447,81 @@ bh_thread_pool_t *bh_thread_pool_new_base(size_t size,
     return result;
 }
 
+void bh_spinlock_init(bh_spinlock_t *lock)
+{
+    lock->handle = 0;
+}
+
+void bh_spinlock_destroy(bh_spinlock_t *lock)
+{
+    (void)lock;
+}
+
+int bh_spinlock_lock(bh_spinlock_t *lock)
+{
+    while (InterlockedExchange(&lock->handle, 1));
+    return BH_OK;
+}
+
+int bh_spinlock_unlock(bh_spinlock_t *lock)
+{
+    if (InterlockedExchange(&lock->handle, 0))
+        return BH_OK;
+    return BH_ERROR;
+}
+
+int bh_spinlock_try_lock(bh_spinlock_t *lock)
+{
+    if (InterlockedExchange(&lock->handle, 1))
+        return BH_ERROR;
+    return BH_OK;
+}
+
+bh_tls_info_t *bh_tls_info(void)
+{
+    static bh_tls_info_t info = {0, 0};
+
+    return &info;
+}
+
+bh_tls_t *bh_tls_fetch(void)
+{
+    static bh_spinlock_t tls_lock = {0};
+    static DWORD tls_index = TLS_OUT_OF_INDEXES;
+
+    bh_tls_t *tls;
+
+    /* Protect TLS index by spinlock */
+    bh_spinlock_lock(&tls_lock);
+
+    /* Allocate TLS index if it's not allocated */
+    if (tls_index == TLS_OUT_OF_INDEXES)
+        tls_index = TlsAlloc();
+
+    /* Unlock spinlock */
+    bh_spinlock_unlock(&tls_lock);
+
+    /* Check that TLS index is valid */
+    if (tls_index == TLS_OUT_OF_INDEXES)
+        abort();
+
+    /* Get TLS pointer and setup TLS if neccesery */
+    tls = (bh_tls_t *)TlsGetValue(tls_index);
+    if (tls == NULL)
+    {
+        /* Allocate space for TLS */
+        tls = malloc(sizeof(*tls));
+        if (!tls)
+            abort();
+
+        /* Zero out TLS */
+        memset(tls, 0, sizeof(*tls));
+
+        /* Set TLS value */
+        if (!TlsSetValue(tls_index, tls))
+            abort();
+    }
+
+    /* Return TLS */
+    return tls;
+}