gadget-flash-extractor/cbuf.c

/*  
 *      This is free software: you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License version 2, as 
 *      published by the Free Software Foundation.
 *
 *      this software is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *
 *      You should have received a copy of the GNU General Public License
 *      along with this software.  If not, see <http://www.gnu.org/licenses/>.
 *
 *      Authors: Maxime Vincent, Daniele Lacamera
 *      
 *
 */  

#include <string.h>
#include "cbuf.h"

struct cbuf {
    uint8_t *buf;
    uint8_t *readptr;
    uint8_t *writeptr;
    int     bufsize;
};

#define BUFSIZE 128
#define NBUFS 3

static struct cbuf cbufs[NBUFS];
static uint8_t buffers[BUFSIZE][NBUFS];
static int cur_cbuf = 0;

struct cbuf * cbuf_create(void)
{
    struct cbuf* inbuf;
    inbuf = &cbufs[cur_cbuf];
    inbuf->buf = buffers[cur_cbuf];
    inbuf->bufsize = BUFSIZE;
    inbuf->readptr = inbuf->buf;
    inbuf->writeptr = inbuf->buf;
    cur_cbuf++;
    return inbuf;
}

/* 0 on success, -1 on fail */
int cbuf_writebyte(struct cbuf *cb, uint8_t byte)
{
    if (!cb)
        return -1;

    /* check if there is space */
    if (!cbuf_bytesfree(cb))
        return -1;

    *cb->writeptr = byte;
    cb->writeptr++;

    /* wrap if needed */
    if (cb->writeptr > (cb->buf + cb->bufsize - 1u))
        cb->writeptr = cb->buf;

    return 0;
}

/* 0 on success, -1 on fail */
int cbuf_readbyte(struct cbuf *cb, uint8_t *byte)
{
    if (!cb || !byte)
        return -1;

    /* check if there is data */
    if (!cbuf_bytesinuse(cb))
        return -1;

    *byte = *cb->readptr++;

    /* wrap if needed */
    if (cb->readptr > (cb->buf + cb->bufsize - 1u))
        cb->readptr = cb->buf;

    return 0;
}

int cbuf_readbytes(struct cbuf *cb, void *bytes, int len)
{
    int buflen;
    int i;
    char *dst = bytes;
    if (!cb || !bytes)
        return -1;

    /* check if there is data */
    buflen = cbuf_bytesinuse(cb);
    if (buflen == 0)
        return -1;
    if (len > buflen)
        len = buflen;

    for (i = 0; i < len; i++) {
        dst[i] = *(cb->readptr++);
        /* wrap if needed */
        if (cb->readptr > (cb->buf + cb->bufsize - 1u))
            cb->readptr = cb->buf;
    }
    return len;
}

/* written len on success, 0 on fail */
int cbuf_writebytes(struct cbuf *cb, const uint8_t * bytes, int len)
{
    uint8_t byte;
    int freesize;
    int tot_len = len;
    if (!cb)
        return 0;

    /* check if there is space */
    freesize = cbuf_bytesfree(cb);
    if (!freesize)
        return 0;
    if (freesize < len) {
        len = freesize;
        tot_len = freesize;
    }

    /* Wrap needed ? */
    if ((cb->writeptr + len) > (cb->buf + cb->bufsize) - 1u)
    {
        int len_first_part = cb->buf + cb->bufsize - cb->writeptr; /* end - current position */
        memcpy(cb->writeptr, bytes, len_first_part);
        bytes += len_first_part;
        cb->writeptr = cb->buf; /* set to start of buffer */
        len -= len_first_part;
    }
    /* write remaining part */
    if (len)
    {
        memcpy(cb->writeptr, bytes, len);
        cb->writeptr += len;
    }

    return tot_len;
}

int cbuf_bytesfree(struct cbuf *cb)
{
    int bytes;
    if (!cb)
        return -1;

    bytes = (int)(cb->readptr - cb->writeptr - 1);
    if (cb->writeptr >= cb->readptr)
        bytes += cb->bufsize;

    return bytes;
}

int cbuf_bytesinuse(struct cbuf *cb)
{
    int bytes;
    if (!cb)
        return -1;

    bytes = (int)(cb->writeptr - cb->readptr);
    if (cb->writeptr < cb->readptr)
        bytes += cb->bufsize;

    return (bytes);
}