sven
/
libicsneo
mirror of https://github.com/intrepidcs/libicsneo.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

Windows: PCAP: Rework for Ethernet Packetizer and Performance

pull/32/head
Paul Hollinsky 2020-09-22 19:22:40 -04:00
parent 76619e2496
commit 6f0654c336
2 changed files with 81 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
include/icsneo/platform/windows/pcap.h
View File

@ -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;
};
};
}

158
platform/windows/pcap.cpp
View File

@ -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, ICS_UNSET_MAC, sizeof(packet.destMAC)) == 0
memcmp(packet.destMAC, BROADCAST_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(packet.etherType != 0xCAB2)
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());
if(ethPacketizer.inputUp({data, data + header->caplen})) {
const auto bytes = ethPacketizer.outputUp();
readQueue.enqueue_bulk(bytes.data(), bytes.size());
}
}
}
void PCAP::writeTask() {
WriteOperation writeOp;
uint16_t sequence = 0;
EthernetPacket sendPacket;
EventManager::GetInstance().downgradeErrorsOnCurrentThread();
// Set MAC address of packet
memcpy(sendPacket.srcMAC, interface.macAddress, sizeof(sendPacket.srcMAC));
memcpy(sendPacket.destMAC, deviceMAC, sizeof(deviceMAC));
pcap_send_queue* queue1 = pcap.sendqueue_alloc(128000);
pcap_send_queue* queue2 = pcap.sendqueue_alloc(128000);
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++;
sendPacket.payload = std::move(writeOp.bytes);
auto bs = sendPacket.getBytestream();
if(!closing)
pcap.sendpacket(interface.fp, bs.data(), (int)bs.size());
// TODO Handle packet send errors
unsigned int i = 0;
do {
ethPacketizer.inputDown(std::move(writeOp.bytes));
} while(writeQueue.try_dequeue(writeOp) && i++ < (queue->maxlen - queue->len) / 1518 / 3);
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::EthernetPacket::EthernetPacket(const std::vector<uint8_t>& bytestream) {
loadBytestream(bytestream);
pcap.sendqueue_destroy(queue1);
pcap.sendqueue_destroy(queue2);
}
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);
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();
}
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;
}