11 files changed, 514 insertions, 51 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c275f67..07b23f0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -66,6 +66,10 @@ if(WIN32)
             src/thread_null.c
         )
     endif()
+    
+    if(BHLIB_NO_WINXP)
+        add_definitions(-D_WIN32_WINNT=_WIN32_WINNT_VISTA -DWINVER=_WIN32_WINNT_VISTA)
+    endif()
 elseif(UNIX)
     message(STATUS "Platform: Unix (Linux/BSD/MacOS X)")
 
diff --git a/include/bh/config.in b/include/bh/config.in
index 5868721..21dea6e 100644
--- a/include/bh/config.in
+++ b/include/bh/config.in
@@ -3,5 +3,6 @@
 
 #cmakedefine BHLIB_USE_WINTHREAD
 #cmakedefine BHLIB_USE_PTHREAD
+#cmakedefine BHLIB_NO_WINXP
 
 #endif /* BHLIB_CONFIG_H */
diff --git a/include/bh/internal/thread_null.h b/include/bh/internal/thread_null.h
index 9a22da5..322ef26 100644
--- a/include/bh/internal/thread_null.h
+++ b/include/bh/internal/thread_null.h
@@ -8,6 +8,11 @@ struct bh_mutex_s
     void *handle;
 };
 
+struct bh_semaphore_s
+{
+    void *handle;
+}:
+
 struct bh_cond_s
 {
     void *handle;
diff --git a/include/bh/internal/thread_posix.h b/include/bh/internal/thread_posix.h
index bef12a9..f02ce3a 100644
--- a/include/bh/internal/thread_posix.h
+++ b/include/bh/internal/thread_posix.h
@@ -1,4 +1,5 @@
 #include <pthread.h>
+#include <semaphore.h>
 
 struct bh_thread_s
 {
@@ -11,6 +12,11 @@ struct bh_mutex_s
     pthread_mutex_t handle;
 };
 
+struct bh_semaphore_s
+{
+    sem_t handle;
+};
+
 struct bh_cond_s
 {
     pthread_cond_t handle;
@@ -22,6 +28,10 @@ int bh_thread_init(bh_thread_t *thread,
 int bh_mutex_init(bh_mutex_t *mutex);
 void bh_mutex_destroy(bh_mutex_t *mutex);
 
+int bh_semaphore_init(bh_semaphore_t *semaphore,
+                      int count);
+void bh_semaphore_destroy(bh_semaphore_t *semaphore);
+
 int bh_cond_init(bh_cond_t *cond);
 void bh_cond_destroy(bh_cond_t *cond);
 
diff --git a/include/bh/internal/thread_win.h b/include/bh/internal/thread_win.h
index e2108bc..57150f0 100644
--- a/include/bh/internal/thread_win.h
+++ b/include/bh/internal/thread_win.h
@@ -7,7 +7,7 @@ struct bh_thread_s
     int allocated;
 };
 
-struct bh_thread_data_s
+typedef struct bh_thread_data_s
 {
     bh_task_t *task;
     bh_thread_end_cb_t end;
@@ -18,14 +18,26 @@ struct bh_mutex_s
     CRITICAL_SECTION handle;
 };
 
+struct bh_semaphore_s
+{
+    HANDLE handle;
+};
+
+#if WINVER >= _WIN32_WINNT_VISTA
 struct bh_cond_s
 {
-    CRITICAL_SECTION lock;
-    HANDLE wait;
-    HANDLE done;
+    CONDITION_VARIABLE handle;
+};
+#else
+struct bh_cond_s
+{
+    bh_mutex_t lock;
+    bh_semaphore_t wait;
+    bh_semaphore_t done;
     int waiting;
     int signals;
 };
+#endif
 
 int bh_thread_init_base(bh_thread_t *thread,
                         bh_task_t *task,
@@ -38,6 +50,10 @@ int bh_thread_init_base(bh_thread_t *thread,
 int bh_mutex_init(bh_mutex_t *mutex);
 void bh_mutex_destroy(bh_mutex_t *mutex);
 
+int bh_semaphore_init(bh_semaphore_t *semaphore, 
+                      int count);
+void bh_semaphore_destroy(bh_semaphore_t *semaphore);
+
 int bh_cond_init(bh_cond_t *cond);
 void bh_cond_destroy(bh_cond_t *cond);
 
diff --git a/include/bh/thread.h b/include/bh/thread.h
index 3ec037f..cfc02e8 100644
--- a/include/bh/thread.h
+++ b/include/bh/thread.h
@@ -8,6 +8,7 @@
 typedef void (*bh_thread_cb_t)(void *);
 typedef struct bh_thread_s bh_thread_t;
 typedef struct bh_mutex_s bh_mutex_t;
+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;
@@ -189,6 +190,14 @@ int bh_mutex_try_lock(bh_mutex_t *mutex);
  */
 int bh_mutex_unlock(bh_mutex_t *mutex);
 
+bh_semaphore_t *bh_semaphore_new(int count);
+void bh_semaphore_free(bh_semaphore_t *semaphore);
+int bh_semaphore_post(bh_semaphore_t *semaphore);
+int bh_semaphore_wait(bh_semaphore_t *semaphore);
+int bh_semaphore_wait_for(bh_semaphore_t *semaphore,
+                          unsigned long timeout);
+int bh_semaphore_try_wait(bh_semaphore_t *semaphore);
+
 /**
  * @brief   Create condition variable.
  *
diff --git a/main.c b/main.c
index dc5dbaf..d4f4dba 100644
--- a/main.c
+++ b/main.c
@@ -45,34 +45,57 @@ int factor(int x)
 
 void factor_task(void *arg)
 {
-    printf("Task start\n");
-    fflush(stdout);
-    printf("Factor: %d\n", factor(42));
-    fflush(stdout);
+    //printf("Task start\n");
+    //fflush(stdout);
+    //printf("Factor: %d\n", factor(42));
+    //fflush(stdout);
+    volatile int i = factor(16);
+    
+    //printf("Factor: %d\n", factor(15));
 }
 
+#include <x86intrin.h>
+
 void bar()
 {
     bh_thread_pool_t *pool;
     bh_task_t *task;
     size_t i;
+    
+    bh_uint64_t start, end;
 
-    printf("Pool create\n");
-    fflush(stdout);
+    //printf("Pool create\n");
+    //fflush(stdout);
     pool = bh_thread_pool_new(16);
+    if (!pool)
+    {
+        //printf("Pool creation error\n");
+        return;
+    }
 
-    printf("Prepare\n");
-    fflush(stdout);
-    for (i = 0; i < 32; i++)
+    //printf("Prepare\n");
+    //fflush(stdout);
+    
+    start = __rdtsc();
+    
+    for (i = 0; i < 128 * 1024; i++)
     {
-        printf("Task create\n");
-        fflush(stdout);
+        //printf("Task create\n");
+        //fflush(stdout);
         task = bh_task_new(factor_task, NULL, BH_THREAD_CLEANUP);
         bh_thread_pool_add(pool, task);
     }
+    
+    //system("pause");
 
     bh_thread_pool_wait(pool);
+    end = __rdtsc();
     bh_thread_pool_free(pool);
+    
+    
+    
+    printf("Elapsed: %llu\n", end - start);
+    //system("pause");
 }
 
 int main()
@@ -81,11 +104,12 @@ int main()
     void *iter;
     size_t i, j;
 
-    foo();
+    //foo();
 
     printf("Thread?\n");
     fflush(stdout);
     bar();
+    return 0;
 
     queue = bh_queue_new();
 
diff --git a/src/thread.c b/src/thread.c
index 77f36bb..84bc2c0 100644
--- a/src/thread.c
+++ b/src/thread.c
@@ -68,6 +68,26 @@ void bh_mutex_free(bh_mutex_t *mutex)
     free(mutex);
 }
 
+bh_semaphore_t *bh_semaphore_new(int count)
+{
+    bh_semaphore_t *result;
+    
+    result = malloc(sizeof(*result));
+    if (result && bh_semaphore_init(result, count))
+    {
+        free(result);
+        result = NULL;
+    }
+    
+    return result;
+}
+
+void bh_semaphore_free(bh_semaphore_t *semaphore)
+{
+    bh_semaphore_destroy(semaphore);
+    free(semaphore);
+}
+
 bh_cond_t *bh_cond_new(void)
 {
     bh_cond_t *result;
diff --git a/src/thread_null.c b/src/thread_null.c
index 494e0b6..af73fd6 100644
--- a/src/thread_null.c
+++ b/src/thread_null.c
@@ -55,6 +55,43 @@ int bh_mutex_unlock(bh_mutex_t *mutex)
     return -1;
 }
 
+int bh_semaphore_init(bh_semaphore_t *semaphore, int count)
+{
+    (void)semaphore;
+    (void)count;
+    return -1;
+}
+
+void bh_semaphore_destroy(bh_semaphore_t *semaphore)
+{
+    (void)semaphore;
+}
+
+int bh_semaphore_post(bh_semaphore_t *semaphore)
+{
+    (void)semaphore;
+    return -1;
+}
+
+int bh_semaphore_wait(bh_semaphore_t *semaphore)
+{
+    (void)semaphore;
+    return -1;
+}
+
+int bh_semaphore_wait_for(bh_semaphore_t *semaphore,
+                          unsigned long timeout)
+{
+    (void)semaphore;
+    return -1;
+}
+
+int bh_semaphore_try_wait(bh_semaphore_t *semaphore)
+{
+    (void)semaphore;
+    return -1;
+}
+
 int bh_cond_init(bh_cond_t *cond)
 {
     (void)cond;
diff --git a/src/thread_posix.c b/src/thread_posix.c
index 22d4c62..21ca2be 100644
--- a/src/thread_posix.c
+++ b/src/thread_posix.c
@@ -87,6 +87,42 @@ int bh_mutex_unlock(bh_mutex_t *mutex)
     return pthread_mutex_unlock(&mutex->handle);
 }
 
+int bh_semaphore_init(bh_semaphore_t *semaphore, int count)
+{
+    return sem_init(&semaphore->handle, 0, count);
+}
+
+void bh_semaphore_destroy(bh_semaphore_t *semaphore)
+{
+    sem_destroy(&semaphore->handle);
+}
+
+int bh_semaphore_post(bh_semaphore_t *semaphore)
+{
+    return sem_post(&semaphore->handle);
+}
+
+int bh_semaphore_wait(bh_semaphore_t *semaphore)
+{
+    return sem_wait(&semaphore->handle);
+}
+
+int bh_semaphore_wait_for(bh_semaphore_t *semaphore,
+                          unsigned long timeout)
+{
+    struct timespec ts;
+
+    ts.tv_sec = timeout / 1000;
+    ts.tv_nsec = (timeout - ts.tv_sec * 1000) * 1000000;
+
+    return sem_timedwait(&semaphore->handle, &ts);
+}
+
+int bh_semaphore_try_wait(bh_semaphore_t *semaphore)
+{
+    return sem_trywait(&semaphore->handle);
+}
+
 int bh_cond_init(bh_cond_t *cond)
 {
     return pthread_cond_init(&cond->handle, NULL);
diff --git a/src/thread_win.c b/src/thread_win.c
index ae5f6a9..098e7dd 100644
--- a/src/thread_win.c
+++ b/src/thread_win.c
@@ -1,68 +1,167 @@
 #include <bh/internal/thread.h>
 
+static unsigned __stdcall bh_thread_run(void *arg)
+{
+    bh_thread_data_t data;
+    
+    data = *(bh_thread_data_t *)arg;
+    free(arg);
+    
+    data.task->func(data.task->data);
+    data.task->flags |= BH_THREAD_DONE;
+
+    if (data.task->flags & BH_THREAD_CLEANUP)
+        bh_task_free(data.task);
+    
+    data.end(0);
+    return 0;
+}
+
 int bh_thread_init_base(bh_thread_t *thread,
                         bh_task_t *task,
                         bh_thread_begin_cb_t begin,
                         bh_thread_end_cb_t end)
 {
-    (void)thread;
-    (void)task;
-    return -1;
+    bh_thread_data_t *data;
+    
+    data = malloc(sizeof(*data));
+    if (!data)
+        return -1;
+    
+    data->task = task;
+    data->end = end;
+    
+    thread->allocated = 0;
+    thread->handle = (HANDLE)_beginthreadex(NULL, 0, bh_thread_run, data, 0, NULL);
+    
+    if (!thread->handle)
+    {
+        free(data);
+        return -1;
+    }
+    
+    return 0;
 }
 
 bh_thread_t *bh_thread_new_base(bh_task_t *task,
                                 bh_thread_begin_cb_t begin,
                                 bh_thread_end_cb_t end)
 {
-    (void)task;
-    return NULL;
+    bh_thread_t *result;
+    
+    result = malloc(sizeof(*result));
+    if (result && !bh_thread_init_base(result, task, begin, end))
+    {
+        free(result);
+        result = NULL;
+    }
+    
+    if (result)
+        result->allocated = 1;
+    
+    return result;
 }
 
 int bh_thread_join(bh_thread_t *thread)
 {
-    (void)thread;
-    return -1;
+    WaitForSingleObject(thread->handle, INFINITE);
+    CloseHandle(thread->handle);
+    
+    if (thread->allocated)
+        free(thread);
+    
+    return 0;
 }
 
 int bh_thread_detach(bh_thread_t *thread)
 {
-    (void)thread;
-    return -1;
+    CloseHandle(thread->handle);
+    
+    if (thread->allocated)
+        free(thread);
+    
+    return 0;
 }
 
 int bh_mutex_init(bh_mutex_t *mutex)
 {
-    (void)mutex;
-    return -1;
+    if (!InitializeCriticalSectionAndSpinCount(&mutex->handle, 0x400))
+        return -1;
+    return 0;
 }
 
 void bh_mutex_destroy(bh_mutex_t *mutex)
 {
-    (void)mutex;
+    DeleteCriticalSection(&mutex->handle);
 }
 
 int bh_mutex_lock(bh_mutex_t *mutex)
 {
-    (void)mutex;
-    return -1;
+    EnterCriticalSection(&mutex->handle);
+    return 0;
 }
 
 int bh_mutex_try_lock(bh_mutex_t *mutex)
 {
-    (void)mutex;
-    return -1;
+    if (!TryEnterCriticalSection(&mutex->handle))
+        return -1;
+    return 0;
 }
 
 int bh_mutex_unlock(bh_mutex_t *mutex)
 {
-    (void)mutex;
-    return -1;
+    LeaveCriticalSection(&mutex->handle);
+    return 0;
+}
+
+int bh_semaphore_init(bh_semaphore_t *semaphore, int count)
+{
+    semaphore->handle = CreateSemaphore(NULL, count, 0x7FFF, NULL);
+    if (!semaphore->handle)
+        return -1;
+    
+    return 0;
+}
+
+void bh_semaphore_destroy(bh_semaphore_t *semaphore)
+{
+    CloseHandle(semaphore->handle);
+}
+
+int bh_semaphore_post(bh_semaphore_t *semaphore)
+{
+    if (!ReleaseSemaphore(semaphore->handle, 1, NULL))
+        return -1;
+    return 0;
+}
+
+int bh_semaphore_wait(bh_semaphore_t *semaphore)
+{
+    if (WaitForSingleObject(semaphore->handle, INFINITE))
+        return -1;
+    return 0;
+}
+
+int bh_semaphore_wait_for(bh_semaphore_t *semaphore,
+                          unsigned long timeout)
+{
+    if (WaitForSingleObject(semaphore->handle, timeout))
+        return -1;
+    return 0;
 }
 
+int bh_semaphore_try_wait(bh_semaphore_t *semaphore)
+{
+    if (WaitForSingleObject(semaphore->handle, 0))
+        return -1;
+    return 0;
+}
+
+#if WINVER >= _WIN32_WINNT_VISTA
 int bh_cond_init(bh_cond_t *cond)
 {
-    (void)cond;
-    return -1;
+    InitializeConditionVariable(&cond->handle);
+    return 0;
 }
 
 void bh_cond_destroy(bh_cond_t *cond)
@@ -73,40 +172,235 @@ void bh_cond_destroy(bh_cond_t *cond)
 int bh_cond_wait(bh_cond_t *cond,
                  bh_mutex_t *mutex)
 {
-    (void)cond;
-    (void)mutex;
-    return -1;
+    return bh_cond_wait_for(cond, mutex, INFINITE);
 }
 
 int bh_cond_wait_for(bh_cond_t *cond,
                      bh_mutex_t *mutex,
                      unsigned long timeout)
 {
-    (void)cond;
-    (void)mutex;
-    (void)timeout;
-    return -1;
+    if (!SleepConditionVariableCS(&cond->handle, &mutex->handle, timeout))
+        return -1;
+    return 0;
 }
 
 int bh_cond_signal(bh_cond_t *cond)
 {
-    (void)cond;
-    return -1;
+    WakeConditionVariable(&cond->handle);
+    return 0;
 }
 
 int bh_cond_broadcast(bh_cond_t *cond)
 {
-    (void)cond;
-    return -1;
+    WakeAllConditionVariable(&cond->handle);
+    return 0;
+}
+#else
+/* Condition variable implementation based on BeOS article
+ * http://www-classic.be.com/aboutbe/benewsletter/volume_III/Issue40.html
+ */ 
+
+int bh_cond_init(bh_cond_t *cond)
+{
+    if (bh_mutex_init(&cond->lock))
+        return -1;
+    
+    if (bh_semaphore_init(&cond->wait, 0))
+    {
+        bh_mutex_destroy(&cond->lock);
+        return -1;
+    }
+    
+    if (bh_semaphore_init(&cond->done, 0))
+    {
+        bh_semaphore_destroy(&cond->wait);
+        bh_mutex_destroy(&cond->lock);
+        return -1;
+    }
+    cond->waiting = 0;
+    cond->signals = 0;
+
+    return 0;
+}
+
+void bh_cond_destroy(bh_cond_t *cond)
+{
+    bh_semaphore_destroy(&cond->done);
+    bh_semaphore_destroy(&cond->wait);
+    bh_mutex_destroy(&cond->lock);
+}
+
+int bh_cond_wait(bh_cond_t *cond,
+                 bh_mutex_t *mutex)
+{
+    int retval;
+    
+    bh_mutex_lock(&cond->lock);
+    cond->waiting++;
+    bh_mutex_unlock(&cond->lock);
+    bh_mutex_unlock(mutex);
+    
+    retval = bh_semaphore_wait(&cond->wait);
+    
+    bh_mutex_lock(&cond->lock);
+    if (cond->signals > 0)
+    {
+        if (retval)
+            bh_semaphore_wait(&cond->wait);
+        
+        bh_semaphore_post(&cond->done);
+        cond->signals--;
+    }
+    cond->waiting--;
+    bh_mutex_unlock(&cond->lock);    
+    bh_mutex_lock(mutex);
+    
+    return retval;
+}
+
+int bh_cond_wait_for(bh_cond_t *cond,
+                     bh_mutex_t *mutex,
+                     unsigned long timeout)
+{
+    int retval;
+    
+    bh_mutex_lock(&cond->lock);
+    cond->waiting++;
+    bh_mutex_unlock(&cond->lock);
+    bh_mutex_unlock(mutex);
+    
+    retval = bh_semaphore_wait_for(&cond->wait, timeout);
+    
+    bh_mutex_lock(&cond->lock);
+    if (cond->signals > 0)
+    {
+        if (retval)
+            bh_semaphore_wait(&cond->wait);
+        
+        bh_semaphore_post(&cond->done);
+        cond->signals--;
+    }
+    cond->waiting--;
+    bh_mutex_unlock(&cond->lock);    
+    bh_mutex_lock(mutex);
+    
+    return retval;
+}
+
+int bh_cond_signal(bh_cond_t *cond)
+{
+    bh_mutex_lock(&cond->lock);
+    
+    if (cond->waiting > cond->signals)
+    {
+        cond->signals++;
+        
+        bh_semaphore_post(&cond->wait);
+        bh_mutex_unlock(&cond->lock);
+        bh_semaphore_wait(&cond->done);
+    }
+    else
+        bh_mutex_unlock(&cond->lock);
+    
+    return 0;
 }
 
+int bh_cond_broadcast(bh_cond_t *cond)
+{
+    int i, waiting;
+    
+    bh_mutex_lock(&cond->lock);
+    if (cond->waiting > cond->signals)
+    {
+        waiting = cond->waiting - cond->signals;
+        cond->signals = cond->waiting;
+        
+        for (i = 0; i < waiting; i++)
+            bh_semaphore_post(&cond->wait);
+        
+        bh_mutex_unlock(&cond->lock);
+               
+        for (i = 0; i < waiting; i++)
+            bh_semaphore_wait(&cond->done);
+    }
+    else
+        bh_mutex_unlock(&cond->lock);
+    
+    return 0;
+}
+#endif
+
 int bh_thread_pool_init_base(bh_thread_pool_t *pool,
                              size_t size,
                              bh_thread_begin_cb_t begin,
                              bh_thread_end_cb_t end)
 {
-    (void)pool;
-    (void)size;
+    size_t i;
+    static bh_task_t pool_task;
+    static int pool_task_init = 0;
+
+    /* Initialize static thread pool task */
+    if (!pool_task_init)
+    {
+        bh_task_init(&pool_task, bh_thread_pool_worker, pool, 0);
+        pool_task_init = 1;
+    }
+
+    /* Zero out pool structure */
+    memset(pool, 0, sizeof(*pool));
+
+    /* Initialize mutex and condition variables */
+    if (bh_mutex_init(&pool->lock))
+        goto lock_fail;
+
+    if (bh_cond_init(&pool->new_task))
+        goto task_fail;
+
+    if (bh_cond_init(&pool->done_task))
+        goto done_fail;
+
+    /* Allocate array of threads */
+    pool->threads = malloc(size * sizeof(bh_thread_t));
+    pool->size = size;
+
+    if (!pool->threads)
+        goto thread_fail;
+
+    /* Initialize queue */
+    bh_queue_init(&pool->tasks);
+    if (bh_queue_reserve(&pool->tasks, 64))
+        goto queue_fail;
+
+    /* Initialize threads */
+    for (i = 0; i < size; i++)
+    {
+        if (bh_thread_init_base(&pool->threads[i], &pool_task, begin, end))
+        {
+            pool->shutdown = 1;
+            bh_cond_broadcast(&pool->new_task);
+            for(; i; i--)
+                bh_thread_join(&pool->threads[i - 1]);
+
+            bh_queue_destroy(&pool->tasks);
+            goto queue_fail;
+        }
+    }
+
+    return 0;
+
+queue_fail:
+    free(pool->threads);
+
+thread_fail:
+    bh_cond_destroy(&pool->done_task);
+
+done_fail:
+    bh_cond_destroy(&pool->new_task);
+
+task_fail:
+    bh_mutex_destroy(&pool->lock);
+
+lock_fail:
     return -1;
 }
 
@@ -114,7 +408,14 @@ bh_thread_pool_t *bh_thread_pool_new_base(size_t size,
                                           bh_thread_begin_cb_t begin,
                                           bh_thread_end_cb_t end)
 {
-    (void)size;
-    return NULL;
+    bh_thread_pool_t *result;
+    result = malloc(sizeof(*result));
+    if (result && bh_thread_pool_init_base(result, size, begin, end))
+    {
+        free(result);
+        result = NULL;
+    }
+
+    return result;
 }