Windows: PCAP: Rework for Ethernet Packetizer and Performance

include/icsneo/platform/windows/pcap.h

@@ -7,6 +7,7 @@
 #include "icsneo/device/neodevice.h"
 #include "icsneo/communication/driver.h"
 #include "icsneo/api/eventmanager.h"
+#include "icsneo/communication/ethernetpacketizer.h"
 #include <string>
 
 namespace icsneo {
@@ -33,8 +34,16 @@ private:
 	neodevice_t& device;
 	uint8_t deviceMAC[6];
 	bool openable = true;
+	EthernetPacketizer ethPacketizer;
+	
+	std::thread transmitThread;
+	pcap_send_queue* transmitQueue = nullptr;
+	std::condition_variable transmitQueueCV;
+	std::mutex transmitQueueMutex;
+	
 	void readTask();
 	void writeTask();
+	void transmitTask();
 
 	class NetworkInterface {
 	public:
@@ -51,27 +60,6 @@ private:
 	};
 	static std::vector<NetworkInterface> knownInterfaces;
 	NetworkInterface interface;
-
-	class EthernetPacket {
-	public: // Don't worry about endian when setting fields, this is all taken care of in getBytestream
-		EthernetPacket() {};
-		EthernetPacket(const std::vector<uint8_t>& bytestream);
-		EthernetPacket(const uint8_t* data, size_t size);
-		int loadBytestream(const std::vector<uint8_t>& bytestream);
-		std::vector<uint8_t> getBytestream() const;
-		uint8_t errorWhileDecodingFromBytestream = 0; // Not part of final bytestream, only for checking the result of the constructor
-		uint8_t destMAC[6] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
-		uint8_t srcMAC[6] = { 0x00, 0xFC, 0x70, 0xFF, 0xFF, 0xFF };
-		uint16_t etherType = 0xCAB1; // Big endian, Should be 0xCAB1 or 0xCAB2
-		uint32_t icsEthernetHeader = 0xAAAA5555; // Big endian, Should be 0xAAAA5555
-		// At this point in the packet, there is a 16-bit payload size, little endian
-		// This is calculated from payload size in getBytestream
-		uint16_t packetNumber = 0;
-		bool firstPiece = true; // These booleans make up a 16-bit bitfield, packetInfo
-		bool lastPiece = true;
-		bool bufferHalfFull = false;
-		std::vector<uint8_t> payload;
-	};
 };
 
 }

platform/windows/pcap.cpp

@@ -2,6 +2,7 @@
 #include "icsneo/communication/network.h"
 #include "icsneo/communication/communication.h"
 #include "icsneo/communication/packetizer.h"
+#include "icsneo/communication/ethernetpacketizer.h"
 #include <pcap.h>
 #include <iphlpapi.h>
 #pragma comment(lib, "IPHLPAPI.lib")
@@ -96,18 +97,18 @@ std::vector<PCAP::PCAPFoundDevice> PCAP::FindAll() {
 			knownInterfaces.emplace_back(interface);
 	}
 
-	constexpr auto openflags = (PCAP_OPENFLAG_PROMISCUOUS | PCAP_OPENFLAG_MAX_RESPONSIVENESS | PCAP_OPENFLAG_NOCAPTURE_LOCAL);
+	constexpr auto openflags = (PCAP_OPENFLAG_MAX_RESPONSIVENESS | PCAP_OPENFLAG_NOCAPTURE_LOCAL);
 	for(size_t i = 0; i < knownInterfaces.size(); i++) {
 		auto& interface = knownInterfaces[i];
 		if(interface.fullName.length() == 0)
 			continue; // Win32 did not find this interface in the previous step
 
-		interface.fp = pcap.open(interface.nameFromWinPCAP.c_str(), 30, openflags, 1, nullptr, errbuf);
+		interface.fp = pcap.open(interface.nameFromWinPCAP.c_str(), 1518, openflags, 1, nullptr, errbuf);
 
 		if(interface.fp == nullptr)
 			continue; // Could not open the interface
 
-		EthernetPacket requestPacket;
+		EthernetPacketizer::EthernetPacket requestPacket;
 		memcpy(requestPacket.srcMAC, interface.macAddress, sizeof(requestPacket.srcMAC));
 		requestPacket.payload.reserve(4);
 		requestPacket.payload = {
@@ -132,12 +133,11 @@ std::vector<PCAP::PCAPFoundDevice> PCAP::FindAll() {
 			if(res == 0)
 				continue; // Keep waiting for that packet
 			
-			EthernetPacket packet(data, header->caplen);
+			EthernetPacketizer::EthernetPacket packet(data, header->caplen);
 			// Is this an ICS response packet (0xCAB2) from an ICS MAC, either to broadcast or directly to us?
 			if(packet.etherType == 0xCAB2 && packet.srcMAC[0] == 0x00 && packet.srcMAC[1] == 0xFC && packet.srcMAC[2] == 0x70 && (
 				memcmp(packet.destMAC, interface.macAddress, sizeof(packet.destMAC)) == 0 ||
-				memcmp(packet.destMAC, BROADCAST_MAC, sizeof(packet.destMAC)) == 0 ||
+				memcmp(packet.destMAC, BROADCAST_MAC, sizeof(packet.destMAC)) == 0
-				memcmp(packet.destMAC, ICS_UNSET_MAC, sizeof(packet.destMAC)) == 0
 			)) {
 				/* We have received a packet from a device. We don't know if this is the device we're
 				 * looking for, we don't know if it's actually a response to our RequestSerialNumber
@@ -180,7 +180,7 @@ bool PCAP::IsHandleValid(neodevice_handle_t handle) {
 	return (netifIndex < knownInterfaces.size());
 }
 
-PCAP::PCAP(const device_eventhandler_t& err, neodevice_t& forDevice) : Driver(err), device(forDevice), pcap(PCAPDLL::getInstance()) {
+PCAP::PCAP(const device_eventhandler_t& err, neodevice_t& forDevice) : Driver(err), device(forDevice), pcap(PCAPDLL::getInstance()), ethPacketizer(err) {
 	if(IsHandleValid(device.handle)) {
 		interface = knownInterfaces[(device.handle >> 24) & 0xFF];
 		interface.fp = nullptr; // We're going to open our own connection to the interface. This should already be nullptr but just in case.
@@ -191,6 +191,8 @@ PCAP::PCAP(const device_eventhandler_t& err, neodevice_t& forDevice) : Driver(er
 		deviceMAC[3] = (device.handle >> 16) & 0xFF;
 		deviceMAC[4] = (device.handle >> 8) & 0xFF;
 		deviceMAC[5] = device.handle & 0xFF;
+		memcpy(ethPacketizer.deviceMAC, deviceMAC, 6);
+		memcpy(ethPacketizer.hostMAC, interface.macAddress, 6);
 	} else {
 		openable = false;
 	}
@@ -213,7 +215,7 @@ bool PCAP::open() {
 	}	
 
 	// Open the interface
-	interface.fp = pcap.open(interface.nameFromWinPCAP.c_str(), 100, PCAP_OPENFLAG_PROMISCUOUS | PCAP_OPENFLAG_MAX_RESPONSIVENESS, 1, nullptr, errbuf);
+	interface.fp = pcap.open(interface.nameFromWinPCAP.c_str(), 65536, PCAP_OPENFLAG_MAX_RESPONSIVENESS | PCAP_OPENFLAG_NOCAPTURE_LOCAL, 50, nullptr, errbuf);
 	if(interface.fp == nullptr) {
 		report(APIEvent::Type::DriverFailedToOpen, APIEvent::Severity::Error);
 		return false;
@@ -222,6 +224,7 @@ bool PCAP::open() {
 	// Create threads
 	readThread = std::thread(&PCAP::readTask, this);
 	writeThread = std::thread(&PCAP::writeTask, this);
+	transmitThread = std::thread(&PCAP::transmitTask, this);
 	
 	return true;
 }
@@ -239,6 +242,7 @@ bool PCAP::close() {
 	closing = true; // Signal the threads that we are closing
 	readThread.join();
 	writeThread.join();
+	transmitThread.join();
 	closing = false;
 
 	pcap.close(interface.fp);
@@ -248,6 +252,7 @@ bool PCAP::close() {
 	WriteOperation flushop;
 	while(readQueue.try_dequeue(flush)) {}
 	while(writeQueue.try_dequeue(flushop)) {}
+	transmitQueue = nullptr;
 
 	return true;
 }
@@ -265,108 +270,75 @@ void PCAP::readTask() {
 		if(readBytes == 0)
 			continue; // Keep waiting for that packet
 
-		EthernetPacket packet(data, header->caplen);
+		if(ethPacketizer.inputUp({data, data + header->caplen})) {
-
+			const auto bytes = ethPacketizer.outputUp();
-		if(packet.etherType != 0xCAB2)
+			readQueue.enqueue_bulk(bytes.data(), bytes.size());
-			continue; // Not a packet to host
+		}
-
-		if(memcmp(packet.destMAC, interface.macAddress, sizeof(packet.destMAC)) != 0 &&
-			memcmp(packet.destMAC, BROADCAST_MAC, sizeof(packet.destMAC)) != 0 &&
-			memcmp(packet.destMAC, ICS_UNSET_MAC, sizeof(packet.destMAC)) != 0)
-			continue; // Packet is not addressed to us or broadcast
-
-		if(memcmp(packet.srcMAC, deviceMAC, sizeof(deviceMAC)) != 0)
-			continue; // Not a packet from the device we're concerned with
-
-		readQueue.enqueue_bulk(packet.payload.data(), packet.payload.size());
 	}
 }
 
 void PCAP::writeTask() {
 	WriteOperation writeOp;
-	uint16_t sequence = 0;
-	EthernetPacket sendPacket;
 	EventManager::GetInstance().downgradeErrorsOnCurrentThread();
-	
+
-	// Set MAC address of packet
+	pcap_send_queue* queue1 = pcap.sendqueue_alloc(128000);
-	memcpy(sendPacket.srcMAC, interface.macAddress, sizeof(sendPacket.srcMAC));
+	pcap_send_queue* queue2 = pcap.sendqueue_alloc(128000);
-	memcpy(sendPacket.destMAC, deviceMAC, sizeof(deviceMAC));
+	pcap_send_queue* queue = queue1;
+	std::vector<uint8_t> extraData;
 
 	while(!closing) {
 		if(!writeQueue.wait_dequeue_timed(writeOp, std::chrono::milliseconds(100)))
 			continue;
 
-		sendPacket.packetNumber = sequence++;
+		unsigned int i = 0;
-		sendPacket.payload = std::move(writeOp.bytes);
+		do {
-		auto bs = sendPacket.getBytestream();
+			ethPacketizer.inputDown(std::move(writeOp.bytes));
-		if(!closing)
+		} while(writeQueue.try_dequeue(writeOp) && i++ < (queue->maxlen - queue->len) / 1518 / 3);
-			pcap.sendpacket(interface.fp, bs.data(), (int)bs.size());
+
-		// TODO Handle packet send errors
+		for(const auto& data : ethPacketizer.outputDown()) {
+			pcap_pkthdr header = {};
+			header.caplen = header.len = bpf_u_int32(data.size());
+			if(pcap.sendqueue_queue(queue, &header, data.data()) == -1)
+				report(APIEvent::Type::FailedToWrite, APIEvent::Severity::EventWarning);
+		}
+		
+		std::unique_lock<std::mutex> lk(transmitQueueMutex);
+		if(!transmitQueue || queue->len + (1518*2) >= queue->maxlen) { // Checking if we want to swap sendqueues with the transmitTask
+			if(transmitQueue) // Need to wait for the queue to become available
+				transmitQueueCV.wait(lk, [this] { return !transmitQueue; });
+			
+			// Time to swap
+			transmitQueue = queue;
+			lk.unlock();
+			transmitQueueCV.notify_one();
+
+			// Set up our next queue
+			if(queue == queue1) {
+				pcap.sendqueue_destroy(queue2);
+				queue = queue2 = pcap.sendqueue_alloc(128000);
+			} else {
+				pcap.sendqueue_destroy(queue1);
+				queue = queue1 = pcap.sendqueue_alloc(128000);
+			}
+		}
+	}
+
+	pcap.sendqueue_destroy(queue1);
+	pcap.sendqueue_destroy(queue2);
+}
+
+void PCAP::transmitTask() {
+	while(!closing) {
+		std::unique_lock<std::mutex> lk(transmitQueueMutex);
+		if(transmitQueueCV.wait_for(lk, std::chrono::milliseconds(100), [this] { return !!transmitQueue; }) && !closing && transmitQueue) {
+			pcap_send_queue* current = transmitQueue;
+			lk.unlock();
+			pcap.sendqueue_transmit(interface.fp, current, 0);
+			{
+				std::lock_guard<std::mutex> lk2(transmitQueueMutex);
+				transmitQueue = nullptr;
+			}
+			transmitQueueCV.notify_one();
+		}
 	}
 }
-
-PCAP::EthernetPacket::EthernetPacket(const std::vector<uint8_t>& bytestream) {
-	loadBytestream(bytestream);
-}
-
-PCAP::EthernetPacket::EthernetPacket(const uint8_t* data, size_t size) {
-	std::vector<uint8_t> bs(size);
-	for(size_t i = 0; i < size; i++)
-		bs[i] = data[i];
-	loadBytestream(bs);
-}
-
-int PCAP::EthernetPacket::loadBytestream(const std::vector<uint8_t>& bytestream) {
-	errorWhileDecodingFromBytestream = 0;
-	for(size_t i = 0; i < 6; i++)
-		destMAC[i] = bytestream[i];
-	for(size_t i = 0; i < 6; i++)
-		srcMAC[i] = bytestream[i + 6];
-	etherType = (bytestream[12] << 8) | bytestream[13];
-	icsEthernetHeader = (bytestream[14] << 24) | (bytestream[15] << 16) | (bytestream[16] << 8) | bytestream[17];
-	uint16_t payloadSize = bytestream[18] | (bytestream[19] << 8);
-	packetNumber = bytestream[20] | (bytestream[21] << 8);
-	uint16_t packetInfo = bytestream[22] | (bytestream[23] << 8);
-	firstPiece = packetInfo & 1;
-	lastPiece = (packetInfo >> 1) & 1;
-	bufferHalfFull = (packetInfo >> 2) & 2;
-	payload = std::vector<uint8_t>(bytestream.begin() + 24, bytestream.end());
-	size_t payloadActualSize = payload.size();
-	if(payloadActualSize < payloadSize)
-		errorWhileDecodingFromBytestream = 1;
-	payload.resize(payloadSize);
-	return errorWhileDecodingFromBytestream;
-}
-
-std::vector<uint8_t> PCAP::EthernetPacket::getBytestream() const {
-	size_t payloadSize = payload.size();
-	std::vector<uint8_t> bytestream;
-	bytestream.reserve(6 + 6 + 2 + 4 + 2 + 2 + 2 + payloadSize);
-	for(size_t i = 0; i < 6; i++)
-		bytestream.push_back(destMAC[i]);
-	for(size_t i = 0; i < 6; i++)
-		bytestream.push_back(srcMAC[i]);
-	// EtherType should be put into the bytestream as big endian
-	bytestream.push_back((uint8_t)(etherType >> 8));
-	bytestream.push_back((uint8_t)(etherType));
-	// Our Ethernet header should be put into the bytestream as big endian
-	bytestream.push_back((uint8_t)(icsEthernetHeader >> 24));
-	bytestream.push_back((uint8_t)(icsEthernetHeader >> 16));
-	bytestream.push_back((uint8_t)(icsEthernetHeader >> 8));
-	bytestream.push_back((uint8_t)(icsEthernetHeader));
-	// The payload size comes next, it's little endian
-	bytestream.push_back((uint8_t)(payloadSize));
-	bytestream.push_back((uint8_t)(payloadSize >> 8));
-	// Packet number is little endian
-	bytestream.push_back((uint8_t)(packetNumber));
-	bytestream.push_back((uint8_t)(packetNumber >> 8));
-	// Packet info gets assembled into a bitfield
-	uint16_t packetInfo = 0;
-	packetInfo |= firstPiece & 1;
-	packetInfo |= (lastPiece & 1) << 1;
-	packetInfo |= (bufferHalfFull & 1) << 2;
-	bytestream.push_back((uint8_t)(packetInfo));
-	bytestream.push_back((uint8_t)(packetInfo >> 8));
-	bytestream.insert(bytestream.end(), payload.begin(), payload.end());
-	return bytestream;
-}