mirror of
https://github.com/intrepidcs/libicsneo.git
synced 2026-09-20 07:58:39 +02:00
Device: Add ISO-15765 hardware acceleration
This commit is contained in:
Kurt Wachowski
committed by
Kyle Schwarz
Kyle Schwarz
parent
34875129ee
commit
3a6501d116
@@ -0,0 +1,2 @@
|
||||
add_executable(libicsneocpp-iso15765-loopback src/ISO15765Loopback.cpp)
|
||||
target_link_libraries(libicsneocpp-iso15765-loopback icsneocpp)
|
||||
@@ -0,0 +1,450 @@
|
||||
#include <cctype>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include "icsneo/icsneocpp.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
enum class Mode {
|
||||
None,
|
||||
Raw,
|
||||
SingleFrame,
|
||||
Both,
|
||||
};
|
||||
|
||||
struct Options {
|
||||
Mode mode = Mode::None;
|
||||
std::string testerSerial;
|
||||
std::string ecuSerial;
|
||||
int64_t baud = 500000;
|
||||
bool keepBaud = false;
|
||||
uint32_t txId = 0x7E0;
|
||||
uint32_t fcId = 0x7E8;
|
||||
std::optional<size_t> payloadBytes;
|
||||
int listenMs = 2000;
|
||||
uint8_t padding = 0x00;
|
||||
uint16_t fsTimeoutMs = 75;
|
||||
uint16_t fsWaitMs = 150;
|
||||
uint16_t cfTimeoutMs = 150;
|
||||
};
|
||||
|
||||
std::mutex gLogMutex;
|
||||
|
||||
void logLine(const std::string& line) {
|
||||
std::lock_guard<std::mutex> lk(gLogMutex);
|
||||
std::cout << line << std::endl;
|
||||
}
|
||||
|
||||
bool iequals(const std::string& a, const std::string& b) {
|
||||
if(a.size() != b.size())
|
||||
return false;
|
||||
for(size_t i = 0; i < a.size(); i++) {
|
||||
if(std::tolower(static_cast<unsigned char>(a[i])) != std::tolower(static_cast<unsigned char>(b[i])))
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string hexBytes(const std::vector<uint8_t>& data) {
|
||||
std::ostringstream oss;
|
||||
oss << std::hex << std::setfill('0');
|
||||
for(size_t i = 0; i < data.size(); i++) {
|
||||
if(i)
|
||||
oss << ' ';
|
||||
oss << std::setw(2) << static_cast<unsigned>(data[i]);
|
||||
}
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
std::string pciDescribe(const std::vector<uint8_t>& data) {
|
||||
if(data.empty())
|
||||
return {};
|
||||
const uint8_t pciType = static_cast<uint8_t>(data[0] >> 4);
|
||||
const uint8_t pciLow = static_cast<uint8_t>(data[0] & 0x0F);
|
||||
std::ostringstream oss;
|
||||
switch(pciType) {
|
||||
case 0x0:
|
||||
if(pciLow > 7 || data.size() < static_cast<size_t>(1 + pciLow))
|
||||
return {};
|
||||
oss << "SF len=" << static_cast<unsigned>(pciLow);
|
||||
break;
|
||||
case 0x1: {
|
||||
if(data.size() < 8)
|
||||
return {};
|
||||
const uint16_t len = static_cast<uint16_t>((pciLow << 8) | data[1]);
|
||||
if(len < 8)
|
||||
return {};
|
||||
oss << "FF len=" << len;
|
||||
break;
|
||||
}
|
||||
case 0x2:
|
||||
if(data.size() < 2)
|
||||
return {};
|
||||
oss << "CF SN=" << static_cast<unsigned>(pciLow);
|
||||
break;
|
||||
case 0x3: {
|
||||
if(data.size() < 3 || pciLow > 2)
|
||||
return {};
|
||||
static const char* fsName[] = { "CTS", "WAIT", "OVFLW" };
|
||||
oss << "FC FS=" << fsName[pciLow];
|
||||
oss << " BS=" << static_cast<unsigned>(data[1]);
|
||||
oss << " STmin=0x" << std::hex << std::setw(2) << std::setfill('0')
|
||||
<< static_cast<unsigned>(data[2]);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
return {};
|
||||
}
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
void printUsage(const char* argv0) {
|
||||
std::cerr
|
||||
<< "Usage: " << argv0 << " --raw|--sf|--both [options]\n\n"
|
||||
<< "Two Intrepid devices with CAN1 wired together (CANH/CANL, terminated).\n"
|
||||
<< "Prints CAN1 frames with ISO-TP PCI decode (SF/FF/CF/FC).\n\n"
|
||||
<< "Modes:\n"
|
||||
<< " --raw Raw CAN ping-pong (cable check)\n"
|
||||
<< " --sf Firmware ISO-TP single-frame TX from tester\n"
|
||||
<< " --both Firmware ISO-TP TX on tester, firmware flow-control on ECU\n\n"
|
||||
<< "Devices:\n"
|
||||
<< " --tester SERIAL Tester (sender) serial\n"
|
||||
<< " --ecu SERIAL ECU / peer serial\n"
|
||||
<< " Optional if exactly two devices are found.\n\n"
|
||||
<< "Optional:\n"
|
||||
<< " --baud N CAN1 baud (default 500000, applied temporarily)\n"
|
||||
<< " --keep-baud Do not change device baud\n"
|
||||
<< " --tx-id HEX Tester ISO-TP CAN ID (default 0x7E0)\n"
|
||||
<< " --fc-id HEX Flow-control CAN ID (default 0x7E8)\n"
|
||||
<< " --bytes N ISO-TP payload size (sf default 5, both default 20)\n"
|
||||
<< " --listen-ms N Time to listen after the action (default 2000)\n";
|
||||
}
|
||||
|
||||
uint32_t parseU32(const std::string& s) {
|
||||
return static_cast<uint32_t>(std::stoul(s, nullptr, 0));
|
||||
}
|
||||
|
||||
bool parseArgs(int argc, char** argv, Options& opt) {
|
||||
for(int i = 1; i < argc; i++) {
|
||||
const std::string a = argv[i];
|
||||
auto need = [&](const char* name) -> std::string {
|
||||
if(i + 1 >= argc)
|
||||
throw std::runtime_error(std::string("missing value for ") + name);
|
||||
return argv[++i];
|
||||
};
|
||||
if(a == "--raw")
|
||||
opt.mode = Mode::Raw;
|
||||
else if(a == "--sf")
|
||||
opt.mode = Mode::SingleFrame;
|
||||
else if(a == "--both")
|
||||
opt.mode = Mode::Both;
|
||||
else if(a == "--tester")
|
||||
opt.testerSerial = need("--tester");
|
||||
else if(a == "--ecu")
|
||||
opt.ecuSerial = need("--ecu");
|
||||
else if(a == "--baud")
|
||||
opt.baud = static_cast<int64_t>(std::stoll(need("--baud")));
|
||||
else if(a == "--keep-baud")
|
||||
opt.keepBaud = true;
|
||||
else if(a == "--tx-id")
|
||||
opt.txId = parseU32(need("--tx-id"));
|
||||
else if(a == "--fc-id")
|
||||
opt.fcId = parseU32(need("--fc-id"));
|
||||
else if(a == "--bytes")
|
||||
opt.payloadBytes = static_cast<size_t>(std::stoul(need("--bytes")));
|
||||
else if(a == "--listen-ms")
|
||||
opt.listenMs = std::stoi(need("--listen-ms"));
|
||||
else if(a == "-h" || a == "--help") {
|
||||
printUsage(argv[0]);
|
||||
return false;
|
||||
} else {
|
||||
std::cerr << "Unknown argument: " << a << "\n";
|
||||
printUsage(argv[0]);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if(opt.mode == Mode::None) {
|
||||
printUsage(argv[0]);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
size_t defaultPayloadSize(Mode mode) {
|
||||
return mode == Mode::SingleFrame ? 5 : 20;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> makePayload(size_t n) {
|
||||
std::vector<uint8_t> data(n);
|
||||
for(size_t i = 0; i < n; i++)
|
||||
data[i] = static_cast<uint8_t>(i & 0xFF);
|
||||
return data;
|
||||
}
|
||||
|
||||
bool isExtended(uint32_t id) {
|
||||
return id > 0x7FFu;
|
||||
}
|
||||
|
||||
void printApiResult(const char* what, bool ok) {
|
||||
std::ostringstream oss;
|
||||
oss << " " << what << ": " << (ok ? "OK" : "FAIL");
|
||||
if(!ok) {
|
||||
const auto err = icsneo::GetLastError();
|
||||
if(err.getType() != icsneo::APIEvent::Type::NoErrorFound)
|
||||
oss << " [" << err << "]";
|
||||
}
|
||||
logLine(oss.str());
|
||||
}
|
||||
|
||||
bool transmitCan(icsneo::Device& device, uint32_t arbId, std::vector<uint8_t> data) {
|
||||
auto frame = std::make_shared<icsneo::CANMessage>();
|
||||
frame->network = icsneo::Network::NetID::DWCAN_01;
|
||||
frame->arbid = arbId;
|
||||
frame->data = std::move(data);
|
||||
frame->isExtended = isExtended(arbId);
|
||||
frame->isCANFD = false;
|
||||
const bool ok = device.transmit(frame);
|
||||
if(!ok)
|
||||
printApiResult("transmit CAN", ok);
|
||||
return ok;
|
||||
}
|
||||
|
||||
void fillIso15765(icsneo::Iso15765MessageArgs& msg, const Options& opt, const std::vector<uint8_t>& payload, bool rx) {
|
||||
msg.setTxIndex(0);
|
||||
msg.setArbId(icsneo::Iso15765MessageArgs::ArbId(opt.txId, isExtended(opt.txId)));
|
||||
msg.setFlowControlArbId(icsneo::Iso15765MessageArgs::ArbId(opt.fcId, isExtended(opt.fcId)));
|
||||
msg.setFlowControlArbIdMask(isExtended(opt.txId) ? 0x1FFFFFFFu : 0x7FFu);
|
||||
msg.setIsCanFd(false);
|
||||
msg.setIsBrsEnabled(false);
|
||||
msg.setTxDl(8);
|
||||
msg.setPaddingValue(opt.padding);
|
||||
msg.setFsTimeout(opt.fsTimeoutMs);
|
||||
msg.setFsWaitTimeout(opt.fsWaitMs);
|
||||
msg.setCfTimeout(opt.cfTimeoutMs);
|
||||
msg.setIsFlowControlEnabled(rx);
|
||||
if(!payload.empty()) {
|
||||
auto copy = payload;
|
||||
msg.setData(std::move(copy));
|
||||
}
|
||||
}
|
||||
|
||||
struct Sniffer {
|
||||
const char* tag = "";
|
||||
std::optional<uint64_t> t0;
|
||||
std::mutex t0Mutex;
|
||||
|
||||
void onCan(const std::shared_ptr<icsneo::CANMessage>& can) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(t0Mutex);
|
||||
if(!t0)
|
||||
t0 = can->timestamp;
|
||||
}
|
||||
|
||||
double relMs = 0.0;
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(t0Mutex);
|
||||
if(t0 && can->timestamp >= *t0)
|
||||
relMs = static_cast<double>(can->timestamp - *t0) / 1e6;
|
||||
}
|
||||
|
||||
std::ostringstream oss;
|
||||
oss << " [" << tag << (can->transmitted ? " TX" : " RX") << "] "
|
||||
<< std::fixed << std::setprecision(3) << relMs << " ms "
|
||||
<< can->network << " 0x" << std::hex << std::uppercase
|
||||
<< std::setw(isExtended(can->arbid) ? 8 : 3) << std::setfill('0') << can->arbid
|
||||
<< std::dec << " [" << can->data.size() << "] " << hexBytes(can->data);
|
||||
const auto pci = pciDescribe(can->data);
|
||||
if(!pci.empty())
|
||||
oss << " " << pci;
|
||||
logLine(oss.str());
|
||||
}
|
||||
};
|
||||
|
||||
void addCanSniffer(icsneo::Device& device, Sniffer& sniffer) {
|
||||
auto filter = std::make_shared<icsneo::MessageFilter>(icsneo::Network::NetID::DWCAN_01);
|
||||
device.addMessageCallback(std::make_shared<icsneo::MessageCallback>(filter, [&sniffer](std::shared_ptr<icsneo::Message> message) {
|
||||
if(message->type != icsneo::Message::Type::Frame)
|
||||
return;
|
||||
auto frame = std::static_pointer_cast<icsneo::Frame>(message);
|
||||
if(frame->network.getType() != icsneo::Network::Type::CAN)
|
||||
return;
|
||||
sniffer.onCan(std::static_pointer_cast<icsneo::CANMessage>(message));
|
||||
}));
|
||||
}
|
||||
|
||||
bool openAndOnline(std::shared_ptr<icsneo::Device>& device, const Options& opt) {
|
||||
logLine(std::string("Opening ") + device->describe() + " ...");
|
||||
if(!device->open()) {
|
||||
printApiResult("open", false);
|
||||
return false;
|
||||
}
|
||||
if(!opt.keepBaud && device->settings) {
|
||||
if(!device->settings->setBaudrateFor(icsneo::Network::NetID::DWCAN_01, opt.baud))
|
||||
printApiResult("setBaudrateFor", false);
|
||||
else if(!device->settings->apply(true))
|
||||
printApiResult("settings->apply(temporary)", false);
|
||||
}
|
||||
if(!device->goOnline()) {
|
||||
printApiResult("goOnline", false);
|
||||
device->close();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void closeDevice(const std::shared_ptr<icsneo::Device>& device) {
|
||||
if(!device)
|
||||
return;
|
||||
device->iso15765DisableAll();
|
||||
if(device->isOnline())
|
||||
device->goOffline();
|
||||
if(device->isOpen())
|
||||
device->close();
|
||||
}
|
||||
|
||||
bool enableIso(icsneo::Device& device, const char* who) {
|
||||
const bool ok = device.iso15765Enable(icsneo::Network(icsneo::Network::NetID::DWCAN_01));
|
||||
printApiResult((std::string(who) + " ISO15765_Enable").c_str(), ok);
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool receiveIso(icsneo::Device& device, const Options& opt) {
|
||||
icsneo::Iso15765MessageArgs msg;
|
||||
fillIso15765(msg, opt, {}, true);
|
||||
const bool ok = device.iso15765SetupRxFlowControl(icsneo::Network(icsneo::Network::NetID::DWCAN_01), msg);
|
||||
printApiResult("ECU ISO15765_ReceiveMessage", ok);
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool transmitIso(icsneo::Device& device, const Options& opt, const std::vector<uint8_t>& payload) {
|
||||
icsneo::Iso15765MessageArgs msg;
|
||||
fillIso15765(msg, opt, payload, false);
|
||||
const bool ok = device.iso15765TransmitMessage(
|
||||
icsneo::Network(icsneo::Network::NetID::DWCAN_01), msg, std::chrono::milliseconds(0));
|
||||
printApiResult("tester ISO15765_TransmitMessage", ok);
|
||||
return ok;
|
||||
}
|
||||
|
||||
std::shared_ptr<icsneo::Device> findBySerial(const std::vector<std::shared_ptr<icsneo::Device>>& devices, const std::string& serial) {
|
||||
for(const auto& d : devices) {
|
||||
if(iequals(d->getSerial(), serial))
|
||||
return d;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
Options opt;
|
||||
try {
|
||||
if(!parseArgs(argc, argv, opt))
|
||||
return 1;
|
||||
} catch(const std::exception& ex) {
|
||||
std::cerr << ex.what() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::cout << "libicsneo " << icsneo::GetVersion() << std::endl;
|
||||
|
||||
auto devices = icsneo::FindAllDevices();
|
||||
std::cout << "Found " << devices.size() << " device(s)\n";
|
||||
for(const auto& d : devices)
|
||||
std::cout << " " << d->describe() << " serial=" << d->getSerial() << "\n";
|
||||
|
||||
if(devices.size() < 2) {
|
||||
std::cerr << "Need two devices connected.\n";
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::shared_ptr<icsneo::Device> tester;
|
||||
std::shared_ptr<icsneo::Device> ecu;
|
||||
if(opt.testerSerial.empty() && opt.ecuSerial.empty()) {
|
||||
if(devices.size() != 2) {
|
||||
std::cerr << "More than two devices found; pass --tester and --ecu serials.\n";
|
||||
return 1;
|
||||
}
|
||||
tester = devices[0];
|
||||
ecu = devices[1];
|
||||
std::cout << "No serials given; using first as tester, second as ECU.\n";
|
||||
} else {
|
||||
if(opt.testerSerial.empty() || opt.ecuSerial.empty()) {
|
||||
std::cerr << "Pass both --tester and --ecu, or neither.\n";
|
||||
return 1;
|
||||
}
|
||||
tester = findBySerial(devices, opt.testerSerial);
|
||||
ecu = findBySerial(devices, opt.ecuSerial);
|
||||
if(!tester || !ecu) {
|
||||
std::cerr << "Could not match tester/ECU serials.\n";
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
const size_t payloadSize = opt.payloadBytes.value_or(defaultPayloadSize(opt.mode));
|
||||
const auto payload = makePayload(payloadSize);
|
||||
|
||||
std::cout << "Tester: " << tester->describe() << "\n"
|
||||
<< "ECU: " << ecu->describe() << "\n";
|
||||
if(opt.mode != Mode::Raw) {
|
||||
std::ostringstream hdr;
|
||||
hdr << "CAN1 TX 0x" << std::hex << std::uppercase << opt.txId
|
||||
<< " FC 0x" << opt.fcId << std::dec
|
||||
<< " payload " << payloadSize << " bytes";
|
||||
std::cout << hdr.str() << "\n";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
if(!openAndOnline(tester, opt) || !openAndOnline(ecu, opt))
|
||||
return 1;
|
||||
|
||||
Sniffer testerSniff;
|
||||
testerSniff.tag = "TESTER";
|
||||
Sniffer ecuSniff;
|
||||
ecuSniff.tag = "ECU ";
|
||||
addCanSniffer(*tester, testerSniff);
|
||||
addCanSniffer(*ecu, ecuSniff);
|
||||
|
||||
switch(opt.mode) {
|
||||
case Mode::Raw:
|
||||
logLine("Mode --raw: tester 0x123, then ECU 0x456");
|
||||
transmitCan(*tester, 0x123, { 0x11, 0x22, 0x33, 0x44 });
|
||||
std::this_thread::sleep_for(200ms);
|
||||
transmitCan(*ecu, 0x456, { 0x55, 0x66, 0x77, 0x88 });
|
||||
break;
|
||||
case Mode::SingleFrame:
|
||||
logLine("Mode --sf: firmware ISO-TP single frame from tester");
|
||||
if(payload.size() > 7)
|
||||
logLine(" warning: payload > 7 bytes will be a First Frame (no flow control in this mode)");
|
||||
enableIso(*tester, "tester");
|
||||
transmitIso(*tester, opt, payload);
|
||||
break;
|
||||
case Mode::Both:
|
||||
logLine("Mode --both: firmware TX on tester, firmware flow-control on ECU");
|
||||
enableIso(*tester, "tester");
|
||||
enableIso(*ecu, "ECU");
|
||||
receiveIso(*ecu, opt);
|
||||
std::this_thread::sleep_for(100ms);
|
||||
transmitIso(*tester, opt, payload);
|
||||
break;
|
||||
case Mode::None:
|
||||
break;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(opt.listenMs));
|
||||
|
||||
closeDevice(tester);
|
||||
closeDevice(ecu);
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user