sven/libicsneo
1
0
Fork 0
mirror of https://github.com/intrepidcs/libicsneo.git synced 2026-08-05 01:18:36 +02:00
Code Issues Projects Releases Wiki Activity

POSIX: PCAP: Use EthernetPacketizer

Browse Source
This commit is contained in:
Paul Hollinsky
2021-09-23 21:31:59 -04:00
parent 1aa3828688
commit ac77fe2b56
2 changed files with 25 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platform/posix/pcap.cpp
+23 -95
View File
@@ -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;
EthernetPacket packet(data, header->caplen);
if(packet.etherType != 0xCAB2)
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());
PCAP* driver = reinterpret_cast<PCAP*>(obj);
if(driver->ethPacketizer.inputUp({data, data + header->caplen})) {
const auto bytes = driver->ethPacketizer.outputUp();
driver->readQueue.enqueue_bulk(bytes.data(), bytes.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++;
sendPacket.payload = std::move(writeOp.bytes);
auto bs = sendPacket.getBytestream();
if(!closing)
pcap_sendpacket(iface.fp, bs.data(), (int)bs.size());
// If we have a bunch of small packets to send, try to pack them into a packet
// We use the average packet size to determine if we're likely to have enough room
size_t bytesPushed = 0;
size_t packetsPushed = 0;
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;
}