EFTZ_PMU/user_src/rkfifo.c

#include "rkfifo.h"
#include <string.h>
#include "common.h"

#define min(x,y)                                                               \
    ({                                                                         \
        uint32_t _min1 = x;                                                   \
        uint32_t _min2 = y;                                                   \
        (void)(&_min1 == &_min2);                                              \
        _min1 < _min2 ? _min1 : _min2;                                         \
    })

#define max(x,y)                                                               \
    ({                                                                         \
        uint32_t _max1 = x;                                                   \
        uint32_t _max2 = y;                                                   \
        (void)(&_max1 == &_max2);                                              \
        _max1 > _max2 ? _max1 : _max2;                                         \
    })

/* 检查是否是 2 的幂次数 */
#define is_power_of_2(x) ((x) != 0 && (((x) & ((x)-1)) == 0))

/**
 * @brief 返回输入参数的最高有效 bit 位
 *
 * @param x 输入参数
 * @return int 最高有效 bit 位, 从低到高 1~32, 0 表示无输入参数为 0
 */
static inline int fls_int(int x)
{
    int r = 32;

    if (!x)
        return 0;
    if (!(x & 0xffff0000u))
    {
        x <<= 16;
        r -= 16;
    }
    if (!(x & 0xff000000u))
    {
        x <<= 8;
        r -= 8;
    }
    if (!(x & 0xf0000000u))
    {
        x <<= 4;
        r -= 4;
    }
    if (!(x & 0xc0000000u))
    {
        x <<= 2;
        r -= 2;
    }
    if (!(x & 0x80000000u))
    {
        x <<= 1;
        r -= 1;
    }
    return r;
}

/**
 * @brief 找到 n 向下取最大的 2 的幂次数
 *
 * @param n 输入整数 n
 * @return uint32_t n 向下取最大的 2 的幂次数
 */
static inline uint32_t rounddown_pow_of_two(uint32_t n)
{
    if (0 == n || is_power_of_2(n))
    {
        return n;
    }
    else
    {
        return 1 << (fls_int(n) - 1);
    }
}

/**
 * @brief 查找 fifo 中的剩余空间
 * 
 * @param fifo 
 * @return unsigned int 
 */
static inline unsigned int kfifo_unused(rkfifo_t *fifo)
{
    return (fifo->mask + 1) - (fifo->in - fifo->out);
}

int rkfifo_init(rkfifo_t *fifo, void *buffer, uint32_t size, uint32_t esize)
{
    size /= esize;

    /* size 必须是 2 的幂次数 */
    if (!is_power_of_2(size))
    {
        size = rounddown_pow_of_two(size);
    }

    fifo->in = 0;
    fifo->out = 0;
    fifo->esize = esize;
    fifo->data = buffer;

    if (size < 2)
    {
        fifo->mask = 0;
        return -RKFIFO_EINVAL;
    }

    fifo->mask = size - 1;

    return 0;
}

static void __rkfifo_copy_in(rkfifo_t *fifo, const void *src, unsigned int len,
                            unsigned int offset)
{
    unsigned int size = fifo->mask + 1;
    unsigned int esize = fifo->esize;
    unsigned int l;

    offset &= fifo->mask;
    if (esize != 1)
    {
        offset *= esize;
        size *= esize;
        len *= esize;
    }
    l = min(len, size - offset);

    memcpy((uint8_t*)fifo->data + offset, src, l);
    memcpy(fifo->data, (uint8_t*)src + l, len - l);

    /* 多核处理器需要内存屏障 */
    /* smp_wmb(); */
}

unsigned int rkfifo_in(rkfifo_t *fifo, const void *buf, unsigned int len)
{
    unsigned int l;

    l = kfifo_unused(fifo);
    if (len > l)
        len = l;

    __rkfifo_copy_in(fifo, buf, len, fifo->in);
    fifo->in += len;
    return len;
}

static void __rkfifo_copy_out(rkfifo_t *fifo, void *dst, unsigned int len,
                              unsigned int off)
{
    unsigned int size = fifo->mask + 1;
    unsigned int esize = fifo->esize;
    unsigned int l;

    off &= fifo->mask;
    if (esize != 1)
    {
        off *= esize;
        size *= esize;
        len *= esize;
    }
    l = min(len, size - off);

    memcpy(dst, (uint8_t*)fifo->data + off, l);
    memcpy((uint8_t*)dst + l, fifo->data, len - l);

    /* smp_wmb(); */
}

unsigned int rkfifo_out_peek(rkfifo_t *fifo, void *buf, unsigned int len)
{
    unsigned int l;

    l = fifo->in - fifo->out;
    if (len > l)
        len = l;

    __rkfifo_copy_out(fifo, buf, len, fifo->out);
    return len;
}

unsigned int rkfifo_out(rkfifo_t *fifo, void *buf, unsigned int len)
{
    len = rkfifo_out_peek(fifo, buf, len);
    fifo->out += len;
    return len;
}