Replace concurrentqueue with ringbuffer

communication/communication.cpp

@@ -268,7 +268,7 @@ void Communication::readTask() {
 
 	while(!closing) {
 		readBytes.clear();
-		if(driver->readWait(readBytes)) {
+		if(driver->readAvailable()) {
 			handleInput(*packetizer, readBytes);
 		}
 	}
@@ -291,7 +291,7 @@ void Communication::handleInput(Packetizer& p, std::vector<uint8_t>& readBytes)
 			handleInput(p, readBytes); // and we might as well process this input ourselves
 		}
 	} else {
-		if(p.input(readBytes)) {
+		if(p.input(driver->getReadBuffer())) {
 			for(const auto& packet : p.output()) {
 				std::shared_ptr<Message> msg;
 				if(!decoder->decode(msg, packet))

communication/driver.cpp

@@ -8,37 +8,24 @@
 
 using namespace icsneo;
 
-bool Driver::read(std::vector<uint8_t>& bytes, size_t limit) {
-	// A limit of zero indicates no limit
-	if(limit == 0)
-		limit = (size_t)-1;
-
-	if(limit > (readQueue.size_approx() + 4))
-		limit = (readQueue.size_approx() + 4);
-
-	if(bytes.capacity() < limit)
-		bytes.resize(limit);
-
-	size_t actuallyRead = readQueue.try_dequeue_bulk(bytes.data(), limit);
-
-	if(bytes.size() > actuallyRead)
-		bytes.resize(actuallyRead);
-
-	return true;
-}
-
 bool Driver::readWait(std::vector<uint8_t>& bytes, std::chrono::milliseconds timeout, size_t limit) {
 	// A limit of zero indicates no limit
 	if(limit == 0)
 		limit = (size_t)-1;
 
-	if(limit > (readQueue.size_approx() + 4))
-		limit = (readQueue.size_approx() + 4);
+	if(limit > (readBuffer.size() + 4))
+		limit = (readBuffer.size() + 4);
 
 	bytes.resize(limit);
 
-	size_t actuallyRead = readQueue.wait_dequeue_bulk_timed(bytes.data(), limit, timeout);
-
+	// wait until we have enough data, or the timout occurs
+	const auto timeoutTime = std::chrono::steady_clock::now() + timeout;
+	while (readBuffer.size() < limit && std::chrono::steady_clock::now() < timeoutTime) {
+		std::this_thread::sleep_for(std::chrono::milliseconds(1));
+	}
+	size_t actuallyRead = std::min(readBuffer.size(), limit);
+	readBuffer.read(bytes.data(), 0, actuallyRead);
+	readBuffer.pop(actuallyRead);
 	bytes.resize(actuallyRead);
 
 #ifdef ICSNEO_DRIVER_DEBUG_PRINTS

communication/packetizer.cpp

@@ -24,11 +24,9 @@ std::vector<uint8_t>& Packetizer::packetWrap(std::vector<uint8_t>& data, bool sh
 	return data;
 }
 
-bool Packetizer::input(const std::vector<uint8_t>& inputBytes) {
+bool Packetizer::input(RingBuffer& bytes) {
 	bool haveEnoughData = true;
 
-	bytes.Copy(inputBytes);
-
 	while(haveEnoughData) {
 		switch(state) {
 			case ReadState::SearchForHeader:
@@ -152,14 +150,14 @@ bool Packetizer::input(const std::vector<uint8_t>& inputBytes) {
 				if(packetLength > 0)
 					packet.data.resize(packetLength - headerSize);
 
-				bytes.CopyTo(packet.data.data(), currentIndex, (packetLength - currentIndex));
+				bytes.read(packet.data.data(), currentIndex, (packetLength - currentIndex));
 				currentIndex = packetLength;
 
 				if(disableChecksum || !checksum || bytes[currentIndex] == ICSChecksum(packet.data)) {
 					// Got a good packet
 					gotGoodPackets = true;
 					processedPackets.push_back(std::make_shared<Packet>(packet));
-					bytes.Erase_front(packetLength);
+					bytes.pop(packetLength);
 
 					if(packet.network == Network::NetID::DiskData && (packetLength - headerSize) % 2 == 0) {
 						bytes.pop_front();

communication/ringbuffer.cpp

@@ -0,0 +1,91 @@
+#include "icsneo/communication/ringbuffer.h"
+#include <stdexcept>
+
+namespace icsneo {
+    
+RingBuffer::RingBuffer(size_t bufferSize) : readCursor(0), writeCursor(0) {
+    // round the buffer size to the nearest power of 2
+    bufferSize = RoundUp(bufferSize);
+    mask = bufferSize - 1;
+    buf = new uint8_t[bufferSize];
+}
+
+RingBuffer::~RingBuffer() {
+    delete[] buf;
+    buf = nullptr;
+}
+
+const uint8_t& RingBuffer::operator[](size_t offset) const { 
+    return get(offset); 
+}
+
+size_t RingBuffer::size() const { 
+    // The values in the cursors are monotonic, i.e. they only ever increment. They can be considered to be the total number of elements ever written or read
+    auto currentWriteCursor = writeCursor.load(std::memory_order_relaxed);
+    auto currentReadCursor = readCursor.load(std::memory_order_relaxed);
+    // Using unmasked values, writeCursor is guaranteed to be >= readCursor. If they are equal that means the buffer is empty
+    return currentWriteCursor - currentReadCursor; 
+}
+
+void RingBuffer::pop_front() {
+    pop(1);
+}
+
+void RingBuffer::pop(size_t count) {
+    if (size() < count) {
+        throw std::runtime_error("RingBuffer: Underflow");
+    }
+    readCursor.fetch_add(count, std::memory_order_release);
+}
+
+const uint8_t& RingBuffer::get(size_t offset) const {
+    if (offset >= size()) {
+        throw std::runtime_error("RingBuffer: Index out of range");
+    }
+    auto currentReadCursor = readCursor.load(std::memory_order_acquire);
+    return *resolve(currentReadCursor, offset);
+}
+
+bool RingBuffer::write(const uint8_t* addr, size_t length) {    
+    const auto freeSpace = (capacity() - size());
+    if (length > freeSpace) {
+        return false;
+    }
+    auto currentWriteCursor = writeCursor.load(std::memory_order_relaxed);
+    auto spaceAtEnd = std::min(freeSpace, capacity() - (currentWriteCursor & mask)); // number of bytes from (masked) writeCursor to the end of the writable space (i.e. we reach the masked read cursor or the end of the buffer)
+    auto firstCopySize = std::min(spaceAtEnd, length);
+    (void)memcpy(resolve(currentWriteCursor, 0), addr, firstCopySize);
+    if (firstCopySize < length)
+    {
+        (void)memcpy(buf, &addr[firstCopySize], length - firstCopySize);
+    }
+
+    writeCursor.store(currentWriteCursor + length, std::memory_order_release);
+    return true;
+}
+
+bool RingBuffer::write(const std::vector<uint8_t>& source) {
+    return write(source.data(), source.size());
+}
+
+bool RingBuffer::read(uint8_t* dest, size_t startIndex, size_t length) const {
+    auto currentSize = size();
+    if ((startIndex >= currentSize) || ((startIndex + length) > size())) {
+        return false;
+    }
+    auto currentReadCursor = readCursor.load(std::memory_order_relaxed);
+    auto bytesAtEnd = std::min<size_t>(capacity() - ((currentReadCursor + startIndex) & mask), length);
+    const auto bytesAtStart = (length - bytesAtEnd);
+
+    (void)memcpy(dest, resolve(currentReadCursor, startIndex), bytesAtEnd);
+    if (bytesAtStart > 0) {
+        (void)memcpy(&dest[bytesAtEnd], buf, bytesAtStart);
+    }
+    return true;
+}
+
+void RingBuffer::clear() {
+    pop(size());
+}
+
+}