単一生産者単一消費者モデルのロックレスキュー実装

#ifndef FIFO_H
#define FIFO_H

struct FIFO
{
    void **buffer;
    unsigned int size;
    unsigned int in;
    unsigned int out;
};

struct FIFO *FIFO_alloc(unsigned int size);

unsigned int FIFO_put(struct FIFO *FIFO,  void** obj_table, unsigned int n);

unsigned int FIFO_get(struct FIFO *FIFO, void** obj_table, unsigned int len);

#endif // FIFO_H

#include "fifo.h"
#include 
#include 
#include 

#define min(a, b) ((a) > (b))?(b):(a)
#define max(a, b) ((a) > (b))?(a):(b)

#ifdef CONFIG_X86_32
/* “lock; addl $0,0(%%esp)” ， 0 ， ， ， 。 XMM2 CPU ， */
#define mb() alternative("lock; addl $0,0(%%esp)", "mfence", X86_FEATURE_XMM2)
#define rmb() alternative("lock; addl $0,0(%%esp)", "lfence", X86_FEATURE_XMM2)
#define wmb() alternative("lock; addl $0,0(%%esp)", "sfence", X86_FEATURE_XMM)
#else
#define mb() asm volatile("mfence":::"memory")
#define rmb() asm volatile("lfence":::"memory")
#define wmb() asm volatile("sfence" ::: "memory")
#endif

static inline int fls(int x)
{
    int r;


    __asm__("bsrl %1,%0
\t"
            "jnz 1f
\t"
            "movl $-1,%0
"
            "1:" : "=r" (r) : "rm" (x));
    return r+1;
}

static inline unsigned int roundup_pow_of_two(unsigned int x)
{
    return 1UL << fls(x - 1);
}

struct FIFO *FIFO_alloc(unsigned int size)
{
    struct FIFO *ret = (struct FIFO*)malloc(sizeof(struct FIFO));

    if (size & (size - 1))
    {
        size = roundup_pow_of_two(size);
    }

    ret->buffer = (void**)malloc(size*sizeof(void*));
    ret->size = size;

    if (!ret->buffer)
    {
        return NULL;
    }

    memset(ret->buffer, 0, size*sizeof(void*));
    ret->in  = 0 ;
    ret->out = 0;

    return ret;
}

unsigned int FIFO_put(struct FIFO *FIFO,  void** obj_table, unsigned int n)
{
    unsigned int l;
    unsigned int len;
    unsigned int i, nCount, nIndex;

    len = min(n, FIFO->size - FIFO->in + FIFO->out);
    mb();

    l = min(len, FIFO->size - (FIFO->in & (FIFO->size - 1)));
    nCount = l;

    nIndex = FIFO->in & (FIFO->size - 1);
    for(i=0; ibuffer[nIndex + i] = obj_table[i];
    }

    nCount = len-l;
    for(i=0; ibuffer[i] = obj_table[l+ i];
    }

    wmb();

    FIFO->in += len;

    return len;
}

unsigned int FIFO_get(struct FIFO *FIFO, void** obj_table, unsigned int len)
{
    unsigned int l;
    unsigned int nCount, nIndex;
    unsigned int i;

    len = min(len, FIFO->in - FIFO->out);

    rmb();

    l = min(len, FIFO->size - (FIFO->out & (FIFO->size - 1)));


    nCount = l;
    nIndex = FIFO->out & (FIFO->size - 1);
    for(i=0; ibuffer[nIndex + i];
    }

    nIndex = nCount;
    nCount = len - l;
    for(i=0; ibuffer[i];
    }

    mb();

    FIFO->out += len;

    return len;
}