--- /dev/null
+#include "WindowedFifo.h"
+
+/**
+ *
+ */
+WindowedFifo::WindowedFifo(unsigned size = 20) {
+ //std::cout << "Create WindowedFifo." << std::endl;
+
+ _size = size;
+ _buffer = (char *) _malloc_align(_size, ALIGNMENT_FACTOR);
+ if(!_buffer) {
+ fprintf(stderr,"[WFIFO] Memory allocation failure\n");
+ exit(-1);
+ }
+ _head = 0;
+ _tail = 0;
+ _headRoom = 0;
+ _tailRoom = 0;
+ _use = 0;
+
+ _isHeadReserved = false;
+ _isTailReserved = false;
+
+ // For DMA transfers
+ _blocked = 0;
+ _activeDMA = false;
+}
+
+/**
+ *
+ */
+WindowedFifo::~WindowedFifo() {
+ //std::cout << "Delete WindowedFifo." << std::endl;
+ if (_buffer) {
+ _free_align(_buffer);
+ }
+ _buffer = 0;
+ _head = 0;
+ _tail = 0;
+ _use = 0;
+ //std::cout << "Deleted WindowedFifo." << std::endl;
+}
+
+/**
+ *
+ */
+unsigned WindowedFifo::reserve(char** destination, unsigned len) {
+
+ //std::cout << "Attempt to reserve " << len << " bytes." << std::endl;
+
+ //can only reserve once piece at a time
+ if (_isHeadReserved) {
+ *destination = 0;
+ return 0;
+ }
+
+ //reserve at most as much memory as still available in the buffer
+ unsigned write = (len <= _size - _use ? len : 0);
+
+ if (write > 0) {
+ //if wrap-around in buffer: return only buffer for the
+ //contiguous buffer space
+ if (_head + write > _size) {
+ write = _size - _head;
+ }
+
+ _headRoom = (_head + write) == _size ? 0 : _head + write;
+ *destination = &(_buffer[_head]);
+ _isHeadReserved = true;
+ }
+
+ //std::cout << "Reserved " << write << " bytes." << std::endl;
+ _writeReserve = write;
+ return write;
+}
+
+/**
+ *
+ */
+void WindowedFifo::release() {
+ if (_isHeadReserved) {
+ //std::cout << "Released " << _headRoom - _head << " bytes." << std::endl;
+ _head = _headRoom;
+ _use += _writeReserve;
+ _isHeadReserved = false;
+ }
+}
+
+/**
+ *
+ */
+unsigned WindowedFifo::capture(char **destination, unsigned len) {
+
+ //std::cout << "Attempt to capture " << len << " bytes." << std::endl;
+
+ if (_isTailReserved) {
+ //std::cout << "Only one attempt to capture allowed." << std::endl;
+ *destination = 0;
+ return 0;
+ }
+
+ //capture at most as much data as available in the buffer
+ unsigned read = (len <= _use ? len : 0);
+
+ if (read > 0) {
+ //if wrap-around in buffer: return only buffer for the
+ //conntiguous buffer space
+ if (_tail + read > _size) {
+ read = _size - _tail;
+ }
+
+ _tailRoom = (_tail + read) == _size ? 0 : _tailRoom = _tail + read;
+ *destination = &(_buffer[_tail]);
+ _isTailReserved = true;
+ }
+
+ _readReserve = read;
+ //std::cout << "Captured " << read << " bytes." << std::endl;
+
+ return read;
+}
+
+/**
+ *
+ */
+void WindowedFifo::consume() {
+ if (_isTailReserved) {
+ //std::cout << "Consumed " << _tailRoom - _tail << " bytes." << std::endl;
+ _tail = _tailRoom;
+ _use -= _readReserve;
+ _isTailReserved = false;
+ }
+}
+
+/**
+ *
+ */
+unsigned WindowedFifo::size() const {
+ return _size;
+}
+
+/**
+ *
+ */
+unsigned WindowedFifo::unused() const {
+ return _size - _use;
+}
+
+/**
+ *
+ */
+unsigned WindowedFifo::used() const {
+ return _use;
+}
+
+/*
+ * Get the pointer to the start of the queue
+ */
+char *WindowedFifo::getQueuePointer() {
+ return _buffer;
+}
+
+/*
+ * Has completed a dma read process, i.e. has read out of the queue
+ */
+void WindowedFifo::dmaRead(unsigned len) {
+ if (len == 0) {
+ _blocked = BLOCKED_MAX_NR;
+ } else {
+ _tail = ((unsigned) (_tail + len) % _size);
+ _use -= len;
+ }
+
+ _activeDMA = false;
+}
+
+/*
+ * Start a DMA request, returns the current space one have
+ */
+unsigned WindowedFifo::dmaStart() {
+ _activeDMA = true;
+
+ if (_tail + _use > _size) {
+ return _size - _tail;
+ } else {
+ return _use;
+ }
+}
+
+/*
+ * Is allowed to start a dma request
+ */
+bool WindowedFifo::dmaAllowed() {
+#ifndef STORE_REQUESTS
+ if (_blocked > 0) {
+ _blocked--;
+ return false;
+ } else {
+ return !_activeDMA;
+ }
+#else
+ return !_activeDMA;
+#endif
+}
+
+/**
+ * Is needed for DMA transfers
+ */
+unsigned WindowedFifo::dmaWrite(const void *source, unsigned len) {
+
+ char* buffer = (char*) source;
+
+ if (_head + len < _size) {
+ memcpy(_buffer + _head, buffer, len);
+ } else {
+ // We should never be here!
+ memcpy(_buffer + _head, buffer, _size - _head);
+ memcpy(_buffer, buffer + _size - _head, len - _size + _head);
+ }
+ _use += len;
+ _head = (_head + len) >= _size ? _head + len - _size : _head + len;
+
+ return len;
+}
+
+/**
+ * Test the implementation
+ */
+/*
+ int main() {
+ WindowedFifo *myFifo = new WindowedFifo(16);
+
+ int* buf1;
+ int* buf2;
+ int x = myFifo->reserve((char**)&buf1, 8);
+ *buf1 = 10;
+ *(buf1 + 1) = 20;
+ myFifo->release();
+ int y = myFifo->capture((char**)&buf2, 8);
+ std::cout << "read " << *buf2 << " " << *(buf2 + 1) << std::endl;
+ myFifo->consume();
+
+ for (int j = 0; j < 3; j++) {
+ for (int i = 0; i < 6; i++) {
+ std::cout << "write " << i << " to Fifo. ";
+ int write = myFifo->reserve((char**)&buf1, sizeof(int));
+ if (write == sizeof(int)) {
+ *buf1 = i;
+ myFifo->release();
+ std::cout << "used: " << std::setw(2) << myFifo->used()
+ << ", unused: " << std::setw(2) << myFifo->unused()
+ << ", size: " << std::setw(2) << myFifo->size()
+ << std::endl;
+ } else {
+ std::cout << std::endl;
+ }
+ }
+ for (int i = 0; i < 16; i++) {
+ char* buf3;
+ int read = myFifo->capture((char**)&buf3, sizeof(char));
+ if (read == sizeof(char)) {
+ std::cout << "read " << (unsigned)*buf3 << " from Fifo ";
+ std::cout << "used: " << std::setw(2) << myFifo->used()
+ << ", unused: " << std::setw(2) << myFifo->unused()
+ << ", size: " << std::setw(2) << myFifo->size()
+ << std::endl;
+ myFifo->consume();
+ } else {
+ std::cout << "read nothing from Fifo." << std::endl;
+ }
+
+ }
+ }
+ delete myFifo;
+ return 0;
+ }
+ */