q3.h

#include <stdint.h>
#include <stdlib.h>
#include <emmintrin.h>

#ifndef likely
#define likely(x)       __builtin_expect((x), 1)
#endif

#ifndef unlikely
#define unlikely(x)     __builtin_expect((x), 0)
#endif


#define QSZ (1024 * 1)
#define QMSK    (QSZ - 1)

struct msg {
    uint64_t dummy;
};

#define CACHE_LINE_SIZE     64

struct queue {
    struct {
        uint32_t mask;
        uint32_t size;
        volatile uint32_t head;
        volatile uint32_t tail;
    } p;
    char pad[CACHE_LINE_SIZE - 4 * sizeof(uint32_t)];

    struct {
        uint32_t mask;
        uint32_t size;
        volatile uint32_t head;
        volatile uint32_t tail;
    } c;
    char pad2[CACHE_LINE_SIZE - 4 * sizeof(uint32_t)];

    void              *msgs[0];
};

static inline struct queue *
qinit(void)
{
    struct queue *q = calloc(1, sizeof(*q) + QSZ * sizeof(void *));
    q->p.size = q->c.size = QSZ;
    q->p.mask = q->c.mask = QMSK;

    return q;
}


static inline int
push(struct queue *q, void *m)
{
    uint32_t head, tail, mask, next;
    int ok;

    mask = q->p.mask;

    do {
        head = q->p.head;
        tail = q->c.tail;
        if ((mask + tail - head) < 1U)
            return -1;
        next = head + 1;
        ok = __sync_bool_compare_and_swap(&q->p.head, head, next);
    } while (!ok);

    q->msgs[head & mask] = m;
    asm volatile ("":::"memory");

    while (unlikely((q->p.tail != head)))
        _mm_pause();

    q->p.tail = next;

    return 0;
}

static inline void *
pop(struct queue *q)
{
    uint32_t head, tail, mask, next;
    int ok;
    void *ret;

    mask = q->c.mask;

    do {
        head = q->c.head;
        tail = q->p.tail;
        if ((tail - head) < 1U)
            return NULL;
        next = head + 1;
        ok = __sync_bool_compare_and_swap(&q->c.head, head, next);
    } while (!ok);

    ret = q->msgs[head & mask];
    asm volatile ("":::"memory");

    while (unlikely((q->c.tail != head)))
        _mm_pause();

    q->c.tail = next;

    return ret;
}