Communication: Create EthernetPacketizer

This code previously was separately maintained in each of the
PCAP driver layers.

While adding complexity for reassembly, I decided it was
time to pull it out into a common implementation.

As of this commit, the old implementations have not been
removed from the PCAP drivers yet.

CMakeLists.txt

@@ -124,6 +124,7 @@ set(SRC_FILES
 	communication/packet/ethernetpacket.cpp
 	communication/decoder.cpp
 	communication/encoder.cpp
+	communication/ethernetpacketizer.cpp
 	communication/packetizer.cpp
 	communication/multichannelcommunication.cpp
 	communication/communication.cpp

communication/ethernetpacketizer.cpp

@@ -0,0 +1,168 @@
+#include "icsneo/communication/ethernetpacketizer.h"
+#include <algorithm>
+#include <iterator>
+
+using namespace icsneo;
+
+#define MAX_PACKET_LEN (1490) // MTU - overhead
+
+static const uint8_t BROADCAST_MAC[6] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+
+void EthernetPacketizer::inputDown(std::vector<uint8_t> bytes) {
+	EthernetPacket sendPacket;
+	std::copy(std::begin(hostMAC), std::end(hostMAC), std::begin(sendPacket.srcMAC));
+	std::copy(std::begin(deviceMAC), std::end(deviceMAC), std::begin(sendPacket.destMAC));
+
+	sendPacket.packetNumber = sequenceDown++;
+	sendPacket.payload = std::move(bytes);
+
+	// Split packets larger than MTU
+	std::vector<uint8_t> extraData;
+	if(sendPacket.payload.size() > MAX_PACKET_LEN) {
+		extraData.insert(extraData.end(), sendPacket.payload.begin() + MAX_PACKET_LEN, sendPacket.payload.end());
+		sendPacket.payload.resize(MAX_PACKET_LEN);
+		sendPacket.lastPiece = false;
+	}
+
+	processedDownPackets.push_back(sendPacket.getBytestream());
+
+	if(!extraData.empty()) {
+		sendPacket.payload = std::move(extraData);
+		sendPacket.firstPiece = false;
+		sendPacket.lastPiece = true;
+		processedDownPackets.push_back(sendPacket.getBytestream());
+	}
+}
+
+std::vector< std::vector<uint8_t> > EthernetPacketizer::outputDown() {
+	std::vector< std::vector<uint8_t> > ret = std::move(processedDownPackets);
+	processedDownPackets.clear();
+	return ret;
+}
+
+bool EthernetPacketizer::inputUp(std::vector<uint8_t> bytes) {
+	EthernetPacket packet(bytes);
+
+	if(packet.errorWhileDecodingFromBytestream)
+		return false; // Bad packet
+
+	if(packet.etherType != 0xCAB2)
+		return false; // Not a packet to host
+
+	if(memcmp(packet.destMAC, hostMAC, sizeof(packet.destMAC)) != 0 &&
+		memcmp(packet.destMAC, BROADCAST_MAC, sizeof(packet.destMAC)) != 0)
+		return false; // Packet is not addressed to us or broadcast
+
+	if(memcmp(packet.srcMAC, deviceMAC, sizeof(deviceMAC)) != 0)
+		return false; // Not a packet from the device we're concerned with
+
+	// Handle single packets
+	if(packet.firstPiece && packet.lastPiece) {
+		// Could ensure no out-of-order reassembly by checking reassembing here,
+		// not doing that here because it should be harmless if it ever happened.
+		processedUpBytes.insert(processedUpBytes.end(), std::make_move_iterator(packet.payload.begin()), std::make_move_iterator(packet.payload.end()));
+		return true;
+	}
+
+	if(packet.firstPiece) {
+		if(reassembling) {
+			//report(APIEvent::Type::FailedToRead, APIEvent::Severity::EventWarning);
+			reassemblingData.clear();
+		}
+
+		reassembling = true;
+		reassemblingId = packet.packetNumber;
+		reassemblingData = std::move(bytes);
+		return !processedUpBytes.empty(); // If there are other packets in the pipe
+	}
+
+	if(!reassembling || reassemblingId != packet.packetNumber) {
+		//report(APIEvent::Type::FailedToRead, APIEvent::Severity::EventWarning);
+		reassembling = false;
+		reassemblingData.clear();
+		return !processedUpBytes.empty(); // If there are other packets in the pipe
+	}
+
+	if(packet.lastPiece) {
+		processedUpBytes.insert(processedUpBytes.end(), std::make_move_iterator(reassemblingData.begin()), std::make_move_iterator(reassemblingData.end()));
+		reassemblingData.clear();
+		reassembling = false;
+		processedUpBytes.insert(processedUpBytes.end(), std::make_move_iterator(packet.payload.begin()), std::make_move_iterator(packet.payload.end()));
+		return true;
+	}
+
+	reassemblingData.insert(reassemblingData.end(), std::make_move_iterator(packet.payload.begin()), std::make_move_iterator(packet.payload.end()));
+	return !processedUpBytes.empty(); // If there are other packets in the pipe
+}
+
+std::vector<uint8_t> EthernetPacketizer::outputUp() {
+	std::vector<uint8_t> ret = std::move(processedUpBytes);
+	processedUpBytes.clear();
+	return ret;
+}
+
+EthernetPacketizer::EthernetPacket::EthernetPacket(const std::vector<uint8_t>& bytestream) {
+	loadBytestream(bytestream);
+}
+
+EthernetPacketizer::EthernetPacket::EthernetPacket(const uint8_t* data, size_t size) {
+	std::vector<uint8_t> bs(data, data + size);
+	loadBytestream(bs);
+}
+
+int EthernetPacketizer::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;
+	else
+		payload.resize(payloadSize);
+	return errorWhileDecodingFromBytestream;
+}
+
+std::vector<uint8_t> EthernetPacketizer::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;
+	packetInfo |= 1 << 8; // Protocol version 1
+	bytestream.push_back((uint8_t)(packetInfo));
+	bytestream.push_back((uint8_t)(packetInfo >> 8));
+	bytestream.insert(bytestream.end(), payload.begin(), payload.end());
+	return bytestream;
+}

include/icsneo/communication/ethernetpacketizer.h

@@ -0,0 +1,80 @@
+#ifndef __ETHERNETPACKETIZER_H_
+#define __ETHERNETPACKETIZER_H_
+
+#ifdef __cplusplus
+
+#include "icsneo/api/eventmanager.h"
+#include <queue>
+#include <vector>
+#include <memory>
+
+namespace icsneo {
+
+/**
+ * A layer of encapsulation on top of the standard packetizer
+ * for 0xCAB1/0xCAB2 Ethernet packets.
+ * 
+ * Not thread-safe by default.
+ */
+class EthernetPacketizer {
+public:
+	EthernetPacketizer(device_eventhandler_t report) : report(report) {}
+
+	/**
+	 * Call with as many packets as desired before calling
+	 * outputDown to get the results. Passing in multiple
+	 * packets may result in better packing.
+	 */
+	void inputDown(std::vector<uint8_t> bytes);
+	std::vector< std::vector<uint8_t> > outputDown();
+
+	/**
+	 * Call with packet data, the packet may be queued waiting
+	 * for reassembly. In this case, false will be returned.
+	 */
+	bool inputUp(std::vector<uint8_t> bytes);
+	std::vector<uint8_t> outputUp();
+
+	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;
+	};
+
+	uint8_t hostMAC[6] = { 0x00, 0xFC, 0x70, 0xFF, 0xFF, 0xFF };
+	uint8_t deviceMAC[6] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+	
+private:
+	bool reassembling = false;
+	uint16_t reassemblingId = 0;
+	std::vector<uint8_t> reassemblingData;
+	uint16_t sequenceDown = 0;
+	uint16_t sequenceUp = 0;
+	bool gotUp = false; // Mark whether sequenceUp is actually valid
+
+	std::vector<uint8_t> processedUpBytes;
+	std::vector< std::vector<uint8_t> > processedDownPackets;
+
+	device_eventhandler_t report;
+};
+
+}
+
+#endif // __cplusplus
+
+#endif