dol: initial dol commit

[jump.git] / dol / src / dol / visitor / protothread / lib / Fifo.cpp

diff --git a/dol/src/dol/visitor/protothread/lib/Fifo.cpp b/dol/src/dol/visitor/protothread/lib/Fifo.cpp
new file mode 100644 (file)
index 0000000..767b784
--- /dev/null
+++ b/dol/src/dol/visitor/protothread/lib/Fifo.cpp
@@ -0,0 +1,130 @@
+#include "Fifo.h"\r
+\r
+/**\r
+ *\r
+ */\r
+Fifo::Fifo(unsigned size = 18) {\r
+    //std::cout << "Create Fifo." << std::endl;\r
+    _size = size;\r
+    _buffer = new char[_size];\r
+    _use = 0; \r
+    _tail = 0;\r
+}\r
+\r
+/**\r
+ *\r
+ */\r
+Fifo::~Fifo() {\r
+    //std::cout << "Delete Fifo." << std::endl;\r
+    if (_buffer) {\r
+        delete _buffer;\r
+    }\r
+    _buffer = 0;\r
+    _use = 0; \r
+    _tail = 0;\r
+    //std::cout << "Deleted Fifo." << std::endl;\r
+}\r
+\r
+/**\r
+ *\r
+ */\r
+unsigned Fifo::read(void *destination, unsigned len) {\r
+    char* buffer = (char*)destination;\r
+    unsigned read = (len <= _use ? len : 0);\r
+    //std::cout << "Try to read " << len << " bytes from Fifo." << std::endl;\r
+\r
+    if (_tail + read <= _size) {\r
+        memcpy(buffer, _buffer + _tail, read);\r
+    }\r
+    else {\r
+        memcpy(buffer, _buffer + _tail, _size - _tail);\r
+        memcpy(buffer + _size - _tail, _buffer, read - _size + _tail);\r
+    }\r
+\r
+    _tail = (_tail + read) % _size;\r
+    _use -= read;\r
+    //std::cout << "Read " << read << " bytes from Fifo." << std::endl;\r
+    return read;\r
+}\r
+\r
+/**\r
+ *\r
+ */\r
+unsigned Fifo::write(const void *source, unsigned len) {\r
+    char* buffer = (char*)source;\r
+    unsigned write = (len <= unused() ? len : 0);\r
+    unsigned head = (_tail + _use) % _size;\r
+    //std::cout << "Try to write " << len << " bytes to Fifo." << std::endl;\r
+\r
+    if (head + write <= _size) {\r
+        memcpy(_buffer + head, buffer, write);\r
+    }\r
+    else {\r
+        memcpy(_buffer + head, buffer, _size - head);\r
+        memcpy(_buffer, buffer + _size - head, write - _size + head);\r
+    }\r
+\r
+    _use += write;\r
+    //std::cout << "Wrote " << write << " bytes to Fifo." << std::endl;\r
+    return write;\r
+}\r
+\r
+/**\r
+ *\r
+ */\r
+unsigned Fifo::size() const {\r
+    return (_size);\r
+}\r
+\r
+/**\r
+ *\r
+ */\r
+unsigned Fifo::unused() const {\r
+    return (_size) - _use;\r
+}\r
+\r
+/**\r
+ *\r
+ */\r
+unsigned Fifo::used() const {\r
+    return _use; \r
+}\r
+\r
+/**\r
+ * Test the implementation\r
+ */\r
+/*\r
+int main() {\r
+    std::cout.width(5);\r
+    Fifo *myFifo = new Fifo();\r
+    for (int j = 0; j < 3; j++) {\r
+        for (int i = 0; i < 6; i++) {\r
+            std::cout << "write " << i << " to Fifo.    ";\r
+            int write = myFifo->write(&i, sizeof(int));\r
+            printf(" %d ", write);\r
+            if (write == sizeof(int)) {\r
+                std::cout << "used: " << std::setw(2) << myFifo->used()\r
+                        << ", unused: " << std::setw(2) << myFifo->unused()\r
+                        << ", size: "  << std::setw(2) << myFifo->size()\r
+                        << std::endl;\r
+            } else {\r
+                std::cout << std::endl;\r
+            }\r
+        }\r
+        for (int i = 0; i < 6; i++) {\r
+            int value;\r
+            int read = myFifo->read(&value, sizeof(int));\r
+            printf(" %d ", read);\r
+            if (read == sizeof(int)) {\r
+                std::cout << "read " << value << "  from Fifo   ";\r
+                std::cout << "used: " << std::setw(2) << myFifo->used()\r
+                        << ", unused: " << std::setw(2) << myFifo->unused()\r
+                        << ", size: "  << std::setw(2) << myFifo->size()\r
+                        << std::endl;\r
+            }\r
+        }\r
+    }\r
+    delete myFifo;\r
+    return 0;\r
+}\r
+*/\r