+#include "Fifo.h"
+
+/**
+ *
+ */
+Fifo::Fifo(unsigned size = 18) {
+ //except at the beginning, _head and _tail must never overlap,
+ //otherwise one does not know whether the buffer is full or
+ //empty. To have nevertheless a buffer with the given capacity,
+ //a buffer with one more element is allocated.
+ _size = size;
+ _buffer = (char *) _malloc_align(_size, ALIGNMENT_FACTOR);
+ if(!_buffer) {
+ fprintf(stderr,"[FIFO] Memory allocation failure\n");
+ exit(-1);
+ }
+
+ _tail = 0;
+ _pos = 0;
+ _blocked = 0;
+
+ _inTail = 0;
+ _activeDMA = false;
+}
+
+/**
+ *
+ */
+Fifo::~Fifo() {
+ if (_buffer) {
+ _free_align(_buffer);
+ }
+ _buffer = 0;
+ _tail = 0;
+ _pos = 0;
+}
+
+/**
+ *
+ */
+unsigned Fifo::read(void *destination, unsigned len) {
+ char* buffer = (char*) destination;
+ unsigned read = (len <= used() ? len : used());
+
+ if (_tail + read < _size) {
+ memcpy(buffer, _buffer + _tail, read);
+ } else {
+ memcpy(buffer, _buffer + _tail, _size - _tail);
+ memcpy(buffer + _size - _tail, _buffer, read - _size + _tail);
+ }
+
+ _tail = ((unsigned) (_tail + read) % _size);
+ _pos -= read;
+ return read;
+}
+
+/**
+ *
+ */
+unsigned Fifo::write(const void *source, unsigned len) {
+
+ char* buffer = (char*) source;
+ unsigned write = (len <= unused() ? len : unused());
+ unsigned head = (_tail + _pos) % _size;
+
+ if (head + write < _size) {
+ memcpy(_buffer + head, buffer, write);
+ } else {
+ memcpy(_buffer + head, buffer, _size - head);
+ memcpy(_buffer, buffer + _size - head, write - _size + head);
+ }
+
+ _pos += write;
+ return write;
+}
+
+/**
+ * Size of this fifo
+ */
+unsigned Fifo::size() const {
+ return (_size);
+}
+
+/**
+ * How many bytes are currently free in the buffer
+ */
+unsigned Fifo::unused() const {
+ return (_size) - used();
+}
+
+/**
+ * How many data are currently stored in the fifo
+ */
+unsigned Fifo::used() const {
+ return _pos;
+}
+
+/*
+ * Get the pointer to the start of the queue
+ */
+char *Fifo::getQueuePointer() {
+ return _buffer;
+}
+
+/*
+ * Has completed a dma read process, i.e. has read out of the queue
+ */
+void Fifo::dmaRead(unsigned len) {
+ _activeDMA = false;
+
+ if (len == 0) {
+ _blocked = BLOCKED_MAX_NR;
+ } else {
+ _tail = ((unsigned) (_tail + len) % _size);
+ _pos -= len;
+ }
+}
+
+/*
+ * Start a DMA request, returns the current space one have
+ */
+unsigned Fifo::dmaStart() {
+ _activeDMA = true;
+
+ if (_tail + _pos > _size) {
+ return _size - _tail;
+ } else {
+ return _pos;
+ }
+}
+
+/*
+ * Is allowed to start a dma request
+ */
+bool Fifo::dmaAllowed() {
+ if (_blocked > 0) {
+ _blocked--;
+ return false;
+ } else {
+ return !_activeDMA;
+ }
+}