2016-06-08 22:26:40 +02:00
|
|
|
/* The internal datastructure for a sample of simulation data.
|
|
|
|
*
|
2022-03-15 09:18:01 -04:00
|
|
|
* Author: Steffen Vogel <post@steffenvogel.de>
|
2022-03-15 09:28:57 -04:00
|
|
|
* SPDX-FileCopyrightText: 2014-2023 Institute for Automation of Complex Power Systems, RWTH Aachen University
|
2022-07-04 18:20:03 +02:00
|
|
|
* SPDX-License-Identifier: Apache-2.0
|
2016-06-08 22:26:40 +02:00
|
|
|
*/
|
|
|
|
|
2019-06-23 16:57:00 +02:00
|
|
|
#include <cinttypes>
|
2023-09-07 11:46:39 +02:00
|
|
|
#include <cmath>
|
|
|
|
#include <cstring>
|
2017-04-07 17:46:50 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
#include <villas/colors.hpp>
|
|
|
|
#include <villas/exceptions.hpp>
|
|
|
|
#include <villas/list.hpp>
|
2021-08-10 10:12:48 -04:00
|
|
|
#include <villas/pool.hpp>
|
|
|
|
#include <villas/sample.hpp>
|
|
|
|
#include <villas/signal.hpp>
|
2023-09-07 11:46:39 +02:00
|
|
|
#include <villas/timing.hpp>
|
|
|
|
#include <villas/utils.hpp>
|
2016-06-08 22:26:40 +02:00
|
|
|
|
2020-01-25 17:39:06 +01:00
|
|
|
using namespace villas;
|
2021-08-10 10:12:48 -04:00
|
|
|
using namespace villas::node;
|
2019-06-04 16:55:38 +02:00
|
|
|
using namespace villas::utils;
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_init(struct Sample *s) {
|
|
|
|
struct Pool *p = sample_pool(s);
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
s->length = 0;
|
|
|
|
s->capacity = (p->blocksz - sizeof(struct Sample)) / sizeof(s->data[0]);
|
|
|
|
s->refcnt = ATOMIC_VAR_INIT(1);
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
new (&s->signals) std::shared_ptr<SignalList>;
|
2021-08-10 10:12:48 -04:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return 0;
|
2017-10-16 08:08:17 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
struct Sample *villas::node::sample_alloc(struct Pool *p) {
|
|
|
|
struct Sample *s;
|
2018-04-26 09:32:13 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
s = (struct Sample *)pool_get(p);
|
|
|
|
if (!s)
|
|
|
|
return nullptr;
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
s->pool_off = (char *)p - (char *)s;
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int ret = sample_init(s);
|
|
|
|
if (ret) {
|
|
|
|
pool_put(p, s);
|
|
|
|
return nullptr;
|
|
|
|
}
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return s;
|
2017-10-16 08:08:17 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
struct Sample *villas::node::sample_alloc_mem(int capacity) {
|
|
|
|
size_t sz = SAMPLE_LENGTH(capacity);
|
2018-05-08 11:43:53 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
auto *s = (struct Sample *)new char[sz];
|
|
|
|
if (!s)
|
|
|
|
throw MemoryAllocationError();
|
2020-01-25 17:39:06 +01:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
memset((void *)s, 0, sz);
|
2018-05-08 11:43:53 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
s->pool_off = SAMPLE_NON_POOL;
|
2018-05-08 11:43:53 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
s->length = 0;
|
|
|
|
s->capacity = capacity;
|
|
|
|
s->refcnt = ATOMIC_VAR_INIT(1);
|
2018-05-08 11:43:53 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return s;
|
2018-05-08 11:43:53 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
void villas::node::sample_free(struct Sample *s) {
|
|
|
|
struct Pool *p = sample_pool(s);
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
if (p)
|
|
|
|
pool_put(p, s);
|
|
|
|
else
|
2024-05-27 08:42:13 +02:00
|
|
|
delete[] (char *)s;
|
2017-10-16 08:08:17 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_alloc_many(struct Pool *p, struct Sample *smps[],
|
|
|
|
int cnt) {
|
|
|
|
int ret;
|
2016-09-10 22:16:23 -04:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
ret = pool_get_many(p, (void **)smps, cnt);
|
|
|
|
if (ret < 0)
|
|
|
|
return ret;
|
2016-09-10 22:16:23 -04:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
for (int i = 0; i < ret; i++) {
|
|
|
|
smps[i]->pool_off = (char *)p - (char *)smps[i];
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
sample_init(smps[i]);
|
|
|
|
}
|
2016-09-10 22:16:23 -04:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return ret;
|
2016-09-10 22:16:23 -04:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
void villas::node::sample_free_many(struct Sample *smps[], int cnt) {
|
|
|
|
for (int i = 0; i < cnt; i++)
|
|
|
|
sample_free(smps[i]);
|
2016-10-22 20:47:36 -04:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_decref_many(struct Sample *const smps[], int cnt) {
|
|
|
|
int released = 0;
|
2017-05-05 19:24:16 +00:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
for (int i = 0; i < cnt; i++) {
|
|
|
|
if (sample_decref(smps[i]) == 0)
|
|
|
|
released++;
|
|
|
|
}
|
2017-05-05 19:24:16 +00:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return released;
|
2017-04-07 17:46:50 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_incref_many(struct Sample *const smps[], int cnt) {
|
|
|
|
for (int i = 0; i < cnt; i++)
|
|
|
|
sample_incref(smps[i]);
|
2017-05-05 19:24:16 +00:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return cnt;
|
2017-04-07 17:46:50 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_incref(struct Sample *s) {
|
|
|
|
return atomic_fetch_add(&s->refcnt, 1) + 1;
|
2016-08-28 23:57:11 -04:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_decref(struct Sample *s) {
|
|
|
|
int prev = atomic_fetch_sub(&s->refcnt, 1);
|
2017-05-05 19:24:16 +00:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
// Did we had the last reference?
|
|
|
|
if (prev == 1)
|
|
|
|
sample_free(s);
|
2017-05-05 19:24:16 +00:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return prev - 1;
|
2016-08-28 23:57:11 -04:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_copy(struct Sample *dst, const struct Sample *src) {
|
|
|
|
dst->length = MIN(src->length, dst->capacity);
|
2017-04-07 17:46:50 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
dst->sequence = src->sequence;
|
|
|
|
dst->flags = src->flags;
|
|
|
|
dst->ts = src->ts;
|
|
|
|
dst->signals = src->signals;
|
2017-04-07 17:46:50 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
memcpy(&dst->data, &src->data, SAMPLE_DATA_LENGTH(dst->length));
|
2017-05-05 19:24:16 +00:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return 0;
|
2017-04-07 17:46:50 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
struct Sample *villas::node::sample_clone(struct Sample *orig) {
|
|
|
|
struct Sample *clone;
|
|
|
|
struct Pool *pool;
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
pool = sample_pool(orig);
|
|
|
|
if (!pool)
|
|
|
|
return nullptr;
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
clone = sample_alloc(pool);
|
|
|
|
if (!clone)
|
|
|
|
return nullptr;
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
sample_copy(clone, orig);
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return clone;
|
2017-10-16 08:08:17 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_clone_many(struct Sample *dsts[],
|
|
|
|
const struct Sample *const srcs[],
|
|
|
|
int cnt) {
|
|
|
|
int alloced, copied;
|
|
|
|
struct Pool *pool;
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
if (cnt <= 0)
|
|
|
|
return 0;
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
pool = sample_pool(srcs[0]);
|
|
|
|
if (!pool)
|
|
|
|
return 0;
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
alloced = sample_alloc_many(pool, dsts, cnt);
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
copied = sample_copy_many(dsts, srcs, alloced);
|
2017-10-16 08:08:17 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return copied;
|
2017-10-16 08:08:17 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_copy_many(struct Sample *const dsts[],
|
|
|
|
const struct Sample *const srcs[], int cnt) {
|
|
|
|
for (int i = 0; i < cnt; i++)
|
|
|
|
sample_copy(dsts[i], srcs[i]);
|
2017-05-05 19:24:16 +00:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return cnt;
|
2017-04-07 17:46:50 +02:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
int villas::node::sample_cmp(struct Sample *a, struct Sample *b, double epsilon,
|
|
|
|
int flags) {
|
|
|
|
if ((a->flags & b->flags & flags) != flags) {
|
|
|
|
printf("flags: a=%#x, b=%#x, wanted=%#x\n", a->flags, b->flags, flags);
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Compare sequence no
|
|
|
|
if (flags & (int)SampleFlags::HAS_SEQUENCE) {
|
|
|
|
if (a->sequence != b->sequence) {
|
|
|
|
printf("sequence no: %" PRIu64 " != %" PRIu64 "\n", a->sequence,
|
|
|
|
b->sequence);
|
|
|
|
return 2;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Compare timestamp
|
|
|
|
if (flags & (int)SampleFlags::HAS_TS_ORIGIN) {
|
|
|
|
if (time_delta(&a->ts.origin, &b->ts.origin) > epsilon) {
|
|
|
|
printf("ts.origin: %f != %f\n", time_to_double(&a->ts.origin),
|
|
|
|
time_to_double(&b->ts.origin));
|
|
|
|
return 3;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Compare data
|
|
|
|
if (flags & (int)SampleFlags::HAS_DATA) {
|
|
|
|
if (a->length != b->length) {
|
|
|
|
printf("length: %u != %u\n", a->length, b->length);
|
|
|
|
return 4;
|
|
|
|
}
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < a->length; i++) {
|
|
|
|
// Compare format
|
|
|
|
if (sample_format(a, i) != sample_format(b, i))
|
|
|
|
return 6;
|
|
|
|
|
|
|
|
switch (sample_format(a, i)) {
|
|
|
|
case SignalType::FLOAT:
|
|
|
|
if (fabs(a->data[i].f - b->data[i].f) > epsilon) {
|
|
|
|
printf("data[%u].f: %f != %f\n", i, a->data[i].f, b->data[i].f);
|
|
|
|
return 5;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
case SignalType::INTEGER:
|
|
|
|
if (a->data[i].i != b->data[i].i) {
|
|
|
|
printf("data[%u].i: %" PRId64 " != %" PRId64 "\n", i, a->data[i].i,
|
|
|
|
b->data[i].i);
|
|
|
|
return 5;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
case SignalType::BOOLEAN:
|
|
|
|
if (a->data[i].b != b->data[i].b) {
|
|
|
|
printf("data[%u].b: %s != %s\n", i, a->data[i].b ? "true" : "false",
|
|
|
|
b->data[i].b ? "true" : "false");
|
|
|
|
return 5;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
case SignalType::COMPLEX:
|
|
|
|
if (std::abs(a->data[i].z - b->data[i].z) > epsilon) {
|
|
|
|
printf("data[%u].z: %f+%fi != %f+%fi\n", i, std::real(a->data[i].z),
|
|
|
|
std::imag(a->data[i].z), std::real(b->data[i].z),
|
|
|
|
std::imag(b->data[i].z));
|
|
|
|
return 5;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
default: {
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
2017-09-04 16:19:43 +02:00
|
|
|
}
|
2018-08-06 11:26:25 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
enum SignalType villas::node::sample_format(const struct Sample *s,
|
|
|
|
unsigned idx) {
|
|
|
|
auto sig = s->signals->getByIndex(idx);
|
2018-08-06 11:26:25 +02:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
return sig ? sig->type : SignalType::INVALID;
|
2018-08-06 11:26:25 +02:00
|
|
|
}
|
2019-01-23 00:49:28 +01:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
void villas::node::sample_dump(Logger logger, struct Sample *s) {
|
|
|
|
logger->info("Sample: sequence={}, length={}, capacity={}, "
|
|
|
|
"flags={:#x}, #signals={}, "
|
|
|
|
"refcnt={}, pool_off={:#x}",
|
|
|
|
s->sequence, s->length, s->capacity, s->flags,
|
|
|
|
s->signals ? s->signals->size() : -1, atomic_load(&s->refcnt),
|
|
|
|
s->pool_off);
|
|
|
|
|
|
|
|
if (s->flags & (int)SampleFlags::HAS_TS_ORIGIN)
|
|
|
|
logger->info(" ts.origin={}.{:09d}", s->ts.origin.tv_sec,
|
|
|
|
s->ts.origin.tv_nsec);
|
|
|
|
|
|
|
|
if (s->flags & (int)SampleFlags::HAS_TS_RECEIVED)
|
|
|
|
logger->info(" ts.received={}.{:09d}", s->ts.received.tv_sec,
|
|
|
|
s->ts.received.tv_nsec);
|
|
|
|
|
|
|
|
if (s->signals) {
|
|
|
|
logger->info(" Signals:");
|
|
|
|
s->signals->dump(logger, s->data, s->length);
|
|
|
|
}
|
2019-01-23 00:49:28 +01:00
|
|
|
}
|
2019-03-08 15:21:46 +01:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
void villas::node::sample_data_insert(struct Sample *smp,
|
|
|
|
const union SignalData *src,
|
|
|
|
size_t offset, size_t len) {
|
|
|
|
memmove(&smp->data[offset + len], &smp->data[offset],
|
|
|
|
sizeof(smp->data[0]) * (smp->length - offset));
|
|
|
|
memcpy(&smp->data[offset], src, sizeof(smp->data[0]) * len);
|
2019-03-08 15:21:46 +01:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
smp->length += len;
|
2019-03-08 15:21:46 +01:00
|
|
|
}
|
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
void villas::node::sample_data_remove(struct Sample *smp, size_t offset,
|
|
|
|
size_t len) {
|
|
|
|
size_t sz = sizeof(smp->data[0]) * len;
|
2019-03-08 15:21:46 +01:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
memmove(&smp->data[offset], &smp->data[offset + len], sz);
|
2019-03-08 15:21:46 +01:00
|
|
|
|
2023-09-07 11:46:39 +02:00
|
|
|
smp->length -= len;
|
2019-03-08 15:21:46 +01:00
|
|
|
}
|