Mirrors/VILLASnode
1
0
Fork 0
mirror of https://git.rwth-aachen.de/acs/public/villas/node/ synced 2025-03-09 00:00:00 +01:00
Code Issues Releases Wiki Activity
VILLASnode/lib/formats/raw.cpp
Steffen Vogel dc436073a2 Use spaces for indention of C++ comments 
Signed-off-by: Steffen Vogel <post@steffenvogel.de>
2024-02-29 23:18:47 +01:00

538 lines
16 KiB
C++
Raw Permalink Blame History

/* RAW IO format.
*
* Author: Steffen Vogel <post@steffenvogel.de>
* SPDX-FileCopyrightText: 2014-2023 Institute for Automation of Complex Power Systems, RWTH Aachen University
* SPDX-License-Identifier: Apache-2.0
*/
#include <villas/compat.hpp>
#include <villas/exceptions.hpp>
#include <villas/formats/raw.hpp>
#include <villas/sample.hpp>
#include <villas/utils.hpp>
typedef float flt32_t;
typedef double flt64_t;
typedef long double flt128_t; // TODO: check
using namespace villas;
using namespace villas::node;
// Convert double to host byte order
#define SWAP_FLOAT_XTOH(o, b, n) \
({ \
union { \
flt##b##_t f; \
uint##b##_t i; \
} x = {.f = n}; \
x.i = (o) ? be##b##toh(x.i) : le##b##toh(x.i); \
x.f; \
})
// Convert double to big/little endian byte order
#define SWAP_FLOAT_HTOX(o, b, n) \
({ \
union { \
flt##b##_t f; \
uint##b##_t i; \
} x = {.f = n}; \
x.i = (o) ? htobe##b(x.i) : htole##b(x.i); \
x.f; \
})
// Convert integer of varying width to host byte order
#define SWAP_INT_XTOH(o, b, n) (o ? be##b##toh(n) : le##b##toh(n))
// Convert integer of varying width to big/little endian byte order
#define SWAP_INT_HTOX(o, b, n) (o ? htobe##b(n) : htole##b(n))
int RawFormat::sprint(char *buf, size_t len, size_t *wbytes,
const struct Sample *const smps[], unsigned cnt) {
int o = 0;
size_t nlen;
void *vbuf = (char *)buf; // Avoid warning about invalid pointer cast
int8_t *i8 = (int8_t *)vbuf;
int16_t *i16 = (int16_t *)vbuf;
int32_t *i32 = (int32_t *)vbuf;
int64_t *i64 = (int64_t *)vbuf;
float *f32 = (float *)vbuf;
double *f64 = (double *)vbuf;
#ifdef HAS_128BIT
__int128 *i128 = (__int128 *)vbuf;
__float128 *f128 = (__float128 *)vbuf;
#endif
for (unsigned i = 0; i < cnt; i++) {
const struct Sample *smp = smps[i];
/* First three values are sequence, seconds and nano-seconds timestamps
*
* These fields are always encoded as integers!
*/
if (fake) {
// Check length
nlen = (o + 3) * (bits / 8);
if (nlen >= len)
goto out;
switch (bits) {
case 8:
i8[o++] = smp->sequence;
i8[o++] = smp->ts.origin.tv_sec;
i8[o++] = smp->ts.origin.tv_nsec;
break;
case 16:
i16[o++] =
SWAP_INT_HTOX(endianess == Endianess::BIG, 16, smp->sequence);
i16[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 16,
smp->ts.origin.tv_sec);
i16[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 16,
smp->ts.origin.tv_nsec);
break;
case 32:
i32[o++] =
SWAP_INT_HTOX(endianess == Endianess::BIG, 32, smp->sequence);
i32[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 32,
smp->ts.origin.tv_sec);
i32[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 32,
smp->ts.origin.tv_nsec);
break;
case 64:
i64[o++] =
SWAP_INT_HTOX(endianess == Endianess::BIG, 64, smp->sequence);
i64[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 64,
smp->ts.origin.tv_sec);
i64[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 64,
smp->ts.origin.tv_nsec);
break;
#ifdef HAS_128BIT
case 128:
i128[o++] =
SWAP_INT_TO_LE(endianess == Endianess::BIG, 128, smp->sequence);
i128[o++] = SWAP_INT_TO_LE(endianess == Endianess::BIG, 128,
smp->ts.origin.tv_sec);
i128[o++] = SWAP_INT_TO_LE(endianess == Endianess::BIG, 128,
smp->ts.origin.tv_nsec);
break;
#endif
}
}
for (unsigned j = 0; j < smp->length; j++) {
enum SignalType fmt = sample_format(smp, j);
const union SignalData *data = &smp->data[j];
// Check length
nlen = (o + (fmt == SignalType::COMPLEX ? 2 : 1)) * (bits / 8);
if (nlen >= len)
goto out;
switch (fmt) {
case SignalType::FLOAT:
switch (bits) {
case 8:
i8[o++] = -1;
break; // Not supported
case 16:
i16[o++] = -1;
break; // Not supported
case 32:
f32[o++] =
SWAP_FLOAT_HTOX(endianess == Endianess::BIG, 32, (float)data->f);
break;
case 64:
f64[o++] = SWAP_FLOAT_HTOX(endianess == Endianess::BIG, 64, data->f);
break;
#ifdef HAS_128BIT
case 128:
f128[o++] =
SWAP_FLOAT_HTOX(endianess == Endianess::BIG, 128, data->f);
break;
#endif
}
break;
case SignalType::INTEGER:
switch (bits) {
case 8:
i8[o++] = data->i;
break;
case 16:
i16[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 16, data->i);
break;
case 32:
i32[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 32, data->i);
break;
case 64:
i64[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 64, data->i);
break;
#ifdef HAS_128BIT
case 128:
i128[o++] = SWAP_INT_HTOX(endianess == Endianess::BIG, 128, data->i);
break;
#endif
}
break;
case SignalType::BOOLEAN:
switch (bits) {
case 8:
i8[o++] = data->b ? 1 : 0;
break;
case 16:
i16[o++] =
SWAP_INT_HTOX(endianess == Endianess::BIG, 16, data->b ? 1 : 0);
break;
case 32:
i32[o++] =
SWAP_INT_HTOX(endianess == Endianess::BIG, 32, data->b ? 1 : 0);
break;
case 64:
i64[o++] =
SWAP_INT_HTOX(endianess == Endianess::BIG, 64, data->b ? 1 : 0);
break;
#ifdef HAS_128BIT
case 128:
i128[o++] =
SWAP_INT_HTOX(endianess == Endianess::BIG, 128, data->b ? 1 : 0);
break;
#endif
}
break;
case SignalType::COMPLEX:
switch (bits) {
case 8:
i8[o++] = -1; // Not supported
i8[o++] = -1;
break;
case 16:
i16[o++] = -1; // Not supported
i16[o++] = -1;
break;
case 32:
f32[o++] = SWAP_FLOAT_HTOX(endianess == Endianess::BIG, 32,
(float)std::real(data->z));
f32[o++] = SWAP_FLOAT_HTOX(endianess == Endianess::BIG, 32,
(float)std::imag(data->z));
break;
case 64:
f64[o++] = SWAP_FLOAT_HTOX(endianess == Endianess::BIG, 64,
std::real(data->z));
f64[o++] = SWAP_FLOAT_HTOX(endianess == Endianess::BIG, 64,
std::imag(data->z));
break;
#ifdef HAS_128BIT
case 128:
f128[o++] = SWAP_FLOAT_HTOX(endianess == Endianess::BIG, 128,
std::real(data->z));
f128[o++] = SWAP_FLOAT_HTOX(endianess == Endianess::BIG, 128,
std::imag(data->z));
break;
#endif
}
break;
case SignalType::INVALID:
return -1;
}
}
}
out:
if (wbytes)
*wbytes = o * (bits / 8);
return cnt;
}
int RawFormat::sscan(const char *buf, size_t len, size_t *rbytes,
struct Sample *const smps[], unsigned cnt) {
void *vbuf = (void *)buf; // Avoid warning about invalid pointer cast
int8_t *i8 = (int8_t *)vbuf;
int16_t *i16 = (int16_t *)vbuf;
int32_t *i32 = (int32_t *)vbuf;
int64_t *i64 = (int64_t *)vbuf;
float *f32 = (float *)vbuf;
double *f64 = (double *)vbuf;
#ifdef HAS_128BIT
__int128 *i128 = (__int128 *)vbuf;
__float128 *f128 = (__float128 *)vbuf;
#endif
/* The raw format can not encode multiple samples in one buffer
* as there is no support for framing. */
struct Sample *smp = smps[0];
int o = 0;
int nlen = len / (bits / 8);
if (cnt > 1)
return -1;
if (len % (bits / 8))
return -1; // Invalid RAW Payload length
if (fake) {
if (nlen < o + 3)
return -1; // Received a packet with no fake header. Skipping...
switch (bits) {
case 8:
smp->sequence = i8[o++];
smp->ts.origin.tv_sec = i8[o++];
smp->ts.origin.tv_nsec = i8[o++];
break;
case 16:
smp->sequence = SWAP_INT_XTOH(endianess == Endianess::BIG, 16, i16[o++]);
smp->ts.origin.tv_sec =
SWAP_INT_XTOH(endianess == Endianess::BIG, 16, i16[o++]);
smp->ts.origin.tv_nsec =
SWAP_INT_XTOH(endianess == Endianess::BIG, 16, i16[o++]);
break;
case 32:
smp->sequence = SWAP_INT_XTOH(endianess == Endianess::BIG, 32, i32[o++]);
smp->ts.origin.tv_sec =
SWAP_INT_XTOH(endianess == Endianess::BIG, 32, i32[o++]);
smp->ts.origin.tv_nsec =
SWAP_INT_XTOH(endianess == Endianess::BIG, 32, i32[o++]);
break;
case 64:
smp->sequence = SWAP_INT_XTOH(endianess == Endianess::BIG, 64, i64[o++]);
smp->ts.origin.tv_sec =
SWAP_INT_XTOH(endianess == Endianess::BIG, 64, i64[o++]);
smp->ts.origin.tv_nsec =
SWAP_INT_XTOH(endianess == Endianess::BIG, 64, i64[o++]);
break;
#ifdef HAS_128BIT
case 128:
smp->sequence =
SWAP_INT_XTOH(endianess == Endianess::BIG, 128, i128[o++]);
smp->ts.origin.tv_sec =
SWAP_INT_XTOH(endianess == Endianess::BIG, 128, i128[o++]);
smp->ts.origin.tv_nsec =
SWAP_INT_XTOH(endianess == Endianess::BIG, 128, i128[o++]);
break;
#endif
}
smp->flags =
(int)SampleFlags::HAS_SEQUENCE | (int)SampleFlags::HAS_TS_ORIGIN;
} else {
smp->flags = 0;
smp->sequence = 0;
smp->ts.origin.tv_sec = 0;
smp->ts.origin.tv_nsec = 0;
}
smp->signals = signals;
unsigned i;
for (i = 0; i < smp->capacity && o < nlen; i++) {
enum SignalType fmt = sample_format(smp, i);
union SignalData *data = &smp->data[i];
switch (fmt) {
case SignalType::FLOAT:
switch (bits) {
case 8:
data->f = -1;
o++;
break; // Not supported
case 16:
data->f = -1;
o++;
break; // Not supported
case 32:
data->f = SWAP_FLOAT_XTOH(endianess == Endianess::BIG, 32, f32[o++]);
break;
case 64:
data->f = SWAP_FLOAT_XTOH(endianess == Endianess::BIG, 64, f64[o++]);
break;
#ifdef HAS_128BIT
case 128:
data->f = SWAP_FLOAT_XTOH(endianess == Endianess::BIG, 128, f128[o++]);
break;
#endif
}
break;
case SignalType::INTEGER:
switch (bits) {
case 8:
data->i = (int8_t)i8[o++];
break;
case 16:
data->i =
(int16_t)SWAP_INT_XTOH(endianess == Endianess::BIG, 16, i16[o++]);
break;
case 32:
data->i =
(int32_t)SWAP_INT_XTOH(endianess == Endianess::BIG, 32, i32[o++]);
break;
case 64:
data->i =
(int64_t)SWAP_INT_XTOH(endianess == Endianess::BIG, 64, i64[o++]);
break;
#ifdef HAS_128BIT
case 128:
data->i = (__int128)SWAP_INT_XTOH(endianess == Endianess::BIG, 128,
i128[o++]);
break;
#endif
}
break;
case SignalType::BOOLEAN:
switch (bits) {
case 8:
data->b = (bool)i8[o++];
break;
case 16:
data->b =
(bool)SWAP_INT_XTOH(endianess == Endianess::BIG, 16, i16[o++]);
break;
case 32:
data->b =
(bool)SWAP_INT_XTOH(endianess == Endianess::BIG, 32, i32[o++]);
break;
case 64:
data->b =
(bool)SWAP_INT_XTOH(endianess == Endianess::BIG, 64, i64[o++]);
break;
#ifdef HAS_128BIT
case 128:
data->b =
(bool)SWAP_INT_XTOH(endianess == Endianess::BIG, 128, i128[o++]);
break;
#endif
}
break;
case SignalType::COMPLEX:
switch (bits) {
case 8:
data->z = std::complex<float>(-1, -1);
o += 2;
break; // Not supported
case 16:
data->z = std::complex<float>(-1, -1);
o += 2;
break; // Not supported
case 32:
data->z = std::complex<float>(
SWAP_FLOAT_XTOH(endianess == Endianess::BIG, 32, f32[o++]),
SWAP_FLOAT_XTOH(endianess == Endianess::BIG, 32, f32[o++]));
break;
case 64:
data->z = std::complex<float>(
SWAP_FLOAT_XTOH(endianess == Endianess::BIG, 64, f64[o++]),
SWAP_FLOAT_XTOH(endianess == Endianess::BIG, 64, f64[o++]));
break;
#if HAS_128BIT
case 128:
data->z = std::complex<float>(
SWAP_FLOAT_XTOH(endianess == Endianess::BIG, 128, f128[o++]),
SWAP_FLOAT_XTOH(endianess == Endianess::BIG, 128, f128[o++]));
break;
#endif
}
break;
case SignalType::INVALID:
return -1; // Unsupported format in RAW payload
}
}
smp->length = i;
if (smp->length > smp->capacity)
smp->length = smp->capacity; // Received more values than supported
if (rbytes)
*rbytes = o * (bits / 8);
return 1;
}
void RawFormat::parse(json_t *json) {
int ret;
json_error_t err;
int fake_tmp = 0;
const char *end = nullptr;
ret = json_unpack_ex(json, &err, 0, "{ s?: s, s?: b, s?: i }", "endianess",
&end, "fake", &fake_tmp, "bits", &bits);
if (ret)
throw ConfigError(json, err, "node-config-format-raw",
"Failed to parse format configuration");
if (bits % 8 != 0 || bits > 128 || bits <= 0)
throw ConfigError(json, "node-config-format-raw-bits",
"Failed to parse format configuration");
if (end) {
if (bits <= 8)
throw ConfigError(
json, "node-config-format-raw-endianess",
"An endianess settings must only provided for bits > 8");
if (!strcmp(end, "little"))
endianess = Endianess::LITTLE;
else if (!strcmp(end, "big"))
endianess = Endianess::BIG;
else
throw ConfigError(json, "node-config-format-raw-endianess",
"Endianess must be either 'big' or 'little'");
}
fake = fake_tmp;
if (fake)
flags |= (int)SampleFlags::HAS_SEQUENCE | (int)SampleFlags::HAS_TS_ORIGIN;
else
flags &=
~((int)SampleFlags::HAS_SEQUENCE | (int)SampleFlags::HAS_TS_ORIGIN);
BinaryFormat::parse(json);
}
// Register formats
static char n1[] = "raw";
static char d1[] = "Raw binary data";
static FormatPlugin<RawFormat, n1, d1, (int)SampleFlags::HAS_DATA> p1;
static char n2[] = "gtnet";
static char d2[] = "RTDS GTNET";
static FormatPlugin<GtnetRawFormat, n2, d2, (int)SampleFlags::HAS_DATA> p2;
Reference in a new issue View git blame Copy permalink