q.h

#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

struct msg {
        struct msg *next;
};

struct queue {
        uint32_t head;
        uint32_t tail;
        struct msg      **msgs;
        struct msg      *list;
};

#define CNT     0x10000
#define GP(x)   (x % (CNT))

static inline struct queue *
qinit(void)
{
        struct queue *q = malloc(sizeof(*q));

        bzero(q, sizeof(*q));
        q->msgs = calloc(sizeof(struct msg *), CNT);

        return q;
}

static inline int
push(struct queue *q, struct msg *m)
{
        uint32_t tail = GP(__sync_fetch_and_add(&q->tail, 1));

        if (!__sync_bool_compare_and_swap(&q->msgs[tail], NULL, m)) {
                struct msg *last;
                do {
                        last = q->list;
                        m->next = last;
                } while (!__sync_bool_compare_and_swap(&q->list, last, m));
        }
        return 0;
}

static inline struct msg *
pop(struct queue *q)
{
        uint32_t head = q->head;
        uint32_t h2;
        struct msg *list;

        if (head == q->tail)
                return NULL;

        h2 = GP(head);
        list = q->list;

        if (list) {
                struct msg *n = list->next;
                if (__sync_bool_compare_and_swap(&q->list, list, n)) {
                        list->next = NULL;
                        push(q, list);
                }
        }
        struct msg *m = q->msgs[h2];
        if (!m)
                return NULL;
        if (!__sync_bool_compare_and_swap(&q->head, head, head + 1))
                return NULL;
        if (!__sync_bool_compare_and_swap(&q->msgs[h2], m, NULL))
                return NULL;

        return m;
}