STM32F4DoorControl/ringbuffer.c

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

unsigned ringbuffer_capacity(ringbuffer *buf) {
  return buf->size;
}

unsigned ringbuffer_free(ringbuffer *buf) {
  unsigned head = buf->head;
  unsigned tail = buf->tail;
  unsigned size = buf->size;

  if (head >= tail) {
    return size - (head - tail) - 1;
  }

  return tail - head - 1;
}

unsigned ringbuffer_level(ringbuffer *buf) {
  unsigned head = buf->head;
  unsigned tail = buf->tail;

  if (head >= tail) {
    return head - tail;
  }

  unsigned size = buf->size;
  return size - tail + head;
}

unsigned ringbuffer_add(ringbuffer *buf, void const *dat_, unsigned len) {
  uint8_t const *dat  = dat_;
  unsigned rem        = ringbuffer_free(buf);
  unsigned head       = buf->head;
  unsigned written    = 0;
  const unsigned size = buf->size;

  if (len > rem) {
    len = rem;
  }

  if (head + len > size) {
    written = size - head;
    memcpy(buf->data + head, dat, written);
    len -= written;
    head = buf->head = 0;
    dat += written;
  }
  memcpy(buf->data + head, dat, len);
  buf->head = (head + len) % buf->size;
  return written + len;
}

unsigned ringbuffer_empty(ringbuffer *buf) {
  return buf->head == buf->tail;
}

unsigned ringbuffer_peek(ringbuffer *buf, void *dst_, unsigned len) {
  uint8_t *dst     = dst_;
  unsigned level   = ringbuffer_level(buf);
  unsigned size    = buf->size;
  unsigned tail    = buf->tail;
  unsigned written = 0;
  if (len > level) {
    len = level;
  }
  if (tail + len > size) {
    written = size - tail;
    memcpy(dst, buf->data + tail, written);
    tail = 0;
    dst += written;
    len -= written;
  }
  memcpy(dst, buf->data + tail, len);
  return len + written;
}

void ringbuffer_skip(ringbuffer *buf, unsigned len) {
  buf->tail = (buf->tail + len) % (buf->size);
}

unsigned ringbuffer_get(ringbuffer *buf, void *dst, unsigned len) {
  len = ringbuffer_peek(buf, dst, len);
  ringbuffer_skip(buf, len);
  return len;
}

void ringbuffer_rst(ringbuffer *buf) {
  buf->head = buf->tail = 0;
}