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

Communication: EthernetMessage: Switch to TX_WRAP

Browse Source
This commit is contained in:
Thomas Stoddard
2025-07-17 01:49:31 +00:00
committed by Kyle Schwarz
parent 6d985ca873
commit 10efacf91e
4 changed files with 125 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
communication/packet/ethernetpacket.cpp
+72 -39
View File
@@ -1,5 +1,5 @@
#include "icsneo/communication/packet/ethernetpacket.h"
#include <cstring> // memcpy
#include <algorithm> // for std::copy
#include <iostream>
using namespace icsneo;
@@ -7,40 +7,31 @@ using namespace icsneo;
std::shared_ptr<EthernetMessage> HardwareEthernetPacket::DecodeToMessage(const std::vector<uint8_t>& bytestream, const device_eventhandler_t& report) {
const HardwareEthernetPacket* packet = (const HardwareEthernetPacket*)((const void*)bytestream.data());
const uint16_t* rawWords = (const uint16_t*)bytestream.data();
// Make sure we have enough to read the packet length first
if(bytestream.size() < sizeof(HardwareEthernetPacket))
return nullptr;
// packet->Length will also encompass the two uint16_t's at the end of the struct, make sure that at least they are here
if(packet->Length < 4)
return nullptr;
const size_t fcsSize = packet->header.FCS_AVAIL ? 4 : 0;
const size_t bytestreamExpectedSize = sizeof(HardwareEthernetPacket) + packet->Length;
const size_t bytestreamActualSize = bytestream.size();
if(bytestreamActualSize < bytestreamExpectedSize)
return nullptr;
// Check for oversized packets, noting that some devices will send an extra byte to have an even number of bytes
if(bytestreamActualSize > bytestreamExpectedSize + 1)
report(APIEvent::Type::PacketDecodingError, APIEvent::Severity::EventWarning);
auto messagePtr = std::make_shared<EthernetMessage>();
EthernetMessage& message = *messagePtr;
message.transmitted = packet->eid.TXMSG;
if(message.transmitted)
message.description = packet->stats;
message.preemptionEnabled = packet->header.PREEMPTION_ENABLED;
if(message.preemptionEnabled)
message.preemptionFlags = (uint8_t)((rawWords[0] & 0x03F8) >> 4);
message.frameTooShort = packet->header.RUNT_FRAME;
if(message.frameTooShort)
message.error = true;
// This timestamp is raw off the device (in timestampResolution increments)
// Decoder will fix as it has information about the timestampResolution increments
message.timestamp = packet->timestamp.TS;
@@ -48,7 +39,7 @@ std::shared_ptr<EthernetMessage> HardwareEthernetPacket::DecodeToMessage(const s
const std::vector<uint8_t>::const_iterator databegin = bytestream.begin() + sizeof(HardwareEthernetPacket);
const std::vector<uint8_t>::const_iterator dataend = databegin + packet->Length - fcsSize;
message.data.insert(message.data.begin(), databegin, dataend);
if(fcsSize) {
uint32_t& fcs = message.fcs.emplace();
std::copy(dataend, dataend + fcsSize, (uint8_t*)&fcs);
@@ -59,41 +50,83 @@ std::shared_ptr<EthernetMessage> HardwareEthernetPacket::DecodeToMessage(const s
bool HardwareEthernetPacket::EncodeFromMessage(const EthernetMessage& message, std::vector<uint8_t>& bytestream, const device_eventhandler_t&) {
const size_t unpaddedSize = message.data.size();
size_t paddedSize = unpaddedSize;
uint16_t description = message.description;
if(!message.noPadding && unpaddedSize < 60)
paddedSize = 60; // Pad out short messages
size_t sizeWithHeader = paddedSize + 4; // DescriptionID and Padded Count
if(unpaddedSize == 0)
return false;
// Description ID Most Significant bit is used to identify preemption frames
uint16_t description = message.description;
if(description & 0x8000)
return false;
if(message.preemptionEnabled) {
sizeWithHeader++; // Make space for the preemption flags
const bool preempt = message.preemptionEnabled;
// full header including parent
const size_t headerByteCount = preempt ? 15 : 14;
// local header for netID, description, and flags
const size_t localHeader = preempt ? 10 : 9;
// allocate space for fcs override
const size_t fcsSize = message.fcs ? 4 : 0;
if(preempt)
description |= 0x8000;
size_t paddedSize = unpaddedSize;
if(!message.noPadding && unpaddedSize < 60)
paddedSize = 60;
// size of full payload including optional fcs
size_t payloadSize = paddedSize + fcsSize;
// totalBufferSize is local header + ethernet payload and option fcs
const size_t totalBufferSize = localHeader + paddedSize + fcsSize;
bytestream.clear();
bytestream.reserve(totalBufferSize);
// Header size field (little endian)
bytestream.push_back(static_cast<uint8_t>(payloadSize & 0xFF));
bytestream.push_back(static_cast<uint8_t>((payloadSize >> 8) & 0xFF));
// Description (big endian)
bytestream.push_back(static_cast<uint8_t>(description >> 8));
bytestream.push_back(static_cast<uint8_t>(description));
bytestream.push_back(0x00);
bytestream.push_back(static_cast<uint8_t>(headerByteCount));
// Network ID (little endian)
uint16_t realID = static_cast<uint16_t>(message.network.getNetID());
bytestream.push_back(static_cast<uint8_t>(realID & 0xFF));
bytestream.push_back(static_cast<uint8_t>((realID >> 8) & 0xFF));
// Flags
constexpr uint8_t FLAG_PADDING = 0x01;
constexpr uint8_t FLAG_FCS = 0x04;
constexpr uint8_t FLAG_PREEMPTION = 0x08;
uint8_t flags = 0x00;
if(!message.noPadding) flags |= FLAG_PADDING;
if(message.fcs) flags |= FLAG_FCS;
if(message.preemptionEnabled) flags |= FLAG_PREEMPTION;
bytestream.push_back(flags);
if(preempt)
bytestream.push_back(static_cast<uint8_t>(message.preemptionFlags));
bytestream.insert(bytestream.end(), message.data.begin(), message.data.end());
// Only zero-fill when we want padding
if(!message.noPadding && unpaddedSize < 60) {
size_t paddingNeeded = 60 - unpaddedSize;
bytestream.insert(bytestream.end(), paddingNeeded, 0); // Zero-fill for padding
}
bytestream.reserve(sizeWithHeader + 8); // Also reserve space for the bytes we'll use later on
bytestream.resize(sizeWithHeader);
size_t index = 0;
// Padded size, little endian
bytestream[index++] = uint8_t(paddedSize);
bytestream[index++] = uint8_t(paddedSize >> 8);
// Description ID, big endian
bytestream[index++] = uint8_t(description >> 8);
bytestream[index++] = uint8_t(description);
// The header is one byte larger if preemption is enabled, shifting the data
if(message.preemptionEnabled)
bytestream[index++] = message.preemptionFlags;
// We only copy in the unpadded size, the rest will be 0
memcpy(bytestream.data() + index, message.data.data(), unpaddedSize);
if(message.fcs) {
uint32_t fcs = message.fcs.value();
const uint8_t* fcsBytes = reinterpret_cast<const uint8_t*>(&fcs);
bytestream.insert(bytestream.end(), fcsBytes, fcsBytes + sizeof(fcs));
}
return true;
}
}