pinebuds/utils/kfifo/kfifo.c

#include "kfifo.h"
#include "hal_trace.h"
#include <stdio.h>
#include <string.h>

static int is_power_of_2(unsigned int n) {
  int r = 0;
  if (n != 0 && ((n & (n - 1)) == 0))
    r = 1;

  return r;
}

void kfifo_init(struct kfifo *fifo, unsigned char *buffer, unsigned int len) {
  ASSERT(is_power_of_2(len), "kfifo_init : len %d not power of 2!!!!!", len);

  fifo->size = len;
  fifo->buffer = buffer;
  fifo->in = fifo->out = 0;
}
unsigned int kfifo_put(struct kfifo *fifo, unsigned char *buffer,
                       unsigned int len) {
  unsigned int l;
  len = MIN(len, fifo->size - fifo->in + fifo->out);
  __sync_synchronize();

  l = MIN(len, fifo->size - (fifo->in & (fifo->size - 1)));
  memcpy(fifo->buffer + (fifo->in & (fifo->size - 1)), buffer, l);
  memcpy(fifo->buffer, buffer + l, len - l);

  __sync_synchronize();
  fifo->in += len;

  return len;
}
unsigned int kfifo_get(struct kfifo *fifo, unsigned char *buffer,
                       unsigned int len) {
  unsigned int l;
  len = MIN(len, fifo->in - fifo->out);
  __sync_synchronize();

  l = MIN(len, fifo->size - (fifo->out & (fifo->size - 1)));
  memcpy(buffer, fifo->buffer + (fifo->out & (fifo->size - 1)), l);
  memcpy(buffer + l, fifo->buffer, len - l);

  __sync_synchronize();
  fifo->out += len;

  return len;
}
unsigned int kfifo_peek(struct kfifo *fifo, unsigned int len_want,
                        unsigned char **buff1, unsigned char **buff2,
                        unsigned int *len1, unsigned int *len2) {
  unsigned int l, len;

  *buff1 = *buff2 = NULL;
  *len1 = *len2 = 0;
  len = MIN(len_want, fifo->in - fifo->out);
  if (len < len_want) {
    return 0;
  }

  __sync_synchronize();
  l = MIN(len, fifo->size - (fifo->out & (fifo->size - 1)));
  *buff1 = fifo->buffer + (fifo->out & (fifo->size - 1));
  *len1 = l;
  if (l < len) {
    *buff2 = fifo->buffer;
    *len2 = len - l;
  }

  return len_want;
}

unsigned int kfifo_peek_to_buf(struct kfifo *fifo, unsigned char *buff,
                               unsigned int len) {
  unsigned char *buf1 = NULL, *buf2 = NULL;
  unsigned int len1 = 0, len2 = 0;

  kfifo_peek(fifo, len, &buf1, &buf2, &len1, &len2);

  if (len == (len1 + len2)) {
    memcpy(buff, buf1, len1);
    memcpy(buff + len1, buf2, len2);
  } else {
    return 0;
  }

  return len;
}

unsigned int kfifo_len(struct kfifo *fifo) { return (fifo->in - fifo->out); }

#if 0
struct kfifo test_kfifo;
unsigned char kfifo_buffer[32];
void kfifo_test(void)
{
    unsigned char b[10];
    kfifo_init(&test_kfifo, kfifo_buffer, 32);
    TRACE(1,"init 32 : fifo size %d", test_kfifo.size);

    kfifo_put(&test_kfifo, "1234567890", 10);
    TRACE(1,"put 10 : fifo  len %d", kfifo_len(&test_kfifo));

    kfifo_put(&test_kfifo, "abcdefghij", 10);
    TRACE(1,"put 10 : fifo  len %d", kfifo_len(&test_kfifo));

    kfifo_put(&test_kfifo, "!@#$%^&*()", 10);
    TRACE(1,"put 10 : fifo  len %d", kfifo_len(&test_kfifo));

    kfifo_put(&test_kfifo, "VVVV", 4);
    TRACE(1,"put 4 : fifo  len %d", kfifo_len(&test_kfifo));

    kfifo_get(&test_kfifo, b, 2);
    TRACE(1,"get 2 : fifo  len %d", kfifo_len(&test_kfifo));
    TRACE(0,"data:");
    for(int i = 0; i < 2; ++i) {
        TRACE(1,"%c-", b[i]);
    }
    TRACE(0," ");
    kfifo_get(&test_kfifo, b, 10);
    TRACE(1,"get 10 : fifo  len %d", kfifo_len(&test_kfifo));
    TRACE(0,"data:");
    for(int i = 0; i < 10; ++i) {
        TRACE(1,"%c-", b[i]);
    }
    TRACE(0," ");
    kfifo_get(&test_kfifo, b, 10);
    TRACE(1,"get 10 : fifo  len %d", kfifo_len(&test_kfifo));
    TRACE(0,"data:");
    for(int i = 0; i < 10; ++i) {
        TRACE(1,"%c-", b[i]);
    }
    TRACE(0," ");
    kfifo_get(&test_kfifo, b, 10);
    TRACE(1,"get 10 : fifo  len %d", kfifo_len(&test_kfifo));
    TRACE(0,"data:");
    for(int i = 0; i < 10; ++i) {
        TRACE(1,"%c-", b[i]);
    }
    TRACE(0," ");
}
#endif