POSIX: PCAP: Use EthernetPacketizer

include/icsneo/platform/posix/pcap.h

@@ -5,6 +5,7 @@
 
 #include "icsneo/device/neodevice.h"
 #include "icsneo/communication/driver.h"
+#include "icsneo/communication/ethernetpacketizer.h"
 #include "icsneo/api/eventmanager.h"
 #include <string>
 #include <pcap.h>
@@ -32,6 +33,7 @@ private:
 	neodevice_t& device;
 	uint8_t deviceMAC[6];
 	bool openable = true;
+	EthernetPacketizer ethPacketizer;
 	void readTask();
 	void writeTask();
 
@@ -47,27 +49,6 @@ private:
 	};
 	static std::vector<NetworkInterface> knownInterfaces;
 	NetworkInterface iface;
-
-	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/posix/pcap.cpp

@@ -124,7 +124,7 @@ std::vector<PCAP::PCAPFoundDevice> PCAP::FindAll() {
 
 		pcap_setnonblock(iface.fp, 1, errbuf);
 
-		EthernetPacket requestPacket;
+		EthernetPacketizer::EthernetPacket requestPacket;
 		memcpy(requestPacket.srcMAC, iface.macAddress, sizeof(requestPacket.srcMAC));
 		requestPacket.payload.reserve(4);
 		requestPacket.payload = {
@@ -153,7 +153,7 @@ 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, iface.macAddress, sizeof(packet.destMAC)) == 0 ||
@@ -201,7 +201,7 @@ bool PCAP::IsHandleValid(neodevice_handle_t handle) {
 	return (netifIndex < knownInterfaces.size());
 }
 
-PCAP::PCAP(device_eventhandler_t err, neodevice_t& forDevice) : Driver(err), device(forDevice) {
+PCAP::PCAP(device_eventhandler_t err, neodevice_t& forDevice) : Driver(err), device(forDevice), ethPacketizer(err) {
 	if(IsHandleValid(device.handle)) {
 		iface = knownInterfaces[(device.handle >> 24) & 0xFF];
 		iface.fp = nullptr; // We're going to open our own connection to the interface. This should already be nullptr but just in case.
@@ -212,6 +212,8 @@ PCAP::PCAP(device_eventhandler_t err, neodevice_t& forDevice) : Driver(err), dev
 		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, iface.macAddress, 6);
 	} else {
 		openable = false;
 	}
@@ -277,110 +279,36 @@ void PCAP::readTask() {
 	EventManager::GetInstance().downgradeErrorsOnCurrentThread();
 	while (!closing) {
 		pcap_dispatch(iface.fp, -1, [](uint8_t* obj, const struct pcap_pkthdr* header, const uint8_t* data) {
-			PCAP* driver = (PCAP*)obj;
+			PCAP* driver = reinterpret_cast<PCAP*>(obj);
-			EthernetPacket packet(data, header->caplen);
+			if(driver->ethPacketizer.inputUp({data, data + header->caplen})) {
-
+				const auto bytes = driver->ethPacketizer.outputUp();
-			if(packet.etherType != 0xCAB2)
+				driver->readQueue.enqueue_bulk(bytes.data(), bytes.size());
-				return; // Not a packet to host
+			}
-
-			if(memcmp(packet.destMAC, driver->iface.macAddress, sizeof(packet.destMAC)) != 0 &&
-				memcmp(packet.destMAC, BROADCAST_MAC, sizeof(packet.destMAC)) != 0 &&
-				memcmp(packet.destMAC, ICS_UNSET_MAC, sizeof(packet.destMAC)) != 0)
-				return; // Packet is not addressed to us or broadcast
-
-			if(memcmp(packet.srcMAC, driver->deviceMAC, sizeof(driver->deviceMAC)) != 0)
-				return; // Not a packet from the device we're concerned with
-
-			driver->readQueue.enqueue_bulk(packet.payload.data(), packet.payload.size());
 		}, (uint8_t*)this);
 	}
 }
 
 void PCAP::writeTask() {
 	WriteOperation writeOp;
-	uint16_t sequence = 0;
-	EthernetPacket sendPacket;
 	EventManager::GetInstance().downgradeErrorsOnCurrentThread();
-	
-	// Set MAC address of packet
-	memcpy(sendPacket.srcMAC, iface.macAddress, sizeof(sendPacket.srcMAC));
-	memcpy(sendPacket.destMAC, deviceMAC, sizeof(deviceMAC));
 
 	while(!closing) {
 		if(!writeQueue.wait_dequeue_timed(writeOp, std::chrono::milliseconds(100)))
 			continue;
 
-		sendPacket.packetNumber = sequence++;
+		// If we have a bunch of small packets to send, try to pack them into a packet
-		sendPacket.payload = std::move(writeOp.bytes);
+		// We use the average packet size to determine if we're likely to have enough room
-		auto bs = sendPacket.getBytestream();
+		size_t bytesPushed = 0;
-		if(!closing)
+		size_t packetsPushed = 0;
-			pcap_sendpacket(iface.fp, bs.data(), (int)bs.size());
+		do {
+			packetsPushed++;
+			bytesPushed += writeOp.bytes.size();
+			ethPacketizer.inputDown(std::move(writeOp.bytes));
+		} while(bytesPushed < (EthernetPacketizer::MaxPacketLength - (bytesPushed / packetsPushed * 2)) && writeQueue.try_dequeue(writeOp));
+
+		for(const auto& packet : ethPacketizer.outputDown()) {
+			pcap_sendpacket(iface.fp, packet.data(), (int)packet.size());
+		}
 		// TODO Handle packet send errors
 	}
 }
-
-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;
-}