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

POSIX: PCAP: Use EthernetPacketizer

pull/43/head
Paul Hollinsky 2021-09-23 21:31:59 -04:00
parent a1a544045b
commit 51626b9e63
2 changed files with 25 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
include/icsneo/platform/posix/pcap.h
View File

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

118
platform/posix/pcap.cpp
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;
}