diff --git a/include/icsneo/platform/windows/pcap.h b/include/icsneo/platform/windows/pcap.h index f897173..5fc0452 100644 --- a/include/icsneo/platform/windows/pcap.h +++ b/include/icsneo/platform/windows/pcap.h @@ -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 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 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& bytestream); - EthernetPacket(const uint8_t* data, size_t size); - int loadBytestream(const std::vector& bytestream); - std::vector 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 payload; - }; }; } diff --git a/platform/windows/pcap.cpp b/platform/windows/pcap.cpp index 51d20e6..31c149c 100644 --- a/platform/windows/pcap.cpp +++ b/platform/windows/pcap.cpp @@ -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 #include #pragma comment(lib, "IPHLPAPI.lib") @@ -96,18 +97,18 @@ std::vector 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::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 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 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.sendqueue_destroy(queue1); + pcap.sendqueue_destroy(queue2); +} + +void PCAP::transmitTask() { + while(!closing) { + std::unique_lock 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 lk2(transmitQueueMutex); + transmitQueue = nullptr; + } + transmitQueueCV.notify_one(); + } } } - -PCAP::EthernetPacket::EthernetPacket(const std::vector& bytestream) { - loadBytestream(bytestream); -} - -PCAP::EthernetPacket::EthernetPacket(const uint8_t* data, size_t size) { - std::vector bs(size); - for(size_t i = 0; i < size; i++) - bs[i] = data[i]; - loadBytestream(bs); -} - -int PCAP::EthernetPacket::loadBytestream(const std::vector& 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(bytestream.begin() + 24, bytestream.end()); - size_t payloadActualSize = payload.size(); - if(payloadActualSize < payloadSize) - errorWhileDecodingFromBytestream = 1; - payload.resize(payloadSize); - return errorWhileDecodingFromBytestream; -} - -std::vector PCAP::EthernetPacket::getBytestream() const { - size_t payloadSize = payload.size(); - std::vector 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; -} \ No newline at end of file