Task scheduler. Don't use this it's broken.

sg_tpool.c

#include "sg_tpool.h"

#include <pthread.h>
#include <semaphore.h>
#include <stdbool.h>
#include <stdlib.h>

typedef struct
{
    STaskID id;
    // a task notifies it's parent when it completes.
    STaskID parent;
    // cannot start until dependency is removed from the open queue.
    STaskID dependency;
    // higher priority tasks are picked from the heap first.
    int priority;
    // cannot complete until open_children is reduced to 0.
    // work item counts towards the open_children count.
    int open_children;
    SWorkItem work;
} Task;

typedef struct
{
    pthread_t thread;
    int id;
} ThreadData;

typedef struct
{
    STaskID id;
    uint16_t index;
    uint16_t next;
} Index;

typedef struct
{
    STaskID id;
    int priority;
} HeapItem;

static struct {
    HeapItem *heap;
    Index *indices;
    Task *packed;
    ThreadData *thread_data;

    int thread_count;
    uint16_t queue_size;
    uint16_t heap_count;
    uint16_t packed_count;
    uint16_t freelist;

    sem_t packed_n_free;
    sem_t heap_n_used;
    pthread_mutex_t packed_lock;
    pthread_mutex_t heap_lock;
} pool;

void *worker_thread_run(void *data);

void sg_tpool_init(int thread_count, uint16_t queue_size)
{
    sem_init(&pool.packed_n_free, 0, queue_size);
    sem_init(&pool.heap_n_used, 0, 0);
    pthread_mutex_init(&pool.heap_lock, NULL);
    pthread_mutex_init(&pool.packed_lock, NULL);

    pool.queue_size = queue_size;

    pool.heap_count = 0;
    pool.heap = malloc(sizeof(*pool.heap) * queue_size);
    pool.packed_count = 0;
    pool.packed = malloc(sizeof(*pool.packed) * queue_size);

    pool.freelist = 0;
    pool.indices = malloc(sizeof(*pool.indices) * queue_size);
    for(int i = 0; i < queue_size; i++)
    {
        pool.indices[i].id = i;
        pool.indices[i].index = 0xffff;
        pool.indices[i].next = i + 1; /* next in freelist */
    }
    pool.indices[queue_size - 1].next = UINT16_MAX; /* use max as sentinel */

    pool.thread_count = thread_count;
    pool.thread_data = malloc(sizeof(*pool.thread_data) * thread_count);
    for(int i = 0; i < thread_count; i++)
    {
        ThreadData *data = &pool.thread_data[i];
        data->id = i;
        pthread_create(&data->thread, NULL, worker_thread_run, data);
    }
}

#define LEFT(I) (2 * (I) + 1)
#define RIGHT(I) (2 * (I) + 2)
#define PARENT(I) (((I) - 1) >> 1)
static inline void enqueue_task(STaskID id, int priority)
{
    pthread_mutex_lock(&pool.heap_lock);

    int i = pool.heap_count++;
    int p = PARENT(i);
    pool.heap[i].id = id;
    pool.heap[i].priority = priority;
    while(i > 0 && pool.heap[i].priority < priority)
    {
        HeapItem tmp = pool.heap[p];
        pool.heap[p] = pool.heap[i];
        pool.heap[i] = tmp;
        i = p;
        p = PARENT(i);
    }

    pthread_mutex_unlock(&pool.heap_lock);
    sem_post(&pool.heap_n_used);
}

static inline STaskID dequeue_task(void)
{
    sem_wait(&pool.heap_n_used);
    pthread_mutex_lock(&pool.heap_lock);

    int i = 0;
    int l = LEFT(i);
    int r = RIGHT(i);
    STaskID res = pool.heap[i].id;
    pool.heap[i] = pool.heap[--pool.heap_count];
    while(l < pool.heap_count)
    {
        int m = (pool.heap[l].priority > pool.heap[i].priority) ? l : i;
        if(r < pool.heap_count)
            m = (pool.heap[r].priority > pool.heap[m].priority) ? r : m;
        if(m == i)
            break;
        HeapItem tmp = pool.heap[m];
        pool.heap[m] = pool.heap[i];
        pool.heap[i] = tmp;
        i = m;
        l = LEFT(i);
        r = RIGHT(i);
    }

    pthread_mutex_unlock(&pool.heap_lock);
    return res;
}
#undef LEFT
#undef RIGHT
#undef PARENT

static inline bool has_task(STaskID id)
{
    Index *idx = &pool.indices[id & 0xffff];
    bool ret = idx->id == id && idx->index != UINT16_MAX;
    return ret;
}

static inline STaskID add_task(Task t)
{
    sem_wait(&pool.packed_n_free);
    pthread_mutex_lock(&pool.packed_lock);

    Index *idx = &pool.indices[pool.freelist];
    pool.freelist = idx->next;
    idx->id += 0x10000;
    idx->index = pool.packed_count++;
    pool.packed[idx->index] = t;
    pool.packed[idx->index].id = idx->id;

    if(t.dependency == SG_INVALID_TASK || !has_task(t.dependency))
        enqueue_task(idx->id, t.priority);

    pthread_mutex_unlock(&pool.packed_lock);
    return idx->id;
}

static inline void remove_task(STaskID id)
{
    pthread_mutex_lock(&pool.packed_lock);

    uint16_t idx_slot = id & 0xffff;
    Index *idx = &pool.indices[idx_slot];
    Task *tsk = &pool.packed[idx->index];
    *tsk = pool.packed[--pool.packed_count];
    pool.indices[tsk->id & 0xffff].index = idx->index;
    idx->next = pool.freelist;
    idx->index = 0xffff;
    pool.freelist = idx_slot;

    // scan for tasks which depended on the recently completeted task and queue
    // them up. The queue has to be reasonably small for this to be effective.
    for(int i = 0; i < pool.packed_count; i++)
    {
        Task *t = &pool.packed[i];
        if(t->dependency == id)
        {
            enqueue_task(t->id, t->priority);
        }
    }

    pthread_mutex_unlock(&pool.packed_lock);
    sem_post(&pool.packed_n_free);
}

static inline Task *get_task(STaskID id)
{
    return &pool.packed[pool.indices[id & 0xffff].index];
}

STaskID sg_tpool_add(SWorkItem work, int priority, STaskID dependency)
{
    STaskID id = add_task((Task) {
        .work = work,
        .priority = priority,
        .parent = SG_INVALID_TASK,
        .dependency = dependency,
        .open_children = 0
    });
    return id;
}

void run_one_task(void)
{
    STaskID task_id = dequeue_task();

    Task *task = get_task(task_id);
    task->work.fun(task->work.data);

    remove_task(task_id);
}

void *worker_thread_run(void *data)
{
    (void)data;
    for(;;)
        run_one_task();

    return NULL;
}

void sg_tpool_shutdown(void)
{
    for(int i = 0; i < pool.thread_count; i++)
    {
        pthread_join(pool.thread_data[i].thread, NULL);
    }
}

sg_tpool.h

#ifndef SG_TASK_H
#define SG_TASK_H

//
// sg_tpool
//
// A thread pool implementation.
//
// The thread pool operates on a priority queue of tasks each of which
// encapsulate a work item. A task, identified by its id, cannot start until
// its dependency is completed and cannot finish until its open_children is
// decreased to zero. The work item encapsulated by the task counts towards the
// open_children count, as do all child tasks added between add_begin and
// add_finish.
//
// Worker threads continually pull tasks with the highest priority from the
// work queue and execute their work items when a task is finished (i.e its
// open_children is zero) its parent's open_children count is decremented.
//

#include <stdint.h>

typedef uint32_t STaskID;
#define SG_INVALID_TASK 0xffffffff

typedef struct
{
    void (*fun)(void *data);
    void *data;
} SWorkItem;

void sg_tpool_init(int thread_count, uint16_t queue_size);

//
// Adds a work item to the thread pool with a given parent and task dependency.
// Keeps the task from completing before add_finish is called so you can add
// child tasks.
//
STaskID sg_tpool_add_begin(SWorkItem work, int priority, STaskID dependency);

//
// Completes a previous add_begin and allows the task to finish when it has
// completed.
//
void sg_tpool_add_finish(STaskID id);

//
// Adds a work item to the thread pool with a given parent and task dependency.
//
STaskID sg_tpool_add(SWorkItem work, int priority, STaskID dependency);

//
// Waits for a given task to complete, using the current thread to execute
// enqueued tasks in the mean time.
//
void sg_tpool_wait(STaskID id);

void sg_tpool_shutdown(void);

#endif