Use lock free queue

Author: howard0su

thpool.c

@@ -25,6 +25,31 @@ this is not part of original thpool, thats me hacking it to work on windows
 // ref: https://github.com/ggerganov/whisper.cpp/issues/168
 #include <windows.h>
 #endif
+#include <stdbool.h>
+
+typedef volatile LONG atomic_int;
+typedef atomic_int atomic_bool;
+
+static void atomic_store(atomic_int* ptr, LONG val) {
+    InterlockedExchange(ptr, val);
+}
+static LONG atomic_load(atomic_int* ptr) {
+    return InterlockedCompareExchange(ptr, 0, 0);
+}
+static LONG atomic_fetch_add(atomic_int* ptr, LONG inc) {
+    return InterlockedExchangeAdd(ptr, inc);
+}
+static LONG atomic_fetch_sub(atomic_int* ptr, LONG dec) {
+    return atomic_fetch_add(ptr, -(dec));
+}
+static bool atomic_compare_exchange_strong(atomic_int* ptr, atomic_int* expected, int desired) {
+    int old_value = InterlockedCompareExchange((LONG*)ptr, desired, *expected);
+    bool success = (old_value == *expected);
+    if (!success) {
+        *expected = old_value;
+    }
+    return success;
+}
 
 unsigned int sleep(unsigned int seconds) {
 	Sleep(seconds * 1000);
@@ -179,8 +204,7 @@ typedef struct thpool_{
 	thread**   threads;                  /* pointer to threads        */
 	volatile int num_threads_alive;      /* threads currently alive   */
 	volatile int num_threads_working;    /* threads currently working */
-	pthread_mutex_t  thcount_lock;       /* used for thread count etc */
-	pthread_cond_t  threads_all_idle;    /* signal to thpool_wait     */
+	bsem 	threads_all_idle;            /* signal to thpool_wait     */
 	jobqueue  jobqueue;                  /* job queue                 */
 } thpool_;
 
@@ -251,8 +275,7 @@ struct thpool_* thpool_init(int num_threads){
 		return NULL;
 	}
 
-	pthread_mutex_init(&(thpool_p->thcount_lock), NULL);
-	pthread_cond_init(&thpool_p->threads_all_idle, NULL);
+	bsem_init(&thpool_p->threads_all_idle, 0);
 
 	/* Thread init */
 	int n;
@@ -283,11 +306,9 @@ int thpool_add_work(thpool_* thpool_p, void (*function_p)(void*), job * newjob){
 
 /* Wait until all jobs have finished */
 void thpool_wait(thpool_* thpool_p){
-	pthread_mutex_lock(&thpool_p->thcount_lock);
 	while (thpool_p->jobqueue.len || thpool_p->num_threads_working) {
-		pthread_cond_wait(&thpool_p->threads_all_idle, &thpool_p->thcount_lock);
+		bsem_wait(&thpool_p->threads_all_idle);
 	}
-	pthread_mutex_unlock(&thpool_p->thcount_lock);
 }
 
 
@@ -422,19 +443,15 @@ static void* thread_do(struct thread* thread_p){
 	}*/
 
 	/* Mark thread as alive (initialized) */
-	pthread_mutex_lock(&thpool_p->thcount_lock);
-	thpool_p->num_threads_alive += 1;
-	pthread_mutex_unlock(&thpool_p->thcount_lock);
+	atomic_fetch_add(&thpool_p->num_threads_alive, 1);
 
 	while(threads_keepalive){
 
 		bsem_wait(thpool_p->jobqueue.has_jobs);
 
 		if (threads_keepalive){
 
-			pthread_mutex_lock(&thpool_p->thcount_lock);
-			thpool_p->num_threads_working++;
-			pthread_mutex_unlock(&thpool_p->thcount_lock);
+			atomic_fetch_add(&thpool_p->num_threads_working, 1);
 
 			/* Read job from queue and execute it */
 			void (*func_buff)(void*);
@@ -446,18 +463,13 @@ static void* thread_do(struct thread* thread_p){
 				func_buff(arg_buff);
 			}
 
-			pthread_mutex_lock(&thpool_p->thcount_lock);
-			thpool_p->num_threads_working--;
-			if (!thpool_p->num_threads_working) {
-				pthread_cond_signal(&thpool_p->threads_all_idle);
+			if (atomic_fetch_sub(&thpool_p->num_threads_working, 1) == 1) {
+				bsem_post_all(&thpool_p->threads_all_idle);
 			}
-			pthread_mutex_unlock(&thpool_p->thcount_lock);
-
 		}
 	}
-	pthread_mutex_lock(&thpool_p->thcount_lock);
-	thpool_p->num_threads_alive --;
-	pthread_mutex_unlock(&thpool_p->thcount_lock);
+	
+	atomic_fetch_sub(&thpool_p->num_threads_working, 1);
 
 	return NULL;
 }
@@ -507,65 +519,77 @@ static void jobqueue_clear(jobqueue* jobqueue_p){
 
 }
 
-
 /* Add (allocated) job to queue
  */
-static void jobqueue_push(jobqueue* jobqueue_p, struct job* newjob){
+static void jobqueue_push(jobqueue *jobqueue_p, struct job *newjob)
+{
 
-	pthread_mutex_lock(&jobqueue_p->rwmutex);
 	newjob->prev = NULL;
-
-	switch(jobqueue_p->len){
-
-		case 0:  /* if no jobs in queue */
-					jobqueue_p->front = newjob;
-					jobqueue_p->rear  = newjob;
-					break;
-
-		default: /* if jobs in queue */
-					jobqueue_p->rear->prev = newjob;
-					jobqueue_p->rear = newjob;
-
+	struct job *expected_rear = NULL;
+
+	// Use atomic compare-and-swap to add newjob to the end of the queue
+	if (atomic_compare_exchange_strong(&jobqueue_p->rear, &expected_rear, newjob))
+	{
+		if (jobqueue_p->front == NULL)
+		{
+			// if no jobs in queue, set front to newjob
+			atomic_store(&jobqueue_p->front, newjob);
+		}
+	}
+	else
+	{
+		// if jobs in queue, update previous pointer of previous rear and set rear to newjob
+		struct job *prev_rear = (struct job *)atomic_load(&jobqueue_p->rear);
+		atomic_store(&prev_rear->prev, newjob);
+		atomic_store(&jobqueue_p->rear, newjob);
 	}
-	jobqueue_p->len++;
 
+	atomic_fetch_add(&jobqueue_p->len, 1);
 	bsem_post(jobqueue_p->has_jobs);
-	pthread_mutex_unlock(&jobqueue_p->rwmutex);
 }
 
-
 /* Get first job from queue(removes it from queue)
  * Notice: Caller MUST hold a mutex
  */
-static struct job* jobqueue_pull(jobqueue* jobqueue_p){
-
-	pthread_mutex_lock(&jobqueue_p->rwmutex);
-	job* job_p = jobqueue_p->front;
+static struct job *jobqueue_pull(jobqueue *jobqueue_p)
+{
 
-	switch(jobqueue_p->len){
+	struct job *expected_front = NULL;
+	struct job *expected_rear = NULL;
+	struct job *job_p = NULL;
 
-		case 0:  /* if no jobs in queue */
-		  			break;
-
-		case 1:  /* if one job in queue */
-					jobqueue_p->front = NULL;
-					jobqueue_p->rear  = NULL;
-					jobqueue_p->len = 0;
-					break;
-
-		default: /* if >1 jobs in queue */
-					jobqueue_p->front = job_p->prev;
-					jobqueue_p->len--;
-					/* more than one job in queue -> post it */
-					bsem_post(jobqueue_p->has_jobs);
+	// Use atomic compare-and-swap to remove the front job from the queue
+	if (atomic_compare_exchange_strong(&jobqueue_p->front, &expected_front, NULL))
+	{
+		// Queue is empty
+		return NULL;
+	}
+	else
+	{
+		job_p = expected_front;
+	}
 
+	// Update rear pointer if necessary
+	if (jobqueue_p->rear == job_p)
+	{
+		if (atomic_compare_exchange_strong(&jobqueue_p->rear, &expected_rear, NULL))
+		{
+			// The queue only had one element
+			atomic_fetch_sub(&jobqueue_p->len, 1);
+			return job_p;
+		}
+		else
+		{
+			atomic_store(&jobqueue_p->rear, expected_front->prev);
+		}
 	}
 
-	pthread_mutex_unlock(&jobqueue_p->rwmutex);
+	// Remove the job from the queue
+	atomic_fetch_sub(&jobqueue_p->len, 1);
+	bsem_post(jobqueue_p->has_jobs);
 	return job_p;
 }
 
-
 /* Free all queue resources back to the system */
 static void jobqueue_destroy(jobqueue* jobqueue_p){
 	jobqueue_clear(jobqueue_p);