mirror of
https://github.com/intrepidcs/libicsneo.git
synced 2026-08-05 01:18:36 +02:00
Add support for RADGalaxy and neoVI FIRE 2 over Ethernet
This commit is contained in:
Paul Hollinsky
@@ -0,0 +1,12 @@
|
||||
#ifndef __PCAP_H_
|
||||
#define __PCAP_H_
|
||||
|
||||
#if defined _WIN32
|
||||
#include "platform/windows/include/pcap.h"
|
||||
// #elif defined __linux__
|
||||
// #include "platform/linux/include/ftdi.h"
|
||||
#else
|
||||
#warning "This platform is not supported by the PCAP driver"
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,70 @@
|
||||
#ifndef __PCAP_WINDOWS_H_
|
||||
#define __PCAP_WINDOWS_H_
|
||||
|
||||
#include "platform/windows/internal/include/pcapdll.h"
|
||||
#include "device/include/neodevice.h"
|
||||
#include "communication/include/icommunication.h"
|
||||
#include <string>
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class PCAP : public ICommunication {
|
||||
public:
|
||||
static std::vector<neodevice_t> FindByProduct(int product);
|
||||
static std::string GetEthDevSerialFromMacAddress(uint8_t product, uint16_t macSerial);
|
||||
static bool IsHandleValid(neodevice_handle_t handle);
|
||||
|
||||
PCAP(neodevice_t& forDevice);
|
||||
bool open();
|
||||
bool isOpen();
|
||||
bool close();
|
||||
private:
|
||||
PCAPDLL pcap;
|
||||
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 nameFromWin32API;
|
||||
std::string descriptionFromWinPCAP;
|
||||
std::string descriptionFromWin32API;
|
||||
std::string friendlyNameFromWin32API;
|
||||
std::string fullName;
|
||||
pcap_t* fp = nullptr;
|
||||
pcap_stat stats;
|
||||
};
|
||||
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;
|
||||
};
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,53 @@
|
||||
#ifndef __PCAPDLL_WINDOWS_H_
|
||||
#define __PCAPDLL_WINDOWS_H_
|
||||
|
||||
#include <Windows.h>
|
||||
#include <winsock2.h>
|
||||
#include <pcap.h>
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
// Helper loader for the PCAP DLL
|
||||
class PCAPDLL {
|
||||
public:
|
||||
// Functions
|
||||
typedef int(__cdecl* PCAPFINDDEVICE)(char* source, struct pcap_rmtauth* auth, pcap_if_t** alldevs, char* errbuf);
|
||||
typedef pcap_t*(__cdecl* PCAPOPEN)(const char* source, int snaplen, int flags, int read_timeout, struct pcap_rmtauth* auth, char* errbuf);
|
||||
typedef void(__cdecl* PCAPFREEDEVS)(pcap_if_t* alldevsp);
|
||||
typedef void(__cdecl* PCAPCLOSE)(pcap_t* p);
|
||||
typedef int(__cdecl* PCAPSTATS)(pcap_t* p, struct pcap_stat* ps);
|
||||
typedef int(__cdecl* PCAPNEXTEX)(pcap_t* p, struct pcap_pkthdr** pkt_header, const u_char** pkt_data);
|
||||
typedef int(__cdecl* PCAPSENDPACKET)(pcap_t* p, const u_char* buf, int size);
|
||||
// typedef pcap_send_queue*(__cdecl* PCAPSENDQUEUEALLOC)(u_int memsize);
|
||||
// typedef int(__cdecl* PCAPSENDQUEUEQUEUE)(pcap_send_queue* queue, const struct pcap_pkthdr* pkt_header, const u_char* pkt_data);
|
||||
// typedef void(__cdecl* PCAPSENDQUEUEDESTROY)(pcap_send_queue* queue);
|
||||
// typedef u_int(__cdecl* PCAPSENDQUEUETRANSMIT)(pcap_t* p, pcap_send_queue* queue, int sync);
|
||||
typedef int(__cdecl* PCAPDATALINK)(pcap_t* p);
|
||||
typedef int(__cdecl* PCAPCREATESRCSTR)(char* source, int type, const char* host, const char* port, const char* name, char* errbuf);
|
||||
typedef int(__cdecl* PCAPSETBUFF)(pcap_t* p, int dim);
|
||||
PCAPFINDDEVICE findalldevs_ex;
|
||||
PCAPOPEN open;
|
||||
PCAPFREEDEVS freealldevs;
|
||||
PCAPCLOSE close;
|
||||
PCAPSTATS stats;
|
||||
PCAPNEXTEX next_ex;
|
||||
PCAPSENDPACKET sendpacket;
|
||||
// PCAPSENDQUEUEALLOC sendqueue_alloc;
|
||||
// PCAPSENDQUEUEQUEUE sendqueue_queue;
|
||||
// PCAPSENDQUEUEDESTROY sendqueue_destroy;
|
||||
// PCAPSENDQUEUETRANSMIT sendqueue_transmit;
|
||||
PCAPDATALINK datalink;
|
||||
PCAPCREATESRCSTR createsrcstr;
|
||||
PCAPSETBUFF setbuff;
|
||||
|
||||
PCAPDLL();
|
||||
~PCAPDLL() { closeDLL(); }
|
||||
bool ok() const { return dll != nullptr; }
|
||||
private:
|
||||
HINSTANCE dll;
|
||||
void closeDLL();
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,43 @@
|
||||
#include "platform/windows/internal/include/pcapdll.h"
|
||||
|
||||
using namespace icsneo;
|
||||
|
||||
PCAPDLL::PCAPDLL() {
|
||||
dll = LoadLibrary("wpcap.dll");
|
||||
|
||||
if(dll == NULL) {
|
||||
closeDLL();
|
||||
} else {
|
||||
findalldevs_ex = (PCAPFINDDEVICE)GetProcAddress(dll, "pcap_findalldevs_ex");
|
||||
open = (PCAPOPEN)GetProcAddress(dll, "pcap_open");
|
||||
freealldevs = (PCAPFREEDEVS)GetProcAddress(dll, "pcap_freealldevs");
|
||||
close = (PCAPCLOSE)GetProcAddress(dll, "pcap_close");
|
||||
stats = (PCAPSTATS)GetProcAddress(dll, "pcap_stats");
|
||||
next_ex = (PCAPNEXTEX)GetProcAddress(dll, "pcap_next_ex");
|
||||
sendpacket = (PCAPSENDPACKET)GetProcAddress(dll, "pcap_sendpacket");
|
||||
// sendqueue_alloc = (PCAPSENDQUEUEALLOC)GetProcAddress(dll, "pcap_sendqueue_alloc");
|
||||
// sendqueue_queue = (PCAPSENDQUEUEQUEUE)GetProcAddress(dll, "pcap_sendqueue_queue");
|
||||
// sendqueue_destroy = (PCAPSENDQUEUEDESTROY)GetProcAddress(dll, "pcap_sendqueue_destroy");
|
||||
// sendqueue_transmit = (PCAPSENDQUEUETRANSMIT)GetProcAddress(dll, "pcap_sendqueue_transmit");
|
||||
datalink = (PCAPDATALINK)GetProcAddress(dll, "pcap_datalink");
|
||||
createsrcstr = (PCAPCREATESRCSTR)GetProcAddress(dll, "pcap_createsrcstr");
|
||||
setbuff = (PCAPSETBUFF)GetProcAddress(dll, "pcap_setbuff");
|
||||
if(findalldevs_ex == NULL || open == NULL ||
|
||||
freealldevs == NULL || close == NULL ||
|
||||
stats == NULL || next_ex == NULL ||
|
||||
sendpacket == NULL ||
|
||||
// sendqueue_alloc == NULL ||
|
||||
// sendqueue_queue == NULL || sendqueue_destroy == NULL ||
|
||||
// sendqueue_transmit == NULL ||
|
||||
datalink == NULL ||
|
||||
createsrcstr == NULL || setbuff == NULL) {
|
||||
closeDLL();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PCAPDLL::closeDLL() {
|
||||
if(dll)
|
||||
FreeLibrary(dll);
|
||||
dll = nullptr;
|
||||
}
|
||||
@@ -0,0 +1,375 @@
|
||||
#include "platform/windows/include/pcap.h"
|
||||
#include "communication/include/network.h"
|
||||
#include "communication/include/communication.h"
|
||||
#include <pcap.h>
|
||||
#include <iphlpapi.h>
|
||||
#pragma comment(lib, "IPHLPAPI.lib")
|
||||
#include <codecvt>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
|
||||
using namespace icsneo;
|
||||
|
||||
static std::wstring_convert<std::codecvt_utf8_utf16<wchar_t>> converter;
|
||||
|
||||
std::vector<PCAP::NetworkInterface> PCAP::knownInterfaces;
|
||||
|
||||
std::vector<neodevice_t> PCAP::FindByProduct(int product) {
|
||||
std::vector<neodevice_t> foundDevices;
|
||||
PCAPDLL pcap;
|
||||
if(!pcap.ok()) {
|
||||
std::cout << "PCAP not okay" << std::endl;
|
||||
return std::vector<neodevice_t>();
|
||||
}
|
||||
|
||||
// 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_ex too quickly can cause various errors. Retry a few times in this case.
|
||||
for(auto retry = 0; retry < 10; retry++) {
|
||||
auto ret = pcap.findalldevs_ex(PCAP_SRC_IF_STRING, nullptr, &alldevs, errbuf);
|
||||
if(ret == 0) {
|
||||
success = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if(!success) {
|
||||
std::cout << "PCAP FindAllDevs_Ex not okay " << errbuf << std::endl;
|
||||
return std::vector<neodevice_t>();
|
||||
}
|
||||
|
||||
std::vector<NetworkInterface> interfaces;
|
||||
for(pcap_if_t* dev = alldevs; dev != nullptr; dev = dev->next) {
|
||||
NetworkInterface netif;
|
||||
netif.nameFromWinPCAP = dev->name;
|
||||
netif.descriptionFromWinPCAP = dev->description;
|
||||
interfaces.push_back(netif);
|
||||
}
|
||||
|
||||
pcap.freealldevs(alldevs);
|
||||
|
||||
// Now we're going to ask Win32 for the information as well
|
||||
ULONG size = 0;
|
||||
if(GetAdaptersAddresses(AF_UNSPEC, GAA_FLAG_INCLUDE_PREFIX, nullptr, nullptr, &size) != ERROR_BUFFER_OVERFLOW) {
|
||||
std::cout << "GetAdaptersAddresses size query not okay" << std::endl;
|
||||
return std::vector<neodevice_t>();
|
||||
}
|
||||
std::vector<uint8_t> adapterAddressBuffer;
|
||||
adapterAddressBuffer.resize(size);
|
||||
if(GetAdaptersAddresses(AF_UNSPEC, GAA_FLAG_INCLUDE_PREFIX, nullptr, (IP_ADAPTER_ADDRESSES*)adapterAddressBuffer.data(), &size) != ERROR_SUCCESS) {
|
||||
std::cout << "GetAdaptersAddresses not okay" << std::endl;
|
||||
return std::vector<neodevice_t>();
|
||||
}
|
||||
|
||||
// aa->AdapterName constains a unique name of the interface like "{3B1D2791-435A-456F-8A7B-9CB0EEE5DAB3}"
|
||||
// interface.nameFromWinPCAP has "rpcap://\Device\NPF_{3B1D2791-435A-456F-8A7B-9CB0EEE5DAB3}"
|
||||
// We're comparing strings to match the Win32 info with the WinPCAP info
|
||||
for(IP_ADAPTER_ADDRESSES* aa = (IP_ADAPTER_ADDRESSES*)adapterAddressBuffer.data(); aa != nullptr; aa = aa->Next) {
|
||||
for(auto& interface : interfaces) {
|
||||
if(interface.nameFromWinPCAP.find(aa->AdapterName) == std::string::npos)
|
||||
continue; // This is not the interface that corresponds
|
||||
|
||||
memcpy(interface.macAddress, aa->PhysicalAddress, sizeof(interface.macAddress));
|
||||
interface.nameFromWin32API = aa->AdapterName;
|
||||
interface.descriptionFromWin32API = converter.to_bytes(aa->Description);
|
||||
interface.friendlyNameFromWin32API = converter.to_bytes(aa->FriendlyName);
|
||||
if(interface.descriptionFromWin32API.find("LAN9512/LAN9514") != std::string::npos) {
|
||||
// This is an Ethernet EVB device
|
||||
interface.fullName = "Intrepid Ethernet EVB ( " + interface.friendlyNameFromWin32API + " : " + interface.descriptionFromWin32API + " )";
|
||||
} else {
|
||||
interface.fullName = interface.friendlyNameFromWin32API + " : " + interface.descriptionFromWin32API;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
constexpr auto openflags = (PCAP_OPENFLAG_PROMISCUOUS | 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);
|
||||
|
||||
if(interface.fp == nullptr)
|
||||
continue; // Could not open the interface
|
||||
|
||||
EthernetPacket requestPacket;
|
||||
requestPacket.payload.reserve(4);
|
||||
requestPacket.payload = {
|
||||
((1 << 4) | (uint8_t)Network::NetID::Main51), // Packet size of 1 on NETID_MAIN51
|
||||
(uint8_t)Communication::Command::RequestSerialNumber
|
||||
};
|
||||
requestPacket.payload.push_back(Communication::ICSChecksum(requestPacket.payload));
|
||||
requestPacket.payload.insert(requestPacket.payload.begin(), 0xAA);
|
||||
|
||||
// Test cases
|
||||
// std::cout << std::endl << "We would send:" << std::endl << std::hex;
|
||||
// for(auto byte : requestPacket.getBytestream()) {
|
||||
// std::cout << (int)byte << ' ';
|
||||
// }
|
||||
// std::cout << std::dec << std::endl;
|
||||
|
||||
// EthernetPacket bsPacket(requestPacket.getBytestream());
|
||||
// std::cout << std::endl << "We would rx:" << std::endl << std::hex;
|
||||
// for(auto byte : bsPacket.getBytestream()) {
|
||||
// std::cout << (int)byte << ' ';
|
||||
// }
|
||||
// std::cout << std::dec << std::endl;
|
||||
|
||||
auto bs = requestPacket.getBytestream();
|
||||
pcap.sendpacket(interface.fp, bs.data(), (int)bs.size());
|
||||
|
||||
auto timeout = std::chrono::high_resolution_clock::now() + std::chrono::milliseconds(5);
|
||||
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);
|
||||
if(packet.etherType == 0xCAB2 && packet.srcMAC[0] == 0x00 && packet.srcMAC[1] == 0xFC && packet.srcMAC[2] == 0x70) {
|
||||
if(product != packet.srcMAC[3]) // This is where the PID is stored in the MAC
|
||||
continue; // This is not a product we're currently looking for
|
||||
|
||||
std::string serialFromMAC = GetEthDevSerialFromMacAddress(packet.srcMAC[3], ((packet.srcMAC[4] << 8) | packet.srcMAC[5]));
|
||||
neodevice_t neodevice;
|
||||
strncpy(neodevice.serial, serialFromMAC.c_str(), sizeof(neodevice.serial));
|
||||
neodevice.serial[sizeof(neodevice.serial) - 1] = 0;
|
||||
neodevice.handle = (i << 24) | (packet.srcMAC[3] << 16) | (packet.srcMAC[4] << 8) | (packet.srcMAC[5]);
|
||||
bool alreadyExists = false;
|
||||
for(auto& dev : foundDevices)
|
||||
if(dev.handle == neodevice.handle)
|
||||
alreadyExists = true;
|
||||
if(!alreadyExists)
|
||||
foundDevices.push_back(neodevice);
|
||||
}
|
||||
}
|
||||
|
||||
pcap.close(interface.fp);
|
||||
interface.fp = nullptr;
|
||||
}
|
||||
|
||||
return foundDevices;
|
||||
}
|
||||
|
||||
std::string PCAP::GetEthDevSerialFromMacAddress(uint8_t product, uint16_t macSerial) {
|
||||
constexpr uint16_t serialOffset = 0x30;
|
||||
std::string serial;
|
||||
switch(product) {
|
||||
case 0x01: // cmProbe
|
||||
serial += "CM";
|
||||
break;
|
||||
case 0x03: // RADGalaxy
|
||||
serial += "RG";
|
||||
break;
|
||||
case 0x04: // FIRE 2
|
||||
serial += "CY";
|
||||
break;
|
||||
case 0x05: // RADStar 2
|
||||
serial += "RS";
|
||||
break;
|
||||
case 0x06: // RADGigalog
|
||||
serial += "GL";
|
||||
break;
|
||||
default: // Should never happen
|
||||
serial += "XX";
|
||||
break;
|
||||
}
|
||||
for(int i = 1000; i > 0; i /= 10) {
|
||||
serial += (char)(macSerial / i + serialOffset);
|
||||
macSerial %= i;
|
||||
}
|
||||
return serial;
|
||||
}
|
||||
|
||||
bool PCAP::IsHandleValid(neodevice_handle_t handle) {
|
||||
uint8_t netifIndex = (uint8_t)(handle >> 24);
|
||||
return (netifIndex < knownInterfaces.size());
|
||||
}
|
||||
|
||||
PCAP::PCAP(neodevice_t& forDevice) : 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(!pcap.ok())
|
||||
return false;
|
||||
|
||||
if(isOpen())
|
||||
return false;
|
||||
|
||||
// Open the interface
|
||||
interface.fp = pcap.open(interface.nameFromWinPCAP.c_str(), 100, PCAP_OPENFLAG_PROMISCUOUS | PCAP_OPENFLAG_MAX_RESPONSIVENESS, 1, nullptr, errbuf);
|
||||
if(interface.fp == nullptr) {
|
||||
std::cout << "Open device " << device.serial << " failed with " << errbuf << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 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();
|
||||
|
||||
pcap.close(interface.fp);
|
||||
interface.fp = nullptr;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void PCAP::readTask() {
|
||||
constexpr size_t READ_BUFFER_SIZE = 10240;
|
||||
uint8_t readbuf[READ_BUFFER_SIZE];
|
||||
struct pcap_pkthdr* header;
|
||||
const uint8_t* data;
|
||||
while(!closing) {
|
||||
auto readBytes = pcap.next_ex(interface.fp, &header, &data);
|
||||
if(readBytes < 0) {
|
||||
std::cout << "pcapnextex failed in read task with " << readBytes << std::endl;
|
||||
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.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.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<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;
|
||||
}
|
||||
Reference in New Issue
Block a user