C Threadsafe Priority Queue O(log n) push, O(1) pop

Question

I wrote this for an A* implementation:

(pqueue.c)

#include "pqueue.h"
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
#include <stdlib.h>
#include <string.h>

struct pqueue* new_pqueue(size_t capacity, int mt) {
    struct pqueue* pqueue = malloc(sizeof(struct pqueue));
    pqueue->capacity = capacity;
    pqueue->data = malloc((capacity == 0 ? 1 : capacity) * sizeof(struct __pqueue_entry));
    pqueue->rc = capacity == 0 ? 1 : 0;
    pqueue->start = 0;
    pqueue->end = 0;
    pqueue->size = 0;
    pqueue->mt = mt;
    if (mt) {
        if (pthread_mutex_init(&pqueue->data_mutex, NULL)) {
            free(pqueue->data);
            pqueue->data = NULL;
            free(pqueue);
            return NULL;
        }
        if (pthread_mutex_init(&pqueue->out_mutex, NULL)) {
            free(pqueue->data);
            pqueue->data = NULL;
            free(pqueue);
            pthread_mutex_destroy(&pqueue->data_mutex);
            return NULL;
        }
        if (pthread_mutex_init(&pqueue->in_mutex, NULL)) {
            free(pqueue->data);
            pqueue->data = NULL;
            free(pqueue);
            pthread_mutex_destroy(&pqueue->data_mutex);
            pthread_mutex_destroy(&pqueue->out_mutex);
            return NULL;
        }
        if (pthread_cond_init(&pqueue->out_cond, NULL)) {
            free(pqueue->data);
            pqueue->data = NULL;
            free(pqueue);
            pthread_mutex_destroy(&pqueue->data_mutex);
            pthread_mutex_destroy(&pqueue->out_mutex);
            pthread_mutex_destroy(&pqueue->in_mutex);
            return NULL;
        }
        if (pthread_cond_init(&pqueue->in_cond, NULL)) {
            free(pqueue->data);
            pqueue->data = NULL;
            free(pqueue);
            pthread_mutex_destroy(&pqueue->data_mutex);
            pthread_mutex_destroy(&pqueue->out_mutex);
            pthread_mutex_destroy(&pqueue->in_mutex);
            pthread_cond_destroy(&pqueue->out_cond);
            return NULL;
        }
    }
    return pqueue;
}

int del_pqueue(struct pqueue* pqueue) {
    if (pqueue == NULL || pqueue->data == NULL) return -1;
    if (pqueue->mt) {
        if (pthread_mutex_destroy(&pqueue->data_mutex)) return -1;
        if (pthread_mutex_destroy(&pqueue->out_mutex)) return -1;
        if (pthread_cond_destroy(&pqueue->out_cond)) return -1;
        if (pthread_mutex_destroy(&pqueue->in_mutex)) return -1;
        if (pthread_cond_destroy(&pqueue->in_cond)) return -1;
    }
    free(pqueue->data);
    pqueue->data = NULL;
    free(pqueue);
    return 0;
}

int add_pqueue(struct pqueue* pqueue, void* data, float priority) {
    if (pqueue->mt) pthread_mutex_lock(&pqueue->data_mutex);
    if (pqueue->size == pqueue->rc && pqueue->capacity == 0) {
        size_t orc = pqueue->rc;
        pqueue->rc += 1024 / sizeof(struct __pqueue_entry);
        struct __pqueue_entry* ndata = malloc(pqueue->rc * sizeof(struct __pqueue_entry));
        if (pqueue->start < pqueue->end) {
            memcpy(ndata, pqueue->data + pqueue->start, (pqueue->end - pqueue->start) * sizeof(struct __pqueue_entry));
        } else {
            memcpy(ndata, pqueue->data + pqueue->start, (orc - pqueue->start) * sizeof(struct __pqueue_entry));
            memcpy(ndata + (orc - pqueue->start), pqueue->data + pqueue->end, (pqueue->start - pqueue->end) * sizeof(struct __pqueue_entry));
        }
        free(pqueue->data);
        pqueue->data = ndata;
    } else if (pqueue->capacity == 0) {
    } else {
        if (!pqueue->mt) return 1;
        pthread_mutex_unlock(&pqueue->data_mutex);
        pthread_mutex_lock(&pqueue->in_mutex);
        while (pqueue->size == pqueue->capacity) {
            pthread_cond_wait(&pqueue->in_cond, &pqueue->in_mutex);
        }
        pthread_mutex_unlock(&pqueue->in_mutex);
        pthread_mutex_lock(&pqueue->data_mutex);
    }
    struct __pqueue_entry pd;
    pd.ptr = data;
    pd.prio = priority;
    float i = (float) pqueue->size / 2.;
    float ts = (float) pqueue->size / 2.;
    size_t rp = pqueue->capacity > 0 ? pqueue->capacity : pqueue->rc;
    int tc = 0;
    int fri = 0;
    while (ts > 0.) {
        int ri = (int) i + pqueue->start;
        if (ri >= rp) ri -= rp;
        if (pqueue->data[ri].prio > pd.prio) i -= ts;
        else i += ts;
        if (i < 0.) i = 0.;
        if (i > pqueue->size) {
            i = pqueue->size - 1;
        }
        if ((ri == pqueue->end || pqueue->data[ri].prio > pd.prio) && (ri == pqueue->start || pqueue->data[ri - 1].prio < pd.prio)) {
            fri = ri;
            break;
        }
        ts /= 2.;
        tc++;
    }
    if (fri <= pqueue->end) {
        if (pqueue->end > pqueue->start) {
            memmove(pqueue->data + fri + 1, pqueue->data + fri, (pqueue->end - fri) * sizeof(struct __pqueue_entry));
        } else if (fri > pqueue->start) {
            memmove(pqueue->data + 1, pqueue->data, pqueue->end * sizeof(struct __pqueue_entry));
            memcpy(pqueue->data, pqueue->data + rp - 1, sizeof(struct __pqueue_entry));
            memmove(pqueue->data + fri + 1, pqueue->data + fri, (rp - (pqueue->start) - fri) * sizeof(struct __pqueue_entry));
        } else {
            memmove(pqueue->data + fri + 1, pqueue->data + fri, (pqueue->end - fri) * sizeof(struct __pqueue_entry));
        }
    }
    pqueue->end++;
    memcpy(pqueue->data + fri, &pd, sizeof(struct __pqueue_entry));
    if (pqueue->end >= rp) {
        if (pqueue->end - rp == pqueue->start) {
            size_t orc = pqueue->rc;
            pqueue->rc += 1024 / sizeof(struct __pqueue_entry);
            struct __pqueue_entry* ndata = malloc(pqueue->rc * sizeof(struct __pqueue_entry));
            if (pqueue->start < pqueue->end) {
                memcpy(ndata, pqueue->data + pqueue->start, (pqueue->end - pqueue->start) * sizeof(struct __pqueue_entry));
            } else {
                memcpy(ndata, pqueue->data + pqueue->start, (orc - pqueue->start) * sizeof(struct __pqueue_entry));
                memcpy(ndata + (orc - pqueue->start), pqueue->data + pqueue->end, (pqueue->start - pqueue->end) * sizeof(struct __pqueue_entry));
            }
            free(pqueue->data);
            pqueue->data = ndata;
        } else pqueue->end -= rp;
    }
    pqueue->size++;
    if (pqueue->mt) {
        pthread_mutex_unlock(&pqueue->data_mutex);
        pthread_cond_signal(&pqueue->out_cond);
    }
    return 0;
}

void* pop_pqueue(struct pqueue* pqueue) {
    if (pqueue->mt) {
        pthread_mutex_lock(&pqueue->out_mutex);
        while (pqueue->size == 0) {
            pthread_cond_wait(&pqueue->out_cond, &pqueue->out_mutex);
        }
        pthread_mutex_unlock(&pqueue->out_mutex);
        pthread_mutex_lock(&pqueue->data_mutex);
    } else if (pqueue->size == 0) {
        return NULL;
    }
    struct __pqueue_entry data = pqueue->data[pqueue->start++];
    size_t rp = pqueue->capacity > 0 ? pqueue->capacity : pqueue->rc;
    if (pqueue->start >= rp) {
        pqueue->start -= rp;
    }
    pqueue->size--;
    if (pqueue->mt) {
        pthread_mutex_unlock(&pqueue->data_mutex);
        pthread_cond_signal(&pqueue->in_cond);
    }
    return data.ptr;
}

void* timedpop_pqueue(struct pqueue* pqueue, struct timespec* abstime) {
    if (pqueue->mt) {
        pthread_mutex_lock(&pqueue->out_mutex);
        while (pqueue->size == 0) {
            int x = pthread_cond_timedwait(&pqueue->out_cond, &pqueue->out_mutex, abstime);
            if (x) {
                pthread_mutex_unlock(&pqueue->out_mutex);
                errno = x;
                return NULL;
            }
        }
        pthread_mutex_unlock(&pqueue->out_mutex);
        pthread_mutex_lock(&pqueue->data_mutex);
    } else if (pqueue->size == 0) {
        return NULL;
    }
    struct __pqueue_entry data = pqueue->data[pqueue->start++];
    size_t rp = pqueue->capacity > 0 ? pqueue->capacity : pqueue->rc;
    if (pqueue->start >= rp) {
        pqueue->start -= rp;
    }
    pqueue->size--;
    if (pqueue->mt) {
        pthread_mutex_unlock(&pqueue->data_mutex);
        pthread_cond_signal(&pqueue->in_cond);
    }
    return data.ptr;
}

(pqueue.h)

#ifndef pqueue_H_
#define pqueue_H_

#include <pthread.h>

struct __pqueue_entry {
        float prio;
        void* ptr;
};

struct pqueue {
        size_t size;
        size_t capacity;
        size_t start;
        size_t end;
        size_t rc;
        struct __pqueue_entry* data;
        pthread_mutex_t data_mutex;
        pthread_mutex_t in_mutex;
        pthread_cond_t in_cond;
        pthread_mutex_t out_mutex;
        pthread_cond_t out_cond;
        int mt;
};

struct pqueue* new_pqueue(size_t capacity, int mt); // mt = multithreaded

int del_pqueue(struct pqueue* pqueue);

int add_pqueue(struct pqueue* pqueue, void* data, float priority);

void* pop_pqueue(struct pqueue* pqueue);

void* timedpop_pqueue(struct pqueue* pqueue, struct timespec* abstime);

#endif /* pqueue_H_ */

Test code:

#include <stdio.h>
#include "pqueue.h"

int main(int argc, char* argv[]) {
    char* s[] = { "s1", "s2", "s3", "s4", "s5", "s6", "s7", "s8" };
    struct pqueue* queue = new_pqueue(0, 0);
    add_pqueue(queue, s[6], 7.);
    add_pqueue(queue, s[1], 2.);
    add_pqueue(queue, s[3], 4.);
    add_pqueue(queue, s[0], 1.);
    add_pqueue(queue, s[4], 5.);
    add_pqueue(queue, s[7], 8.);
    add_pqueue(queue, s[5], 6.);
    add_pqueue(queue, s[2], 3.);
    for (int x = 0; x < 8; x++) {
        printf("%s\n", (char*) pop_pqueue(queue));
    }
    del_pqueue(queue);
    return 0;
}

Answer 1

Doesn't work if mt == 0

If looks like the code is supposed to support both multithreaded and non-multithreaded cases. However, in add_pqueue(), there is this code:

} else {
    if (!pqueue->mt) return 1;

The effect of this is to always abort if multithreading is off. I think you meant to do this instead:

} else if (pqueue->mt) {

Mutexes and conditions not used properly

Consider this code in pop_queue():

if (pqueue->mt) {
    pthread_mutex_lock(&pqueue->out_mutex);
    while (pqueue->size == 0) {
        pthread_cond_wait(&pqueue->out_cond, &pqueue->out_mutex);
    }
    pthread_mutex_unlock(&pqueue->out_mutex);
    pthread_mutex_lock(&pqueue->data_mutex);

The goal of out_mutex and out_cond is to wait for the queue to be non-empty before proceeding to lock data_mutex and actually popping from the queue. However, consider the case where size == 1 initially. What could happen is:

1. Two threads call pop_queue().
2. Each thread gets past the size check and arrives at the line which is about to lock data_mutex.
3. Thread 0 locks data_mutex, pops an element from the queue, and unlocks data_mutex. This empties the queue, setting size to 0.
4. Thread 1 locks data_mutex, and then tries to pop from an empty queue, resulting in an error.

The same problem also happens in add_queue() but with the size full check.

What I suggest is:

1. Only use one mutex, data_mutex.
2. If you get past the size check while holding data_mutex, then just proceed.

The code from above becomes:

if (pqueue->mt) {
    pthread_mutex_lock(&pqueue->data_mutex);
    while (pqueue->size == 0) {
        pthread_cond_wait(&pqueue->out_cond, &pqueue->data_mutex);
    }

Add isn't \$O(\log n)\$

I took a closer look at your add function and it appears to do a binary search to find an insertion point followed by using one or more calls to memmove() to make an open slot in the queue to insert the new element. The memory moving takes \$O(n)\$ time. If you used a heap data structure, you could achieve \$O(\log n)\$.

Also, as a member of the Floating Point Police™, I disapprove of using floating point to do the binary search. This can be done using only integer arithmetic.

Answer 2

Just a small review:

// Did code use the right type here? -----------------------------v
 pqueue->data = malloc((capacity == 0 ? 1 : capacity) * sizeof(struct __pqueue_entry));

How does one check that?
Reviewer needs to search pqueue.h, find the field data as below and then say - OK no problem.

struct __pqueue_entry* data;

---

Instead, suppose the code used the size of the de-referenced variable:

 pqueue->data = malloc(sizeof *(pqueue->data) * (capacity == 0 ? 1 : capacity));

Oh happy day! - no problem here - no time wasted - no need to code the type.

---

Consider this 2nd style. Easier to code, less chance of coding error, easier to review and maintain.

Answer 3

Some additions to what has already been mentioned:

1. In new_pqueue you invoke undefined behaviour:

   if (pthread_mutex_init(&pqueue->out_mutex, NULL)) {
       free(pqueue->data);
       pqueue->data = NULL;
       free(pqueue);
       pthread_mutex_destroy(&pqueue->data_mutex);
       return NULL;
   }
   

   You access pqueue after calling free. Generally you should cleanup resources in the reverse order and since the mutex been created last it should be destroyed first.

2. You don't check the return value of malloc. It would be cleaner to release the allocated memory and simply return NULL and leave the caller to deal with it.

3. The header file is basically the public interface to your implementation. Internal details like struct __pqueue_entry have no business of being there.

4. In the end most variables or field are int or float or whatever other primitive type is available - what distinguishes them is the semantics the programmer gives them. In languages like C the only real way to convey that semantics to the reader is the name you give fields and variables. This makes the code easier to read and maintain and understandable by others.
   Short-cut names like rc or mt are fairly useless since they simply fail to convey the purpose properly.
   Using descriptive names is more important than saving a few keystrokes.

5. This code:

   size_t orc = pqueue->rc;
   pqueue->rc += 1024 / sizeof(struct __pqueue_entry);
   struct __pqueue_entry* ndata = malloc(pqueue->rc * sizeof(struct __pqueue_entry));
   if (pqueue->start < pqueue->end) {
       memcpy(ndata, pqueue->data + pqueue->start, (pqueue->end - pqueue->start) * sizeof(struct __pqueue_entry));
   } else {
       memcpy(ndata, pqueue->data + pqueue->start, (orc - pqueue->start) * sizeof(struct __pqueue_entry));
       memcpy(ndata + (orc - pqueue->start), pqueue->data + pqueue->end, (pqueue->start - pqueue->end) * sizeof(struct __pqueue_entry));
   }
   free(pqueue->data);
   pqueue->data = ndata;
   

   which resizes the queue is duplicated and should be extracted into its own method.