Files
MARTe-Integrated-Components/Client/streamhub/Protocol.cpp
T
2026-06-25 00:45:45 +02:00

606 lines
20 KiB
C++

/**
* @file Protocol.cpp
* @brief StreamHub wire protocol implementation.
*/
#include "Protocol.h"
#include <cstring>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <sstream>
namespace StreamHubClient {
/*---------------------------------------------------------------------------*/
/* Binary frame parser */
/*---------------------------------------------------------------------------*/
/* Read a uint32 LE from buf+offset; advance offset by 4. */
static uint32_t readU32(const uint8_t* buf, size_t& off, size_t len) {
if (off + 4 > len) { return 0u; }
uint32_t v = static_cast<uint32_t>(buf[off])
| (static_cast<uint32_t>(buf[off+1]) << 8)
| (static_cast<uint32_t>(buf[off+2]) << 16)
| (static_cast<uint32_t>(buf[off+3]) << 24);
off += 4;
return v;
}
/* Read a uint16 LE from buf+offset; advance offset by 2. */
static uint16_t readU16(const uint8_t* buf, size_t& off, size_t len) {
if (off + 2 > len) { return 0u; }
uint16_t v = static_cast<uint16_t>(buf[off])
| (static_cast<uint16_t>(buf[off+1]) << 8);
off += 2;
return v;
}
/* Read a float64 LE from buf+offset; advance offset by 8. */
static double readF64(const uint8_t* buf, size_t& off, size_t len) {
if (off + 8 > len) { return 0.0; }
double v;
memcpy(&v, buf + off, 8);
off += 8;
return v;
}
bool ParseBinaryFrame(const uint8_t* buf, size_t len, DataFrame& out) {
out.sourceId.clear();
out.signals.clear();
size_t off = 0;
if (off >= len) { return false; }
uint8_t version = buf[off++];
if (version != 1u) { return false; }
if (off >= len) { return false; }
uint8_t idLen = buf[off++];
if (off + idLen + 4 > len) { return false; }
out.sourceId.assign(reinterpret_cast<const char*>(buf + off), idLen);
off += idLen;
uint32_t numSignals = readU32(buf, off, len);
for (uint32_t s = 0; s < numSignals; s++) {
uint16_t keyLen = readU16(buf, off, len);
if (off + keyLen + 4 > len) { return false; }
FrameSignal sig;
sig.name.assign(reinterpret_cast<const char*>(buf + off), keyLen);
off += keyLen;
uint32_t pairCount = readU32(buf, off, len);
if (off + static_cast<size_t>(pairCount) * 16u > len) { return false; }
sig.t.resize(pairCount);
sig.v.resize(pairCount);
for (uint32_t i = 0; i < pairCount; i++) {
sig.t[i] = readF64(buf, off, len);
}
for (uint32_t i = 0; i < pairCount; i++) {
sig.v[i] = readF64(buf, off, len);
}
out.signals.push_back(std::move(sig));
}
return true;
}
bool ParseCaptureFrame(const uint8_t* buf, size_t len, CaptureFrame& out) {
out.signals.clear();
size_t off = 0;
if (off >= len) { return false; }
uint8_t version = buf[off++];
if (version != 2u) { return false; }
if (off + 8u * 3u + 4u > len) { return false; }
out.trigTime = readF64(buf, off, len);
out.preSec = readF64(buf, off, len);
out.postSec = readF64(buf, off, len);
uint32_t numSignals = readU32(buf, off, len);
for (uint32_t s = 0; s < numSignals; s++) {
uint16_t keyLen = readU16(buf, off, len);
if (off + keyLen + 4 > len) { return false; }
CaptureSignal sig;
sig.key.assign(reinterpret_cast<const char*>(buf + off), keyLen);
off += keyLen;
uint32_t pairCount = readU32(buf, off, len);
if (off + static_cast<size_t>(pairCount) * 16u > len) { return false; }
sig.t.resize(pairCount);
sig.v.resize(pairCount);
for (uint32_t i = 0; i < pairCount; i++) {
sig.t[i] = readF64(buf, off, len);
}
for (uint32_t i = 0; i < pairCount; i++) {
sig.v[i] = readF64(buf, off, len);
}
out.signals.push_back(std::move(sig));
}
return true;
}
/*---------------------------------------------------------------------------*/
/* JSON helpers */
/*---------------------------------------------------------------------------*/
static bool jsonGetStr(const char* json, const char* key, char* out, size_t outSz) {
char pat[128];
snprintf(pat, sizeof(pat), "\"%s\":\"", key);
const char* p = strstr(json, pat);
if (!p) { return false; }
p += strlen(pat);
size_t i = 0;
while (*p && *p != '"' && i < outSz - 1) { out[i++] = *p++; }
out[i] = '\0';
return true;
}
static bool jsonGetDouble(const char* json, const char* key, double& out) {
char pat[128];
snprintf(pat, sizeof(pat), "\"%s\":", key);
const char* p = strstr(json, pat);
if (!p) { return false; }
p += strlen(pat);
while (*p == ' ') { p++; }
if (!*p) { return false; }
out = strtod(p, nullptr);
return true;
}
static bool jsonGetUint64(const char* json, const char* key, uint64_t& out) {
double v = 0.0;
if (!jsonGetDouble(json, key, v)) { return false; }
out = static_cast<uint64_t>(v);
return true;
}
static bool jsonGetInt(const char* json, const char* key, int& out) {
double v = 0.0;
if (!jsonGetDouble(json, key, v)) { return false; }
out = static_cast<int>(v);
return true;
}
/*---------------------------------------------------------------------------*/
/* JSON command builders */
/*---------------------------------------------------------------------------*/
std::string BuildPing() { return "{\"type\":\"ping\"}"; }
std::string BuildGetSources() { return "{\"type\":\"getSources\"}"; }
std::string BuildGetStats() { return "{\"type\":\"getStats\"}"; }
std::string BuildGetConfig(const std::string& sourceId) {
char buf[256];
snprintf(buf, sizeof(buf), "{\"type\":\"getConfig\",\"sourceId\":\"%s\"}",
sourceId.c_str());
return buf;
}
std::string BuildAddSource(const std::string& label, const std::string& addr,
const std::string& multicastGroup, uint16_t dataPort) {
char buf[512];
int n = snprintf(buf, sizeof(buf),
"{\"type\":\"addSource\",\"label\":\"%s\",\"addr\":\"%s\"",
label.c_str(), addr.c_str());
if (!multicastGroup.empty() && n > 0 && n < (int)sizeof(buf)) {
n += snprintf(buf + n, sizeof(buf) - n,
",\"multicastGroup\":\"%s\"", multicastGroup.c_str());
}
if (dataPort != 0 && n > 0 && n < (int)sizeof(buf)) {
n += snprintf(buf + n, sizeof(buf) - n,
",\"dataPort\":%u", static_cast<unsigned>(dataPort));
}
if (n > 0 && n < (int)sizeof(buf)) {
snprintf(buf + n, sizeof(buf) - n, "}");
}
return buf;
}
std::string BuildRemoveSource(const std::string& id) {
char buf[256];
snprintf(buf, sizeof(buf), "{\"type\":\"removeSource\",\"id\":\"%s\"}",
id.c_str());
return buf;
}
std::string BuildArm() { return "{\"type\":\"arm\"}"; }
std::string BuildDisarm() { return "{\"type\":\"disarm\"}"; }
std::string BuildRearm() { return "{\"type\":\"rearm\"}"; }
std::string BuildTrigStop(bool stopped) {
char buf[64];
snprintf(buf, sizeof(buf), "{\"type\":\"trigStop\",\"stopped\":%s}",
stopped ? "true" : "false");
return buf;
}
std::string BuildSaveSources() { return "{\"type\":\"saveSources\"}"; }
std::string BuildSetTrigger(const std::string& signalKey, const std::string& edge,
double threshold, double windowSec,
double prePercent, const std::string& mode) {
char buf[512];
snprintf(buf, sizeof(buf),
"{\"type\":\"setTrigger\","
"\"signal\":\"%s\",\"edge\":\"%s\","
"\"threshold\":%.9g,\"windowSec\":%.9g,"
"\"prePercent\":%.9g,\"mode\":\"%s\"}",
signalKey.c_str(), edge.c_str(),
threshold, windowSec, prePercent, mode.c_str());
return buf;
}
std::string BuildZoom(uint32_t reqId, double t0, double t1, int n,
const std::string& signalsCsv) {
char head[256];
snprintf(head, sizeof(head),
"{\"type\":\"zoom\",\"reqId\":%u,"
"\"t0\":%.17g,\"t1\":%.17g,\"n\":%d,\"signals\":\"",
static_cast<unsigned>(reqId), t0, t1, n);
std::string out(head);
out += signalsCsv;
out += "\"}";
return out;
}
std::string BuildHistoryZoom(uint32_t reqId, double t0, double t1, int n,
const std::string& signalsCsv) {
char head[256];
snprintf(head, sizeof(head),
"{\"type\":\"historyZoom\",\"reqId\":%u,"
"\"t0\":%.17g,\"t1\":%.17g,\"n\":%d,\"signals\":\"",
static_cast<unsigned>(reqId), t0, t1, n);
std::string out(head);
out += signalsCsv;
out += "\"}";
return out;
}
std::string BuildHistoryInfo() {
return "{\"type\":\"historyInfo\"}";
}
std::string BuildSetMaxPoints(uint32_t n) {
char buf[128];
snprintf(buf, sizeof(buf), "{\"type\":\"setMaxPoints\",\"maxPoints\":%u}",
static_cast<unsigned>(n));
return buf;
}
/*---------------------------------------------------------------------------*/
/* JSON event parsers */
/*---------------------------------------------------------------------------*/
std::string ParseType(const std::string& json) {
char val[64] = "";
jsonGetStr(json.c_str(), "type", val, sizeof(val));
return val;
}
/* ---- Sources ------------------------------------------------------------ */
bool ParseSources(const std::string& json, std::vector<SourceInfo>& out) {
out.clear();
/* Iterate over source objects in the array */
const char* p = json.c_str();
while ((p = strstr(p, "\"id\":\"")) != nullptr) {
SourceInfo info;
char tmp[256] = "";
jsonGetStr(p - 1, "id", tmp, sizeof(tmp)); info.id = tmp;
jsonGetStr(p - 1, "label", tmp, sizeof(tmp)); info.label = tmp;
jsonGetStr(p - 1, "addr", tmp, sizeof(tmp)); info.addr = tmp;
jsonGetStr(p - 1, "state", tmp, sizeof(tmp)); info.state = tmp;
double portF = 0.0;
jsonGetDouble(p - 1, "port", portF);
info.port = static_cast<uint32_t>(portF);
out.push_back(info);
p++; /* advance past current match */
}
return !out.empty();
}
/* ---- Config ------------------------------------------------------------ */
bool ParseConfig(const std::string& json, std::string& sourceId,
int& publishMode, std::vector<SignalMeta>& signals) {
signals.clear();
char tmp[256] = "";
jsonGetStr(json.c_str(), "sourceId", tmp, sizeof(tmp));
sourceId = tmp;
double pm = 0.0;
jsonGetDouble(json.c_str(), "publishMode", pm);
publishMode = static_cast<int>(pm);
/* Iterate over signal objects in the "signals" array */
const char* p = json.c_str();
while ((p = strstr(p, "\"name\":\"")) != nullptr) {
SignalMeta m;
jsonGetStr(p - 1, "name", tmp, sizeof(tmp)); m.name = tmp;
jsonGetStr(p - 1, "unit", tmp, sizeof(tmp)); m.unit = tmp;
int iv = 0;
jsonGetInt(p - 1, "typeCode", iv); m.typeCode = iv;
jsonGetInt(p - 1, "quantType",iv); m.quantType = iv;
jsonGetInt(p - 1, "timeMode", iv); m.timeMode = iv;
iv = -1;
jsonGetInt(p - 1, "timeSignalIdx", iv); m.timeSignalIdx = iv;
double dv = 0.0;
jsonGetDouble(p - 1, "samplingRate", dv); m.samplingRate = static_cast<float>(dv);
jsonGetDouble(p - 1, "rangeMin", dv); m.rangeMin = dv;
jsonGetDouble(p - 1, "rangeMax", dv); m.rangeMax = dv;
double nr = 1.0, nc = 1.0;
jsonGetDouble(p - 1, "numRows", nr);
jsonGetDouble(p - 1, "numCols", nc);
if (nr < 1.0) { nr = 1.0; }
if (nc < 1.0) { nc = 1.0; }
m.numRows = static_cast<uint32_t>(nr);
m.numCols = static_cast<uint32_t>(nc);
m.numElements = m.numRows * m.numCols;
signals.push_back(m);
p++;
}
return !sourceId.empty();
}
/* ---- Stats ------------------------------------------------------------- */
bool ParseStats(const std::string& json,
std::vector<std::pair<std::string, SourceStats>>& out) {
out.clear();
/* Stats JSON: {"type":"stats","sources":{"id":{ ... },...}} */
/* Parse by finding each nested object key */
const char* sourcesBlock = strstr(json.c_str(), "\"sources\":{");
if (!sourcesBlock) { return false; }
sourcesBlock += strlen("\"sources\":{");
const char* p = sourcesBlock;
while (*p && *p != '}') {
/* Skip to next key */
while (*p && *p != '"') { p++; }
if (!*p || *p != '"') { break; }
p++; /* skip opening quote */
/* Read source id */
char id[128] = "";
size_t i = 0;
while (*p && *p != '"' && i < sizeof(id)-1) { id[i++] = *p++; }
id[i] = '\0';
if (*p == '"') { p++; } /* skip closing quote */
if (*p == ':') { p++; } /* skip colon */
if (*p != '{') { break; }
/* Find end of this source object */
const char* objStart = p;
int depth = 0;
while (*p) {
if (*p == '{') { depth++; }
else if (*p == '}') {
depth--;
if (depth == 0) { p++; break; }
}
p++;
}
/* Parse fields within [objStart, p) */
std::string objStr(objStart, p - objStart);
const char* o = objStr.c_str();
SourceStats st;
char tmp[64] = "";
jsonGetStr(o, "state", tmp, sizeof(tmp)); st.state = tmp;
uint64_t u64 = 0;
jsonGetUint64(o, "totalReceived", u64); st.totalReceived = u64;
jsonGetUint64(o, "totalLost", u64); st.totalLost = u64;
double dv = 0.0;
jsonGetDouble(o, "rateHz", dv); st.rateHz = dv;
jsonGetDouble(o, "rateStdHz", dv); st.rateStdHz = dv;
jsonGetDouble(o, "fragsPerCycle", dv); st.fragsPerCycle = dv;
jsonGetDouble(o, "bytesPerCycle", dv); st.bytesPerCycle = dv;
jsonGetDouble(o, "cycleAvgMs", dv); st.cycleAvgMs = dv;
jsonGetDouble(o, "cycleStdMs", dv); st.cycleStdMs = dv;
jsonGetDouble(o, "cycleMinMs", dv); st.cycleMinMs = dv;
jsonGetDouble(o, "cycleMaxMs", dv); st.cycleMaxMs = dv;
jsonGetDouble(o, "cycleHistMin", dv); st.cycleHistMin = dv;
jsonGetDouble(o, "cycleHistMax", dv); st.cycleHistMax = dv;
/* cycleHist: array of 20 numbers */
const char* hArr = strstr(o, "\"cycleHist\":[");
if (hArr) {
hArr += strlen("\"cycleHist\":[");
for (int hb = 0; hb < 20 && *hArr && *hArr != ']'; hb++) {
while (*hArr == ',' || *hArr == ' ') { hArr++; }
if (*hArr == ']') { break; }
char* end = nullptr;
st.cycleHist[hb] = strtod(hArr, &end);
if (end == hArr) { break; }
hArr = end;
}
}
out.emplace_back(std::string(id), st);
/* Skip comma between source entries */
while (*p == ',' || *p == ' ') { p++; }
}
return !out.empty();
}
/* ---- Trigger state ---------------------------------------------------- */
bool ParseTriggerState(const std::string& json, TriggerStateMsg& out) {
char tmp[64] = "";
if (!jsonGetStr(json.c_str(), "state", tmp, sizeof(tmp))) { return false; }
out.state = tmp;
if (jsonGetStr(json.c_str(), "mode", tmp, sizeof(tmp))) { out.mode = tmp; }
out.stopped = (strstr(json.c_str(), "\"stopped\":true") != nullptr);
double tt = 0.0;
out.hasTrigTime = jsonGetDouble(json.c_str(), "trigTime", tt);
out.trigTime = tt;
return true;
}
/* ---- Zoom response ---------------------------------------------------- */
bool ParseZoom(const std::string& json, ZoomResponse& out) {
out.signals.clear();
out.reqId = 0;
double reqIdF = 0.0;
if (jsonGetDouble(json.c_str(), "reqId", reqIdF)) {
out.reqId = static_cast<uint32_t>(reqIdF);
}
/* Parse signals object: {"src:sig":{"t":[...],"v":[...]},...} */
const char* sigsBlock = strstr(json.c_str(), "\"signals\":{");
if (!sigsBlock) { return false; }
sigsBlock += strlen("\"signals\":{");
const char* p = sigsBlock;
while (*p && *p != '}') {
while (*p && *p != '"') { p++; }
if (!*p) { break; }
p++;
char sigName[128] = "";
size_t i = 0;
while (*p && *p != '"' && i < sizeof(sigName)-1) { sigName[i++] = *p++; }
sigName[i] = '\0';
if (*p == '"') { p++; }
if (*p == ':') { p++; }
ZoomSignal sig;
sig.name = sigName;
/* Parse t array */
const char* tArr = strstr(p, "\"t\":[");
if (tArr) {
tArr += 5;
while (*tArr && *tArr != ']') {
while (*tArr == ',' || *tArr == ' ') { tArr++; }
if (*tArr == ']') { break; }
char* end = nullptr;
double val = strtod(tArr, &end);
if (end == tArr) { break; }
sig.t.push_back(val);
tArr = end;
}
}
/* Parse v array */
const char* vArr = strstr(p, "\"v\":[");
if (vArr) {
vArr += 5;
while (*vArr && *vArr != ']') {
while (*vArr == ',' || *vArr == ' ') { vArr++; }
if (*vArr == ']') { break; }
char* end = nullptr;
double val = strtod(vArr, &end);
if (end == vArr) { break; }
sig.v.push_back(val);
vArr = end;
}
}
out.signals.push_back(std::move(sig));
/* Advance past this signal's object */
int depth = 0;
while (*p) {
if (*p == '{') { depth++; }
else if (*p == '}') {
depth--;
if (depth == 0) { p++; break; }
}
p++;
}
while (*p == ',' || *p == ' ') { p++; }
}
return true;
}
/* ---- maxPointsUpdated ------------------------------------------------- */
bool ParseMaxPointsUpdated(const std::string& json, uint32_t& maxPoints) {
double v = 0.0;
if (!jsonGetDouble(json.c_str(), "maxPoints", v)) { return false; }
maxPoints = static_cast<uint32_t>(v);
return true;
}
/* ---- historyInfo -------------------------------------------------------- */
bool ParseHistoryInfo(const std::string& json, HistoryInfoMsg& out) {
out.signals.clear();
out.enabled = false;
/* "enabled" is a JSON boolean (unquoted true/false), not a string */
out.enabled = (strstr(json.c_str(), "\"enabled\":true") != nullptr);
double df = 0.0;
jsonGetDouble(json.c_str(), "durationHours", df);
out.durationHours = df;
double decF = 0.0;
jsonGetDouble(json.c_str(), "decimation", decF);
out.decimation = static_cast<uint32_t>(decF);
/* Parse "signals":{...} block — same key iteration as zoom */
const char *sig = strstr(json.c_str(), "\"signals\":{");
if (!sig) { return out.enabled; }
sig += 11; /* past '{"signals":{' */
while (*sig) {
/* Find next "key":{ */
const char *q = strchr(sig, '"');
if (!q) { break; }
q++;
const char *qEnd = strchr(q, '"');
if (!qEnd) { break; }
HistorySignalRange hr;
hr.key = std::string(q, qEnd);
/* Find the nested object */
const char *brace = strchr(qEnd, '{');
if (!brace) { break; }
jsonGetDouble(brace, "t0", hr.t0);
jsonGetDouble(brace, "t1", hr.t1);
double cntF = 0.0, capF = 0.0;
jsonGetDouble(brace, "count", cntF);
jsonGetDouble(brace, "capacity", capF);
hr.count = static_cast<uint32_t>(cntF);
hr.capacity = static_cast<uint32_t>(capF);
out.signals.push_back(hr);
/* Advance past the closing brace */
const char *end = strchr(brace, '}');
if (!end) { break; }
sig = end + 1;
}
return true;
}
} /* namespace StreamHubClient */