Communication: MultiChannel: Properly mask out communication from non-main VNETs

This also makes it possible for Communication to create more instances of Packetizer
This is necessary because Packetizer is not thread safe,
so when we support more VNETs we will need to create more Packetizers.

communication/communication.cpp

@@ -60,7 +60,7 @@ bool Communication::sendPacket(std::vector<uint8_t>& bytes) {
 
 bool Communication::sendCommand(Command cmd, std::vector<uint8_t> arguments) {
 	std::vector<uint8_t> packet;
-	if(!encoder->encode(packet, cmd, arguments))
+	if(!encoder->encode(*packetizer, packet, cmd, arguments))
 		return false;
 
 	return sendPacket(packet);
@@ -176,7 +176,7 @@ void Communication::readTask() {
 		readBytes.clear();
 		if(impl->readWait(readBytes)) {
 			if(packetizer->input(readBytes)) {
-				for(auto& packet : packetizer->output()) {
+				for(const auto& packet : packetizer->output()) {
 					std::shared_ptr<Message> msg;
 					if(!decoder->decode(msg, packet))
 						continue;

communication/encoder.cpp

@@ -6,7 +6,7 @@
 
 using namespace icsneo;
 
-bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message>& message) {
+bool Encoder::encode(const Packetizer& packetizer, std::vector<uint8_t>& result, const std::shared_ptr<Message>& message) {
 	bool shortFormat = false;
 	bool useResultAsBuffer = false; // Otherwise it's expected that we use message->data
 	result.clear();
@@ -70,7 +70,7 @@ bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message
 							(uint8_t)(size >> 8),
 							(uint8_t)m51msg->command
 						});
-						result = packetizer->packetWrap(message->data, shortFormat);
+						result = packetizer.packetWrap(message->data, shortFormat);
 						return true;
 					} else {
 						message->data.insert(message->data.begin(), { uint8_t(m51msg->command) });
@@ -108,11 +108,11 @@ bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message
 		});
 	}
 
-	result = packetizer->packetWrap(buffer, shortFormat);
+	result = packetizer.packetWrap(buffer, shortFormat);
 	return true;
 }
 
-bool Encoder::encode(std::vector<uint8_t>& result, Command cmd, std::vector<uint8_t> arguments) {
+bool Encoder::encode(const Packetizer& packetizer, std::vector<uint8_t>& result, Command cmd, std::vector<uint8_t> arguments) {
 	std::shared_ptr<Message> msg;
 	if(cmd == Command::UpdateLEDState) {
 		/* NetID::Device is a super old command type.
@@ -148,5 +148,5 @@ bool Encoder::encode(std::vector<uint8_t>& result, Command cmd, std::vector<uint
 		msg->data.insert(msg->data.end(), std::make_move_iterator(arguments.begin()), std::make_move_iterator(arguments.end()));
 	}
 	
-	return encode(result, msg);
+	return encode(packetizer, result, msg);
 }

communication/multichannelcommunication.cpp

@@ -2,19 +2,25 @@
 #include "icsneo/communication/command.h"
 #include "icsneo/communication/decoder.h"
 #include "icsneo/communication/packetizer.h"
-#include <iostream>
-#include <iomanip>
 
 using namespace icsneo;
 
 void MultiChannelCommunication::spawnThreads() {
-	mainChannelReadThread = std::thread(&MultiChannelCommunication::readTask, this);
+	for(size_t i = 0; i < NUM_SUPPORTED_VNETS; i++) {
+		while(vnetQueues[i].pop()) {} // Ensure the queue is empty
+		vnetThreads[i] = std::thread(&MultiChannelCommunication::vnetReadTask, this, i);
+	}
+	hidReadThread = std::thread(&MultiChannelCommunication::hidReadTask, this);
 }
 
 void MultiChannelCommunication::joinThreads() {
 	closing = true;
-	if(mainChannelReadThread.joinable())
-		mainChannelReadThread.join();
+	if(hidReadThread.joinable())
+		hidReadThread.join();
+	for(auto& thread : vnetThreads) {
+		if(thread.joinable())
+			thread.join();
+	}
 	closing = false;
 }
 
@@ -23,7 +29,7 @@ bool MultiChannelCommunication::sendPacket(std::vector<uint8_t>& bytes) {
 	return rawWrite(bytes);
 }
 
-void MultiChannelCommunication::readTask() {
+void MultiChannelCommunication::hidReadTask() {
 	bool readMore = true;
 	bool gotPacket = false; // Have we got the first valid packet (don't flag errors otherwise)
 	std::deque<uint8_t> usbReadFifo;
@@ -51,7 +57,8 @@ void MultiChannelCommunication::readTask() {
 
 					if(!CommandTypeIsValid(currentCommandType)) {
 						// Device to host bytes discarded
-						EventManager::GetInstance().add(APIEvent(APIEvent::Type::FailedToRead, APIEvent::Severity::Error));
+						if(gotPacket)
+							EventManager::GetInstance().add(APIEvent(APIEvent::Type::FailedToRead, APIEvent::Severity::Error));
 						usbReadFifo.pop_front();
 						continue;
 					}
@@ -105,21 +112,65 @@ void MultiChannelCommunication::readTask() {
 						payloadBytes[i] = usbReadFifo[0];
 						usbReadFifo.pop_front();
 					}
-					
-					if(packetizer->input(payloadBytes)) {
-						for(auto& packet : packetizer->output()) {
-							std::shared_ptr<Message> msg;
-							if(!decoder->decode(msg, packet))
-								continue; // Error will have been reported from within decoder
 
-							gotPacket = true;
-							dispatchMessage(msg);
-						}
+					moodycamel::BlockingReaderWriterQueue< std::vector<uint8_t> >* currentQueue = nullptr;
+					switch(currentCommandType) {
+						case CommandType::Vnet1_to_HostPC:
+							currentQueue = &vnetQueues[0];
+							break;
+						case CommandType::Vnet2_to_HostPC:
+							if(NUM_SUPPORTED_VNETS >= 2)
+								currentQueue = &vnetQueues[1];
+							break;
+						case CommandType::Vnet3_to_HostPC:
+							if(NUM_SUPPORTED_VNETS >= 3)
+								currentQueue = &vnetQueues[2];
+							break;
 					}
 
+					if(currentQueue == nullptr) {
+						state = PreprocessState::SearchForCommand;
+						break;
+					}
+
+					if(!currentQueue->enqueue(std::move(payloadBytes)) && gotPacket)
+						EventManager::GetInstance().add(APIEvent(APIEvent::Type::FailedToRead, APIEvent::Severity::Error));
+					payloadBytes.clear();
+					gotPacket = true;
 					state = PreprocessState::SearchForCommand;
+					break;
+			}
+		}
+	}
+}
+
+void MultiChannelCommunication::vnetReadTask(size_t vnetIndex) {
+	moodycamel::BlockingReaderWriterQueue< std::vector<uint8_t> >& queue = vnetQueues[vnetIndex];
+	std::vector<uint8_t> payloadBytes;
+	std::unique_ptr<Packetizer> packetizerLifetime;
+	Packetizer* vnetPacketizer;
+	if(vnetIndex == 0)
+		vnetPacketizer = packetizer.get();
+	else {
+		packetizerLifetime = makeConfiguredPacketizer();
+		vnetPacketizer = packetizerLifetime.get();
+	}
+
+	EventManager::GetInstance().downgradeErrorsOnCurrentThread();
+
+	while(!closing) {
+		if(queue.wait_dequeue_timed(payloadBytes, std::chrono::milliseconds(250))) {
+			if(closing)
+				break;
+			
+			if(vnetPacketizer->input(payloadBytes)) {
+				for(const auto& packet : vnetPacketizer->output()) {
+					std::shared_ptr<Message> msg;
+					if(!decoder->decode(msg, packet))
+						continue; // Error will have been reported from within decoder
+					dispatchMessage(msg);
+				}
 			}
 		}
-		
 	}
 }

communication/packetizer.cpp

@@ -13,7 +13,7 @@ uint8_t Packetizer::ICSChecksum(const std::vector<uint8_t>& data) {
 	return (uint8_t)checksum;
 }
 
-std::vector<uint8_t>& Packetizer::packetWrap(std::vector<uint8_t>& data, bool shortFormat) {
+std::vector<uint8_t>& Packetizer::packetWrap(std::vector<uint8_t>& data, bool shortFormat) const {
 	if(shortFormat) {
 		// Some devices don't use the checksum, so might as well not calculate it if that's the case
 		// Either way the byte is still expected to be present in the bytestream for short messages