From a44952be1334d28de3264cd3ebdf1c559bf7d68c Mon Sep 17 00:00:00 2001 From: Max Brombach Date: Tue, 16 Jan 2024 18:01:10 +0000 Subject: [PATCH] Disk: Update VSA Example and Fix VSA CAN-FD Decode - Send and validate CAN/CAN-FD/Eth frames in VSA example (two devices) - Fix failure to decode CAN-FD frames from VSA records --- device/device.cpp | 2 +- disk/vsa/vsa.cpp | 12 +- disk/vsa/vsa0d.cpp | 3 + disk/vsa/vsa0e.cpp | 7 +- examples/cpp/vsa/src/VSAExample.cpp | 197 +++++++++++++++++++++++++--- include/icsneo/disk/vsa/vsa0d.h | 1 + include/icsneo/disk/vsa/vsa0e.h | 1 + 7 files changed, 196 insertions(+), 27 deletions(-) diff --git a/device/device.cpp b/device/device.cpp index 76abdd2..e1f22ab 100644 --- a/device/device.cpp +++ b/device/device.cpp @@ -2096,7 +2096,7 @@ bool Device::readVSA(const VSAExtractionSettings& extractionSettings) { return false; } - const auto& wasScriptStarted = getScriptStatus()->isCoreminiRunning; + const auto wasScriptStarted = getScriptStatus()->isCoreminiRunning; if(extractionSettings.stopCoreMini && wasScriptStarted) { stopScript(); diff --git a/disk/vsa/vsa.cpp b/disk/vsa/vsa.cpp index 6684cb5..ca22333 100644 --- a/disk/vsa/vsa.cpp +++ b/disk/vsa/vsa.cpp @@ -1,5 +1,6 @@ #include "icsneo/disk/vsa/vsa.h" #include "icsneo/communication/packet/ethernetpacket.h" +#include using namespace icsneo; @@ -31,9 +32,12 @@ void VSAExtendedMessage::truncatePacket(std::shared_ptr packet) static constexpr auto EthernetLengthOffset = 26u; switch(packet->network.getType()) { case Network::Type::Ethernet: - const auto& packetLength = *reinterpret_cast(packet->data.data() + EthernetLengthOffset); - const size_t ethernetFrameSize = packetLength - (sizeof(uint16_t) * 2); - const size_t bytestreamExpectedSize = sizeof(HardwareEthernetPacket) + ethernetFrameSize; - packet->data.resize(bytestreamExpectedSize); + { + const auto& packetLength = *reinterpret_cast(packet->data.data() + EthernetLengthOffset); + const size_t ethernetFrameSize = packetLength - (sizeof(uint16_t) * 2); + const size_t bytestreamExpectedSize = sizeof(HardwareEthernetPacket) + ethernetFrameSize; + packet->data.resize(bytestreamExpectedSize); + } + break; } } \ No newline at end of file diff --git a/disk/vsa/vsa0d.cpp b/disk/vsa/vsa0d.cpp index 76beda2..22a7048 100644 --- a/disk/vsa/vsa0d.cpp +++ b/disk/vsa/vsa0d.cpp @@ -62,6 +62,9 @@ void VSA0DFirst::reorderPayload(std::vector& secondPayload) std::vector tempPayload; tempPayload.insert(tempPayload.end(), secondPayload.begin(), secondPayload.begin() + 4); uint8_t* timestampBytes = reinterpret_cast(×tamp); + if(timestampIsExtended) { + timestampBytes[7] += 0x80; + } tempPayload.insert(tempPayload.end(), timestampBytes, timestampBytes + 8); tempPayload.insert(tempPayload.end(), secondPayload.begin() + 4, secondPayload.end()); payload.clear(); diff --git a/disk/vsa/vsa0e.cpp b/disk/vsa/vsa0e.cpp index 71d120f..ac61b81 100644 --- a/disk/vsa/vsa0e.cpp +++ b/disk/vsa/vsa0e.cpp @@ -42,6 +42,7 @@ VSA0EFirst::VSA0EFirst(uint8_t* const recordBytes, uint32_t& runningChecksum) captureBitfield = *reinterpret_cast(recordBytes + 4); setRecordCount(*reinterpret_cast(recordBytes + 6)); timestamp = *reinterpret_cast(recordBytes + 20) & UINT63_MAX; + timestampIsExtended = (bool)(*reinterpret_cast(recordBytes + 20) & (0x8000000000000000)); checksum = *reinterpret_cast(recordBytes + 30); doChecksum(recordBytes); } @@ -75,16 +76,18 @@ bool VSA0EFirst::filter(const std::shared_ptr filter) void VSA0EFirst::reorderPayload(std::vector& secondPayload) { std::vector tempPayload; - tempPayload.insert(tempPayload.end(), { 0, 0 }); // Leaving this in here temporarily to figure out checksum stuff tempPayload.insert(tempPayload.end(), payload.begin(), payload.end()); tempPayload.insert(tempPayload.end(), secondPayload.begin(), secondPayload.begin() + 6); uint8_t* timestampBytes = reinterpret_cast(×tamp); + if(timestampIsExtended) { + timestampBytes[7] += 0x80; + } tempPayload.insert(tempPayload.end(), timestampBytes, timestampBytes + 8); tempPayload.insert(tempPayload.end(), secondPayload.begin() + 6, secondPayload.end()); payload.clear(); secondPayload.clear(); payload.insert(payload.end(), tempPayload.begin(), tempPayload.begin() + 10); // This is done because the capacity of payload is already 10 - secondPayload.insert(secondPayload.end(), tempPayload.begin() + 12, tempPayload.end()); + secondPayload.insert(secondPayload.end(), tempPayload.begin() + 10, tempPayload.end()); } // Consecutive Record Functions diff --git a/examples/cpp/vsa/src/VSAExample.cpp b/examples/cpp/vsa/src/VSAExample.cpp index c521080..5967786 100644 --- a/examples/cpp/vsa/src/VSAExample.cpp +++ b/examples/cpp/vsa/src/VSAExample.cpp @@ -3,17 +3,103 @@ #include "icsneo/icsneocpp.h" +enum class MessageType { + ShortEth, + LongEth, + CAN, + CANFD +}; + +const std::vector MessageTypeLabels = {"short Ethernet", "long Ethernet", "CAN", "CAN-FD"}; + void onEvent(std::shared_ptr event) { std::cout << event->describe() << std::endl; } +std::vector> constructRandomFrames(size_t frameCount, MessageType frameType) { + static constexpr size_t ClassicCANSize = 8; + static constexpr size_t CANFDSize = 64; + static constexpr size_t ShortEthSize = 500; + static constexpr size_t LongEthSize = 1500; + + std::vector> frames; + std::random_device randDev; + std::mt19937 randEngine(randDev()); + std::uniform_int_distribution randByteDist(0,255); + auto randByteGen = [&]() -> uint8_t { + return static_cast(randByteDist(randEngine)); + }; + + for(size_t i = 0; i < frameCount; i++) { + switch(frameType) { + case MessageType::ShortEth: + // Short Ethernet + { + auto frame = std::make_shared(); + frame->network = icsneo::Network::NetID::Ethernet; + frames.push_back(frame); + frame->data.resize(ShortEthSize); + std::generate(frame->data.begin(), frame->data.end(), randByteGen); + } + break; + case MessageType::LongEth: + // Long Ethernet + { + auto frame = std::make_shared(); + frame->network = icsneo::Network::NetID::Ethernet; + frames.push_back(frame); + frame->data.resize(LongEthSize); + std::generate(frame->data.begin(), frame->data.end(), randByteGen); + } + break; + case MessageType::CAN: + // Classic CAN + { + auto frame = std::make_shared(); + frame->network = icsneo::Network::NetID::HSCAN2; + frames.push_back(frame); + frame->data.resize(ClassicCANSize); + + std::generate(frame->data.begin(), frame->data.end(), randByteGen); + } + break; + case MessageType::CANFD: + // CAN FD + { + auto frame = std::make_shared(); + frame->network = icsneo::Network::NetID::HSCAN3; + frames.push_back(frame); + frame->data.resize(CANFDSize); + std::generate(frame->data.begin(), frame->data.end(), randByteGen); + frame->isCANFD = true; + } + break; + } + } + return frames; +} + +void resetScriptStatus(std::shared_ptr rxDevice, std::shared_ptr txDevice, bool rxInit, bool txInit) { + if(rxInit) { + rxDevice->startScript(); + } else { + rxDevice->stopScript(); + } + if(txInit) { + txDevice->startScript(); + } else { + txDevice->stopScript(); + } +} + int main(int argc, char* argv[]) { - if(argc != 2) { - std::cout << "Usage: libicsneocpp-vsa-example " << std::endl; + if(argc != 3) { + std::cout << "Usage: libicsneocpp-vsa-example " << std::endl; return -1; } - std::string deviceSerial = argv[1]; + std::string txSerial = argv[1]; + std::string rxSerial = argv[2]; // register an event callback so we can see all logged events icsneo::EventManager::GetInstance().addEventCallback(icsneo::EventCallback(onEvent)); @@ -24,44 +110,62 @@ int main(int argc, char* argv[]) { return -1; } - std::shared_ptr device; + std::shared_ptr txDevice; + std::shared_ptr rxDevice; for(auto&& d : devices) { - if(deviceSerial == d->getSerial()) { - device = d; + if(txSerial == d->getSerial()) { + txDevice = d; + } + if(rxSerial == d->getSerial()) { + rxDevice = d; } } - if(!device) { - std::cout << "error: failed to find a device with serial number: " << deviceSerial << std::endl; + if(!txDevice) { + std::cout << "error: failed to find a device with serial number: " << txSerial << std::endl; + return -1; + } + if(!rxDevice) { + std::cout << "error: failed to find a device with serial number: " << rxSerial << std::endl; return -1; } - std::cout << "info: found " << device->describe() << std::endl; + std::cout << "info: found " << txDevice->describe() << std::endl; + std::cout << "info: found " << rxDevice->describe() << std::endl; - if(!device->open()) { + if(!txDevice->open()) { + std::cout << "error: unable to open device" << std::endl; + } + if(!rxDevice->open()) { std::cout << "error: unable to open device" << std::endl; } - device->stopScript(); + const auto& rxInitialCoreminiStatus = rxDevice->getScriptStatus()->isCoreminiRunning; + const auto& txInitialCoreminiStatus = txDevice->getScriptStatus()->isCoreminiRunning; - uint64_t firstOffset; - std::shared_ptr firstRecord; - if(!device->findFirstVSARecord(firstOffset, firstRecord)) { + rxDevice->stopScript(); + txDevice->stopScript(); + + uint64_t origFirstOffset; + std::shared_ptr origFirstRecord; + if(!rxDevice->findFirstVSARecord(origFirstOffset, origFirstRecord)) { std::cout << "error: unable to find first VSA record" << std::endl; + resetScriptStatus(rxDevice, txDevice, rxInitialCoreminiStatus, txInitialCoreminiStatus); return -1; } - std::cout << "info: found first VSA record at " << firstOffset << std::endl; + std::cout << "info: found first VSA record at " << origFirstOffset << std::endl; uint64_t origLastOffset; std::shared_ptr origLastRecord; - if(!device->findLastVSARecord(origLastOffset, origLastRecord)) { + if(!rxDevice->findLastVSARecord(origLastOffset, origLastRecord)) { std::cout << "error: unable to find last VSA record" << std::endl; + resetScriptStatus(rxDevice, txDevice, rxInitialCoreminiStatus, txInitialCoreminiStatus); return -1; } std::cout << "info: found last VSA record at " << origLastOffset << std::endl; uint64_t canFrameCount = 0; uint64_t ethFrameCount = 0; - device->addMessageCallback(std::make_shared([&](std::shared_ptr msg) { + rxDevice->addMessageCallback(std::make_shared([&](std::shared_ptr msg) { if(msg->type != icsneo::Message::Type::Frame) { return; } @@ -80,15 +184,68 @@ int main(int argc, char* argv[]) { } { auto& filter = settings.filters.emplace_back(); - filter.readRange.first = firstRecord->getTimestampICSClock() + std::chrono::seconds(0); - filter.readRange.second = firstRecord->getTimestampICSClock() + std::chrono::seconds(10); + filter.readRange.first = origFirstRecord->getTimestampICSClock() + std::chrono::seconds(0); + filter.readRange.second = origFirstRecord->getTimestampICSClock() + std::chrono::seconds(10); } std::cout << "info: reading two blocks of VSA, 10s from the start and 10s from the end..." << std::endl; - if(!device->readVSA(settings)) { + if(!rxDevice->readVSA(settings)) { std::cout << "error: unable to read VSA" << std::endl; + resetScriptStatus(rxDevice, txDevice, rxInitialCoreminiStatus, txInitialCoreminiStatus); return -1; } std::cout << "info: processed " << canFrameCount << " CAN frames and " << ethFrameCount << " Ethernet frames" << std::endl; + rxDevice->startScript(); + txDevice->goOnline(); + + const uint8_t NumFrameTypes = 4; + const size_t FrameCountPerType = 2500; + std::vector> frames; + for(uint8_t i = 0; i < NumFrameTypes; i++) { + std::cout << "info: transmitting " << FrameCountPerType << " random " << MessageTypeLabels[i] << " frames" << std::endl; + auto tempFrames = constructRandomFrames(FrameCountPerType, static_cast(i)); + frames.insert(frames.end(), tempFrames.begin(), tempFrames.end()); + if(!txDevice->transmit(tempFrames)) { + std::cout << "error: failed to transmit frames" << std::endl; + resetScriptStatus(rxDevice, txDevice, rxInitialCoreminiStatus, txInitialCoreminiStatus); + return -1; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(600)); + } + + size_t currentMessage = 0; + rxDevice->addMessageCallback(std::make_shared([&](std::shared_ptr msg) { + if(msg->type != icsneo::Message::Type::Frame) { + return; + } + auto frame = std::static_pointer_cast(msg); + if(frames[currentMessage]->data == frame->data) { + currentMessage++; + } + })); + + // Read from original last frame until end of buffer + icsneo::VSAExtractionSettings transmitSettings; + { + auto& filter = transmitSettings.filters.emplace_back(); + filter.readRange.first = origLastRecord->getTimestampICSClock(); + } + + std::cout << "info: reading transmitted random frames..." << std::endl; + if(!rxDevice->readVSA(transmitSettings)) { + std::cout << "error: unable to read VSA" << std::endl; + resetScriptStatus(rxDevice, txDevice, rxInitialCoreminiStatus, txInitialCoreminiStatus); + return -1; + } + + std::cout << "info: " << currentMessage << " transmitted frames found" << std::endl; + resetScriptStatus(rxDevice, txDevice, rxInitialCoreminiStatus, txInitialCoreminiStatus); + + if(currentMessage != FrameCountPerType * NumFrameTypes) { + std::cout << "error: unable to find all transmitted frames" << std::endl; + return -1; + } + return 0; } \ No newline at end of file diff --git a/include/icsneo/disk/vsa/vsa0d.h b/include/icsneo/disk/vsa/vsa0d.h index 6ab2c3a..208953b 100644 --- a/include/icsneo/disk/vsa/vsa0d.h +++ b/include/icsneo/disk/vsa/vsa0d.h @@ -82,6 +82,7 @@ private: uint8_t reserved : 6; // Unused bytes } vNetInfo; // Struct used to indicate which bytes are actually used for VNetSlot uint16_t checksum; // The sum of the previous 15 words + bool timestampIsExtended = false; }; /** diff --git a/include/icsneo/disk/vsa/vsa0e.h b/include/icsneo/disk/vsa/vsa0e.h index b2c4495..4b3d556 100644 --- a/include/icsneo/disk/vsa/vsa0e.h +++ b/include/icsneo/disk/vsa/vsa0e.h @@ -78,6 +78,7 @@ private: uint16_t captureBitfield; // The data capture this record is a part of uint64_t timestamp; // Timestamp of this record in 25 nanosecond ticks since January 1, 2007 uint16_t checksum; // Sum of the previous 15 words + bool timestampIsExtended = false; }; /**