From 424d3d98a381de87ad1b73209bbcaa96982b6d33 Mon Sep 17 00:00:00 2001 From: Paul Hollinsky Date: Mon, 18 Mar 2019 11:37:20 -0400 Subject: [PATCH] Linux Ethernet device support --- CMakeLists.txt | 2 + cmake/FindPCAP.cmake | 74 +++++ include/icsneo/device/radpluto/radpluto.h | 101 +++++++ include/icsneo/platform/pcap.h | 4 +- include/icsneo/platform/posix/devices.h | 7 +- include/icsneo/platform/posix/pcap.h | 74 +++++ include/icsneo/platform/windows/devices.h | 1 + platform/posix/pcap.cpp | 347 ++++++++++++++++++++++ 8 files changed, 605 insertions(+), 5 deletions(-) create mode 100644 cmake/FindPCAP.cmake create mode 100644 include/icsneo/device/radpluto/radpluto.h create mode 100644 include/icsneo/platform/posix/pcap.h create mode 100644 platform/posix/pcap.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4621fbd..a677890 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -188,6 +188,8 @@ if(NOT WIN32) set_property(TARGET ftdi1-static PROPERTY POSITION_INDEPENDENT_CODE ON) target_link_libraries(icsneocpp PUBLIC ftdi1-static) target_link_libraries(icsneocpp PUBLIC ${CMAKE_THREAD_LIBS_INIT}) + find_package(PCAP REQUIRED) + target_link_libraries(icsneocpp PUBLIC ${PCAP_LIBRARY}) endif() set(CPACK_PROJECT_NAME ${PROJECT_NAME}) diff --git a/cmake/FindPCAP.cmake b/cmake/FindPCAP.cmake new file mode 100644 index 0000000..90fab58 --- /dev/null +++ b/cmake/FindPCAP.cmake @@ -0,0 +1,74 @@ +# - Try to find libpcap include dirs and libraries +# +# Usage of this module as follows: +# +# find_package(PCAP) +# +# Variables used by this module, they can change the default behaviour and need +# to be set before calling find_package: +# +# PCAP_ROOT_DIR Set this variable to the root installation of +# libpcap if the module has problems finding the +# proper installation path. +# +# Variables defined by this module: +# +# PCAP_FOUND System has libpcap, include and library dirs found +# PCAP_INCLUDE_DIR The libpcap include directories. +# PCAP_LIBRARY The libpcap library (possibly includes a thread +# library e.g. required by pf_ring's libpcap) +# HAVE_PF_RING If a found version of libpcap supports PF_RING + +find_path(PCAP_ROOT_DIR + NAMES include/pcap.h +) + +find_path(PCAP_INCLUDE_DIR + NAMES pcap.h + HINTS ${PCAP_ROOT_DIR}/include +) + +find_library(PCAP_LIBRARY + NAMES pcap + HINTS ${PCAP_ROOT_DIR}/lib +) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(PCAP DEFAULT_MSG + PCAP_LIBRARY + PCAP_INCLUDE_DIR +) + +include(CheckCSourceCompiles) +set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY}) +check_c_source_compiles("int main() { return 0; }" PCAP_LINKS_SOLO) +set(CMAKE_REQUIRED_LIBRARIES) + +# check if linking against libpcap also needs to link against a thread library +if (NOT PCAP_LINKS_SOLO) + find_package(Threads) + if (THREADS_FOUND) + set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY} ${CMAKE_THREAD_LIBS_INIT}) + check_c_source_compiles("int main() { return 0; }" PCAP_NEEDS_THREADS) + set(CMAKE_REQUIRED_LIBRARIES) + endif () + if (THREADS_FOUND AND PCAP_NEEDS_THREADS) + set(_tmp ${PCAP_LIBRARY} ${CMAKE_THREAD_LIBS_INIT}) + list(REMOVE_DUPLICATES _tmp) + set(PCAP_LIBRARY ${_tmp} + CACHE STRING "Libraries needed to link against libpcap" FORCE) + else () + message(FATAL_ERROR "Couldn't determine how to link against libpcap") + endif () +endif () + +include(CheckFunctionExists) +set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY}) +check_function_exists(pcap_get_pfring_id HAVE_PF_RING) +set(CMAKE_REQUIRED_LIBRARIES) + +mark_as_advanced( + PCAP_ROOT_DIR + PCAP_INCLUDE_DIR + PCAP_LIBRARY +) diff --git a/include/icsneo/device/radpluto/radpluto.h b/include/icsneo/device/radpluto/radpluto.h new file mode 100644 index 0000000..618d1a2 --- /dev/null +++ b/include/icsneo/device/radpluto/radpluto.h @@ -0,0 +1,101 @@ +#ifndef __RADPLUTO_H_ +#define __RADPLUTO_H_ + +#include "icsneo/device/device.h" +#include "icsneo/device/devicetype.h" +#include "icsneo/platform/pcap.h" +#include "icsneo/communication/packetizer.h" +#include "icsneo/communication/decoder.h" + +namespace icsneo { + +class RADPluto : public Device { +public: + // Serial numbers start with PL + static constexpr DeviceType::Enum DEVICE_TYPE = DeviceType::RADPluto; + static constexpr const uint16_t PRODUCT_ID = 0xffff; // Not yet set + static constexpr const char* SERIAL_START = "PL"; + static std::vector> Find() { + std::vector> found; + + for(auto& foundDev : PCAP::FindAll()) { + auto fakedev = std::shared_ptr(new RADPluto({})); + for(auto& payload : foundDev.discoveryPackets) + fakedev->com->packetizer->input(payload); + for(auto& packet : fakedev->com->packetizer->output()) { + std::shared_ptr msg; + if(!fakedev->com->decoder->decode(msg, packet)) + continue; // We failed to decode this packet + + if(!msg || msg->network.getNetID() != Network::NetID::Main51) + continue; // Not a message we care about + auto sn = std::dynamic_pointer_cast(msg); + if(!sn) + continue; // Not a serial number message + + if(sn->deviceSerial.length() < 2) + continue; + if(sn->deviceSerial.substr(0, 2) != SERIAL_START) + continue; // Not a RADPluto + + foundDev.device.serial[sn->deviceSerial.copy(foundDev.device.serial, sizeof(foundDev.device.serial))] = '\0'; + found.emplace_back(new RADPluto(foundDev.device)); + break; + } + } + + return found; + } + + static const std::vector& GetSupportedNetworks() { + static std::vector supportedNetworks = { + Network::NetID::HSCAN, + Network::NetID::HSCAN2, + + Network::NetID::LIN, + + Network::NetID::Ethernet, + + Network::NetID::OP_Ethernet1, + Network::NetID::OP_Ethernet2, + Network::NetID::OP_Ethernet3, + Network::NetID::OP_Ethernet4 + }; + return supportedNetworks; + } + + RADPluto(neodevice_t neodevice) : Device(neodevice) { + initialize(); + getWritableNeoDevice().type = DEVICE_TYPE; + productId = PRODUCT_ID; + } + +protected: + void setupPacketizer(Packetizer& packetizer) override { + Device::setupPacketizer(packetizer); + packetizer.disableChecksum = true; + packetizer.align16bit = false; + } + + virtual void setupEncoder(Encoder& encoder) override { + Device::setupEncoder(encoder); + encoder.supportCANFD = true; + } + + virtual void setupDecoder(Decoder& decoder) override { + Device::setupDecoder(decoder); + decoder.timestampMultiplier = 10; // Timestamps are in 10ns increments instead of the usual 25ns + } + + virtual void setupSupportedRXNetworks(std::vector& rxNetworks) override { + for(auto& netid : GetSupportedNetworks()) + rxNetworks.emplace_back(netid); + } + + // The supported TX networks are the same as the supported RX networks for this device + virtual void setupSupportedTXNetworks(std::vector& txNetworks) override { setupSupportedRXNetworks(txNetworks); } +}; + +} + +#endif \ No newline at end of file diff --git a/include/icsneo/platform/pcap.h b/include/icsneo/platform/pcap.h index 192c212..e97b5f4 100644 --- a/include/icsneo/platform/pcap.h +++ b/include/icsneo/platform/pcap.h @@ -3,8 +3,8 @@ #if defined _WIN32 #include "icsneo/platform/windows/pcap.h" -// #elif defined (__unix__) || (defined (__APPLE__) && defined (__MACH__)) -// #include "icsneo/platform/posix/ftdi.h" +#elif defined (__unix__) || (defined (__APPLE__) && defined (__MACH__)) +#include "icsneo/platform/posix/pcap.h" #else #warning "This platform is not supported by the PCAP driver" #endif diff --git a/include/icsneo/platform/posix/devices.h b/include/icsneo/platform/posix/devices.h index c8e2570..335199c 100644 --- a/include/icsneo/platform/posix/devices.h +++ b/include/icsneo/platform/posix/devices.h @@ -4,12 +4,13 @@ #include "icsneo/device/neoobd2pro/neoobd2pro.h" #include "icsneo/device/neoobd2sim/neoobd2sim.h" #include "icsneo/device/neovifire/neovifire.h" -//#include "icsneo/device/neovifire2/neovifire2eth.h" Ethernet not yet supported +#include "icsneo/device/neovifire2/neovifire2eth.h" #include "icsneo/device/neovifire2/neovifire2usb.h" #include "icsneo/device/plasion/neoviion.h" #include "icsneo/device/plasion/neoviplasma.h" -//#include "icsneo/device/radgalaxy/radgalaxy.h" Ethernet not yet supported -//#include "icsneo/device/radstar2/radstar2eth.h" Ethernet not yet supported +#include "icsneo/device/radgalaxy/radgalaxy.h" +#include "icsneo/device/radpluto/radpluto.h" +#include "icsneo/device/radstar2/radstar2eth.h" #include "icsneo/device/radstar2/radstar2usb.h" #include "icsneo/device/radsupermoon/radsupermoon.h" #include "icsneo/device/valuecan3/valuecan3.h" diff --git a/include/icsneo/platform/posix/pcap.h b/include/icsneo/platform/posix/pcap.h new file mode 100644 index 0000000..4f1b094 --- /dev/null +++ b/include/icsneo/platform/posix/pcap.h @@ -0,0 +1,74 @@ +#ifndef __PCAP_POSIX_H_ +#define __PCAP_POSIX_H_ + +#include "icsneo/device/neodevice.h" +#include "icsneo/communication/icommunication.h" +#include "icsneo/api/errormanager.h" +#include +#include + +namespace icsneo { + +class PCAP : public ICommunication { +public: + class PCAPFoundDevice { + public: + neodevice_t device; + std::vector> discoveryPackets; + }; + + static std::vector FindAll(); + static std::string GetEthDevSerialFromMacAddress(uint8_t product, uint16_t macSerial); + static bool IsHandleValid(neodevice_handle_t handle); + + PCAP(device_errorhandler_t err, neodevice_t& forDevice); + bool open(); + bool isOpen(); + bool close(); +private: + device_errorhandler_t err; + char errbuf[PCAP_ERRBUF_SIZE] = { 0 }; + neodevice_t& device; + uint8_t deviceMAC[6]; + bool openable = true; + void readTask(); + void writeTask(); + + class NetworkInterface { + public: + uint8_t uuid; + uint8_t macAddress[8]; + std::string nameFromWinPCAP; + std::string descriptionFromWinPCAP; + std::string fullName; + pcap_t* fp = nullptr; + pcap_stat stats; + }; + 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; + }; +}; + +} + +#endif \ No newline at end of file diff --git a/include/icsneo/platform/windows/devices.h b/include/icsneo/platform/windows/devices.h index f44e6b7..34e460f 100644 --- a/include/icsneo/platform/windows/devices.h +++ b/include/icsneo/platform/windows/devices.h @@ -9,6 +9,7 @@ #include "icsneo/device/plasion/neoviion.h" #include "icsneo/device/plasion/neoviplasma.h" #include "icsneo/device/radgalaxy/radgalaxy.h" +#include "icsneo/device/radgalaxy/radpluto.h" #include "icsneo/device/radstar2/radstar2eth.h" #include "icsneo/device/radstar2/radstar2usb.h" #include "icsneo/device/radsupermoon/radsupermoon.h" diff --git a/platform/posix/pcap.cpp b/platform/posix/pcap.cpp new file mode 100644 index 0000000..5bd76af --- /dev/null +++ b/platform/posix/pcap.cpp @@ -0,0 +1,347 @@ +#include "icsneo/platform/posix/pcap.h" +#include "icsneo/communication/network.h" +#include "icsneo/communication/communication.h" +#include "icsneo/communication/packetizer.h" +#include +#include +#include +#include +#include +#include +#include + +using namespace icsneo; + +static const uint8_t BROADCAST_MAC[6] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; + +std::vector PCAP::knownInterfaces; + +std::vector PCAP::FindAll() { + std::vector foundDevices; + + // First we ask WinPCAP to give us all of the devices + pcap_if_t* alldevs; + char errbuf[PCAP_ERRBUF_SIZE] = { 0 }; + bool success = false; + // Calling pcap_findalldevs too quickly can cause various errors. Retry a few times in this case. + for(auto retry = 0; retry < 10; retry++) { + auto ret = pcap_findalldevs(&alldevs, errbuf); + if(ret == 0) { + success = true; + break; + } + } + + if(!success) { + ErrorManager::GetInstance().add(APIError::PCAPCouldNotFindDevices); + return std::vector(); + } + + std::vector interfaces; + for(pcap_if_t* dev = alldevs; dev != nullptr; dev = dev->next) { + if(dev->name == nullptr) + continue; + if(dev->addresses == nullptr) { + //std::cout << dev->name << " has no addresses" << std::endl; + continue; + } + NetworkInterface netif; + netif.nameFromWinPCAP = dev->name; + if(dev->description) + netif.descriptionFromWinPCAP = dev->description; + pcap_addr* currentAddress = dev->addresses; + bool hasAddress = false; + while(!hasAddress && currentAddress != nullptr) { + if(currentAddress->addr && currentAddress->addr->sa_family == AF_PACKET) { + struct sockaddr_ll* s = (struct sockaddr_ll*)currentAddress->addr; + memcpy(netif.macAddress, s->sll_addr, sizeof(netif.macAddress)); + hasAddress = true; + break; + } + currentAddress = currentAddress->next; + } + + if(!hasAddress) + continue; + + interfaces.push_back(netif); + } + + pcap_freealldevs(alldevs); + + for(auto& interface : interfaces) { + bool exists = false; + for(auto& known : knownInterfaces) + if(memcmp(interface.macAddress, known.macAddress, sizeof(interface.macAddress)) == 0) + exists = true; + if(!exists) + knownInterfaces.emplace_back(interface); + } + + 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 + + errbuf[0] = '\0'; + interface.fp = pcap_open_live(interface.nameFromWinPCAP.c_str(), UINT16_MAX, 1, 0, errbuf); + if(strlen(errbuf) != 0) { // This means a warning + std::cout << "Warning for " << interface.nameFromWinPCAP << " " << errbuf << std::endl; + } + + if(interface.fp == nullptr) { + std::cout << "pcap_open_live failed for " << interface.nameFromWinPCAP << " with " << errbuf << std::endl; + continue; // Could not open the interface + } + + pcap_setnonblock(interface.fp, 1, errbuf); + + EthernetPacket requestPacket; + requestPacket.payload.reserve(4); + requestPacket.payload = { + ((1 << 4) | (uint8_t)Network::NetID::Main51), // Packet size of 1 on NETID_MAIN51 + (uint8_t)Command::RequestSerialNumber + }; + requestPacket.payload.push_back(Packetizer::ICSChecksum(requestPacket.payload)); + requestPacket.payload.insert(requestPacket.payload.begin(), 0xAA); + + auto bs = requestPacket.getBytestream(); + pcap_sendpacket(interface.fp, bs.data(), (int)bs.size()); + + auto timeout = std::chrono::high_resolution_clock::now() + std::chrono::milliseconds(50); + while(std::chrono::high_resolution_clock::now() <= timeout) { // Wait up to 5ms for the response + struct pcap_pkthdr* header; + const uint8_t* data; + auto res = pcap_next_ex(interface.fp, &header, &data); + if(res < 0) { + std::cout << "pcapnextex failed with " << res << std::endl; + break; + } + if(res == 0) + continue; // Keep waiting for that packet + + 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 + )) { + /* 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 + * or not, we just know we got something. + * + * Unlike most transport layers, we can't get the serial number here as we actually + * need to parse this message that has been returned. Some devices parse messages + * differently, so we need to use their communication layer. We could technically + * create a communication layer to parse the packet we have in `payload` here, but + * we'd need to be given a packetizer and decoder for the device. I'm intentionally + * avoiding passing that information down here for code quality's sake. Instead, pass + * the packet we received back up so the device can handle it. + */ + neodevice_handle_t handle = (neodevice_handle_t)((i << 24) | (packet.srcMAC[3] << 16) | (packet.srcMAC[4] << 8) | (packet.srcMAC[5])); + PCAPFoundDevice* alreadyExists = nullptr; + for(auto& dev : foundDevices) + if(dev.device.handle == handle) + alreadyExists = &dev; + + if(alreadyExists == nullptr) { + PCAPFoundDevice foundDevice; + foundDevice.device.handle = handle; + foundDevice.discoveryPackets.push_back(std::move(packet.payload)); + foundDevices.push_back(foundDevice); + } else { + alreadyExists->discoveryPackets.push_back(std::move(packet.payload)); + } + } + } + + pcap_close(interface.fp); + interface.fp = nullptr; + } + + return foundDevices; +} + +bool PCAP::IsHandleValid(neodevice_handle_t handle) { + uint8_t netifIndex = (uint8_t)(handle >> 24); + return (netifIndex < knownInterfaces.size()); +} + +PCAP::PCAP(device_errorhandler_t err, neodevice_t& forDevice) : err(err), device(forDevice) { + 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. + + deviceMAC[0] = 0x00; + deviceMAC[1] = 0xFC; + deviceMAC[2] = 0x70; + deviceMAC[3] = (device.handle >> 16) & 0xFF; + deviceMAC[4] = (device.handle >> 8) & 0xFF; + deviceMAC[5] = device.handle & 0xFF; + } else { + openable = false; + } +} + +bool PCAP::open() { + if(!openable) + return false; + + if(isOpen()) + return false; + + // Open the interface + interface.fp = pcap_open_live(interface.nameFromWinPCAP.c_str(), INT16_MAX, 1, 0, errbuf); + if(interface.fp == nullptr) { + err(APIError::DriverFailedToOpen); + return false; + } + + pcap_setnonblock(interface.fp, 1, errbuf); + + // Create threads + readThread = std::thread(&PCAP::readTask, this); + writeThread = std::thread(&PCAP::writeTask, this); + + return true; +} + +bool PCAP::isOpen() { + return interface.fp != nullptr; +} + +bool PCAP::close() { + if(!isOpen()) + return false; + + closing = true; // Signal the threads that we are closing + readThread.join(); + writeThread.join(); + closing = false; + + pcap_close(interface.fp); + interface.fp = nullptr; + + uint8_t flush; + WriteOperation flushop; + while(readQueue.try_dequeue(flush)) {} + while(writeQueue.try_dequeue(flushop)) {} + + return true; +} + +void PCAP::readTask() { + struct pcap_pkthdr* header; + const uint8_t* data; + while(!closing) { + auto readBytes = pcap_next_ex(interface.fp, &header, &data); + if(readBytes < 0) { + err(APIError::FailedToRead); + break; + } + 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) + 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()); + } +} + +void PCAP::writeTask() { + WriteOperation writeOp; + uint16_t sequence = 0; + EthernetPacket sendPacket; + + // Set MAC address of packet + memcpy(sendPacket.srcMAC, interface.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(interface.fp, bs.data(), (int)bs.size()); + // TODO Handle packet send errors + } +} + +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