942 lines
32 KiB
C
Executable file
942 lines
32 KiB
C
Executable file
/*
|
|
* Copyright(c) 2006 to 2018 ADLINK Technology Limited and others
|
|
*
|
|
* This program and the accompanying materials are made available under the
|
|
* terms of the Eclipse Public License v. 2.0 which is available at
|
|
* http://www.eclipse.org/legal/epl-2.0, or the Eclipse Distribution License
|
|
* v. 1.0 which is available at
|
|
* http://www.eclipse.org/org/documents/edl-v10.php.
|
|
*
|
|
* SPDX-License-Identifier: EPL-2.0 OR BSD-3-Clause
|
|
*/
|
|
#include <time.h>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <signal.h>
|
|
#include <stdarg.h>
|
|
#include <math.h>
|
|
|
|
#include "testtype.h"
|
|
#include "common.h"
|
|
#include "os/os.h"
|
|
|
|
dds_entity_t dp = 0;
|
|
dds_entity_t qosprov = 0;
|
|
const dds_topic_descriptor_t *ts_KeyedSeq;
|
|
const dds_topic_descriptor_t *ts_Keyed32;
|
|
const dds_topic_descriptor_t *ts_Keyed64;
|
|
const dds_topic_descriptor_t *ts_Keyed128;
|
|
const dds_topic_descriptor_t *ts_Keyed256;
|
|
const dds_topic_descriptor_t *ts_OneULong;
|
|
|
|
const char *saved_argv0;
|
|
|
|
//void nowll_as_ddstime(DDS_Time_t *t) {
|
|
// os_time ost = os_timeGet();
|
|
// t->sec = ost.tv_sec;
|
|
// t->nanosec = (DDS_unsigned_long) ost.tv_nsec;
|
|
//}
|
|
//
|
|
//void bindelta(unsigned long long *bins, unsigned long long d, unsigned repeat) {
|
|
// int bin = 0;
|
|
// while (d) {
|
|
// bin++;
|
|
// d >>= 1;
|
|
// }
|
|
// bins[bin] += repeat;
|
|
//}
|
|
//
|
|
//void binprint(unsigned long long *bins, unsigned long long telapsed) {
|
|
// unsigned long long n;
|
|
// unsigned i, minbin = BINS_LENGTH-1, maxbin = 0;
|
|
// n = 0;
|
|
// for (i = 0; i < BINS_LENGTH; i++) {
|
|
// n += bins[i];
|
|
// if (bins[i] && i < minbin)
|
|
// minbin = i;
|
|
// if (bins[i] && i > maxbin)
|
|
// maxbin = i;
|
|
// }
|
|
// printf ("< 2**n | %llu in %.06fs avg %.1f/s\n", n, telapsed * 1e-9, n / (telapsed * 1e-9));
|
|
// for (i = minbin; i <= maxbin; i++) {
|
|
// static const char ats[] = "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@";
|
|
// double pct = 100.0 * (double) bins[i] / n;
|
|
// int nats = (int) ((pct / 100.0) * (sizeof(ats) - 1));
|
|
// printf ("%2d: %6.2f%% %*.*s\n", i, pct, nats, nats, ats);
|
|
// }
|
|
//}
|
|
|
|
struct hist {
|
|
unsigned nbins;
|
|
uint64_t binwidth;
|
|
uint64_t bin0; /* bins are [bin0,bin0+binwidth),[bin0+binwidth,bin0+2*binwidth) */
|
|
uint64_t binN; /* bin0 + nbins*binwidth */
|
|
uint64_t min, max; /* min and max observed since last reset */
|
|
uint64_t under, over; /* < bin0, >= binN */
|
|
uint64_t bins[];
|
|
};
|
|
|
|
struct hist *hist_new(unsigned nbins, uint64_t binwidth, uint64_t bin0) {
|
|
struct hist *h = dds_alloc(sizeof(*h) + nbins * sizeof(*h->bins));
|
|
h->nbins = nbins;
|
|
h->binwidth = binwidth;
|
|
h->bin0 = bin0;
|
|
h->binN = h->bin0 + h->nbins * h->binwidth;
|
|
hist_reset(h);
|
|
return h;
|
|
}
|
|
|
|
void hist_free(struct hist *h) {
|
|
dds_free(h);
|
|
}
|
|
|
|
void hist_reset_minmax(struct hist *h) {
|
|
h->min = UINT64_MAX;
|
|
h->max = 0;
|
|
}
|
|
|
|
void hist_reset(struct hist *h) {
|
|
hist_reset_minmax(h);
|
|
h->under = 0;
|
|
h->over = 0;
|
|
memset(h->bins, 0, h->nbins * sizeof(*h->bins));
|
|
}
|
|
|
|
void hist_record(struct hist *h, uint64_t x, unsigned weight) {
|
|
if (x < h->min)
|
|
h->min = x;
|
|
if (x > h->max)
|
|
h->max = x;
|
|
if (x < h->bin0)
|
|
h->under += weight;
|
|
else if (x >= h->binN)
|
|
h->over += weight;
|
|
else
|
|
h->bins[(x - h->bin0) / h->binwidth] += weight;
|
|
}
|
|
|
|
static void xsnprintf(char *buf, size_t bufsz, size_t *p, const char *fmt, ...) {
|
|
if (*p < bufsz) {
|
|
int n;
|
|
va_list ap;
|
|
va_start(ap, fmt);
|
|
n = os_vsnprintf(buf + *p, bufsz - *p, fmt, ap);
|
|
va_end(ap);
|
|
*p += (size_t)n;
|
|
}
|
|
}
|
|
|
|
void hist_print(struct hist *h, dds_time_t dt, int reset) {
|
|
const size_t l_size = sizeof(char) * h->nbins + 200;
|
|
const size_t hist_size = sizeof(char) * h->nbins + 1;
|
|
char *l = (char *) dds_alloc(l_size);
|
|
char *hist = (char *) dds_alloc(hist_size);
|
|
double dt_s = (double)dt / 1e9, avg;
|
|
uint64_t peak = 0, cnt = h->under + h->over;
|
|
size_t p = 0;
|
|
hist[h->nbins] = 0;
|
|
for (unsigned i = 0; i < h->nbins; i++) {
|
|
cnt += h->bins[i];
|
|
if (h->bins[i] > peak)
|
|
peak = h->bins[i];
|
|
}
|
|
|
|
const uint64_t p1 = peak / 100;
|
|
const uint64_t p10 = peak / 10;
|
|
const uint64_t p20 = 1 * peak / 5;
|
|
const uint64_t p40 = 2 * peak / 5;
|
|
const uint64_t p60 = 3 * peak / 5;
|
|
const uint64_t p80 = 4 * peak / 5;
|
|
for (unsigned i = 0; i < h->nbins; i++) {
|
|
if (h->bins[i] == 0) hist[i] = ' ';
|
|
else if (h->bins[i] <= p1) hist[i] = '.';
|
|
else if (h->bins[i] <= p10) hist[i] = '_';
|
|
else if (h->bins[i] <= p20) hist[i] = '-';
|
|
else if (h->bins[i] <= p40) hist[i] = '=';
|
|
else if (h->bins[i] <= p60) hist[i] = 'x';
|
|
else if (h->bins[i] <= p80) hist[i] = 'X';
|
|
else hist[i] = '@';
|
|
}
|
|
|
|
avg = (double)cnt / dt_s;
|
|
if (avg < 999.5)
|
|
xsnprintf(l, l_size, &p, "%5.3g", avg);
|
|
else if (avg < 1e6)
|
|
xsnprintf(l, l_size, &p, "%4.3gk", avg / 1e3);
|
|
else
|
|
xsnprintf(l, l_size, &p, "%4.3gM", avg / 1e6);
|
|
xsnprintf(l, l_size, &p, "/s (");
|
|
|
|
if (cnt < (uint64_t) 10e3)
|
|
xsnprintf(l, l_size, &p, "%5"PRIu64" ", cnt);
|
|
else if (cnt < (uint64_t) 1e6)
|
|
xsnprintf(l, l_size, &p, "%5.1fk", (double)cnt / 1e3);
|
|
else
|
|
xsnprintf(l, l_size, &p, "%5.1fM", (double)cnt / 1e6);
|
|
|
|
xsnprintf(l, l_size, &p, " in %.1fs) ", dt_s);
|
|
|
|
if (h->min == UINT64_MAX)
|
|
xsnprintf(l, l_size, &p, " inf ");
|
|
else if (h->min < 1000)
|
|
xsnprintf(l, l_size, &p, "%3"PRIu64"n ", h->min);
|
|
else if (h->min + 500 < 1000000)
|
|
xsnprintf(l, l_size, &p, "%3"PRIu64"u ", (h->min + 500) / 1000);
|
|
else if (h->min + 500000 < 1000000000)
|
|
xsnprintf(l, l_size, &p, "%3"PRIu64"m ", (h->min + 500000) / 1000000);
|
|
else
|
|
xsnprintf(l, l_size, &p, "%3"PRIu64"s ", (h->min + 500000000) / 1000000000);
|
|
|
|
if (h->bin0 > 0) {
|
|
int pct = (cnt == 0) ? 0 : 100 * (int) ((h->under + cnt/2) / cnt);
|
|
xsnprintf(l, l_size, &p, "%3d%% ", pct);
|
|
}
|
|
|
|
{
|
|
int pct = (cnt == 0) ? 0 : 100 * (int) ((h->over + cnt/2) / cnt);
|
|
xsnprintf(l, l_size, &p, "|%s| %3d%%", hist, pct);
|
|
}
|
|
|
|
if (h->max < 1000)
|
|
xsnprintf(l, l_size, &p, " %3"PRIu64"n", h->max);
|
|
else if (h->max + 500 < 1000000)
|
|
xsnprintf(l, l_size, &p, " %3"PRIu64"u", (h->max + 500) / 1000);
|
|
else if (h->max + 500000 < 1000000000)
|
|
xsnprintf(l, l_size, &p, " %3"PRIu64"m", (h->max + 500000) / 1000000);
|
|
else
|
|
xsnprintf(l, l_size, &p, " %3"PRIu64"s", (h->max + 500000000) / 1000000000);
|
|
|
|
(void) p;
|
|
puts(l);
|
|
dds_free(l);
|
|
dds_free(hist);
|
|
if (reset)
|
|
hist_reset(h);
|
|
}
|
|
|
|
void error(const char *fmt, ...) {
|
|
va_list ap;
|
|
fprintf (stderr, "%s: error: ", saved_argv0);
|
|
va_start(ap, fmt);
|
|
vfprintf (stderr, fmt, ap);
|
|
va_end(ap);
|
|
fprintf (stderr, "\n");
|
|
}
|
|
|
|
void save_argv0(const char *argv0) {
|
|
saved_argv0 = argv0;
|
|
}
|
|
|
|
int common_init(const char *argv0) {
|
|
save_argv0(argv0);
|
|
dp = dds_create_participant(DDS_DOMAIN_DEFAULT, NULL, NULL);
|
|
error_abort(dp, "dds_create_participant failed");
|
|
|
|
ts_KeyedSeq = &KeyedSeq_desc;
|
|
ts_Keyed32 = &Keyed32_desc;
|
|
ts_Keyed64 = &Keyed64_desc;
|
|
ts_Keyed128 = &Keyed128_desc;
|
|
ts_Keyed256 = &Keyed256_desc;
|
|
ts_OneULong = &OneULong_desc;
|
|
return 0;
|
|
}
|
|
|
|
void common_fini(void) {
|
|
dds_return_t rc;
|
|
if (qosprov != 0) {
|
|
rc = dds_delete(qosprov);
|
|
error_report(rc, "dds_delete qosprov failed");
|
|
}
|
|
rc = dds_delete(dp);
|
|
error_report(rc, "dds_delete participant failed");
|
|
}
|
|
|
|
int change_publisher_partitions(dds_entity_t pub, unsigned npartitions, const char *partitions[]) {
|
|
dds_qos_t *qos;
|
|
dds_return_t rc;
|
|
|
|
qos = dds_qos_create();
|
|
rc = dds_get_qos(pub, qos);
|
|
if (rc == DDS_SUCCESS) {
|
|
dds_qset_partition(qos, npartitions, partitions);
|
|
rc = dds_set_qos(pub, qos);
|
|
}
|
|
dds_qos_delete(qos);
|
|
return rc;
|
|
}
|
|
|
|
int change_subscriber_partitions(dds_entity_t sub, unsigned npartitions, const char *partitions[]) {
|
|
dds_qos_t *qos;
|
|
dds_return_t rc;
|
|
|
|
qos = dds_qos_create();
|
|
rc = dds_get_qos(sub, qos);
|
|
if (rc == DDS_SUCCESS) {
|
|
dds_qset_partition(qos, npartitions, partitions);
|
|
rc = dds_set_qos(sub, qos);
|
|
}
|
|
dds_qos_delete(qos);
|
|
return rc;
|
|
}
|
|
|
|
static dds_qos_t *get_topic_qos(dds_entity_t tp) {
|
|
dds_qos_t *tQos = dds_qos_create();
|
|
dds_return_t rc = dds_get_qos(tp, tQos);
|
|
error_abort(rc, "dds_qos_get_topic_qos");
|
|
return tQos;
|
|
}
|
|
|
|
dds_qos_t *new_tqos(void) {
|
|
dds_qos_t *q = dds_qos_create();
|
|
|
|
/* Not all defaults are those of DCPS: */
|
|
dds_qset_reliability(q, DDS_RELIABILITY_RELIABLE, DDS_SECS(1));
|
|
dds_qset_destination_order(q, DDS_DESTINATIONORDER_BY_SOURCE_TIMESTAMP);
|
|
return q;
|
|
}
|
|
|
|
dds_qos_t *new_rdqos(dds_entity_t tp) {
|
|
dds_qos_t *tQos = get_topic_qos(tp);
|
|
dds_qos_t *qos = dds_qos_create();
|
|
|
|
dds_return_t rc = dds_qos_copy(qos, tQos);
|
|
error_abort(rc ,"new_rdqos: dds_qos_copy");
|
|
dds_qos_delete(tQos);
|
|
return qos;
|
|
}
|
|
|
|
dds_qos_t *new_wrqos(dds_entity_t tp) {
|
|
dds_qos_t *tQos = get_topic_qos(tp);
|
|
dds_qos_t *qos = dds_qos_create();
|
|
|
|
dds_return_t rc = dds_qos_copy(qos, tQos);
|
|
error_abort(rc ,"new_wrqos: dds_qos_copy");
|
|
dds_qos_delete(tQos);
|
|
|
|
/* Not all defaults are those of DCPS: */
|
|
dds_qset_writer_data_lifecycle(qos, false);
|
|
return qos;
|
|
}
|
|
|
|
dds_entity_t new_topic(const char *name, const dds_topic_descriptor_t *topicDesc, const dds_qos_t *q) {
|
|
dds_entity_t tp = dds_create_topic(dp, topicDesc, name, q, NULL);
|
|
error_abort(tp, "dds_create_topic failed");
|
|
return tp;
|
|
}
|
|
|
|
dds_entity_t new_publisher(dds_qos_t *q, unsigned npartitions, const char **partitions) {
|
|
dds_qos_t *pQos;
|
|
if (q == NULL) {
|
|
pQos = dds_qos_create();
|
|
} else {
|
|
pQos = q;
|
|
}
|
|
dds_qset_partition(pQos, npartitions, partitions);
|
|
dds_entity_t pub = dds_create_publisher(dp, pQos, NULL);
|
|
error_abort(pub, "new_publisher: dds_create_publisher");
|
|
if (q == NULL)
|
|
dds_qos_delete(pQos);
|
|
return pub;
|
|
}
|
|
|
|
dds_entity_t new_subscriber(dds_qos_t *q, unsigned npartitions, const char **partitions) {
|
|
dds_qos_t *sQos;
|
|
if (q == NULL) {
|
|
sQos = dds_qos_create();
|
|
} else {
|
|
sQos = q;
|
|
}
|
|
dds_qset_partition(sQos, npartitions, partitions);
|
|
dds_entity_t sub = dds_create_subscriber(dp, sQos, NULL);
|
|
error_abort(sub, "new_subscriber: dds_create_subscriber");
|
|
if (q == NULL)
|
|
dds_qos_delete(sQos);
|
|
return sub;
|
|
}
|
|
|
|
dds_entity_t new_datawriter_listener(const dds_entity_t pub, const dds_entity_t tp, const dds_qos_t *q, const dds_listener_t *l) {
|
|
dds_entity_t wr = dds_create_writer(pub, tp, q, l);
|
|
error_abort(wr, "dds_create_writer failed");
|
|
return wr;
|
|
}
|
|
|
|
dds_entity_t new_datawriter(const dds_entity_t pub, const dds_entity_t tp, const dds_qos_t *q) {
|
|
return new_datawriter_listener(pub, tp, q, NULL);
|
|
}
|
|
|
|
dds_entity_t new_datareader_listener(const dds_entity_t sub, const dds_entity_t tp, const dds_qos_t *q, const dds_listener_t *l) {
|
|
dds_entity_t rd = dds_create_reader(sub, tp, q, l);
|
|
error_abort(rd, "dds_create_reader failed");
|
|
return rd;
|
|
}
|
|
|
|
dds_entity_t new_datareader(const dds_entity_t sub, const dds_entity_t tp, const dds_qos_t *q) {
|
|
return new_datareader_listener(sub, tp, q, NULL);
|
|
}
|
|
|
|
static void inapplicable_qos(dds_entity_kind_t qt, const char *n) {
|
|
const char *en = "?";
|
|
switch (qt) {
|
|
case DDS_KIND_TOPIC: en = "topic"; break;
|
|
case DDS_KIND_PUBLISHER: en = "publisher"; break;
|
|
case DDS_KIND_SUBSCRIBER: en = "subscriber"; break;
|
|
case DDS_KIND_WRITER: en = "writer"; break;
|
|
case DDS_KIND_READER: en = "reader"; break;
|
|
case DDS_KIND_DONTCARE: en = "dontcare"; break;
|
|
case DDS_KIND_PARTICIPANT: en = "participant"; break;
|
|
case DDS_KIND_COND_READ: en = "cond read"; break;
|
|
case DDS_KIND_COND_QUERY: en = "cond query"; break;
|
|
case DDS_KIND_WAITSET: en = "waitset"; break;
|
|
case DDS_KIND_INTERNAL: en = "internal"; break;
|
|
default: en = "?"; break;
|
|
}
|
|
fprintf(stderr, "warning: %s entity ignoring inapplicable QoS \"%s\"\n", en, n);
|
|
}
|
|
|
|
#define get_qos_T(qt, q, n) ((qt == DDS_KIND_TOPIC) ? q : (inapplicable_qos((qt), n), (dds_qos_t*)0))
|
|
#define get_qos_R(qt, q, n) ((qt == DDS_KIND_READER) ? q : (inapplicable_qos((qt), n), (dds_qos_t*)0))
|
|
#define get_qos_W(qt, q, n) ((qt == DDS_KIND_WRITER) ? q : (inapplicable_qos((qt), n), (dds_qos_t*)0))
|
|
#define get_qos_TW(qt, q, n) ((qt == DDS_KIND_TOPIC) || (qt == DDS_KIND_WRITER) ? q : (inapplicable_qos((qt), n), (dds_qos_t*)0))
|
|
#define get_qos_RW(qt, q, n) ((qt == DDS_KIND_READER) || (qt == DDS_KIND_WRITER) ? q : (inapplicable_qos((qt), n), (dds_qos_t*)0))
|
|
#define get_qos_PS(qt, q, n) ((qt == DDS_KIND_PUBLISHER) || (qt == DDS_KIND_SUBSCRIBER) ? q : (inapplicable_qos((qt), n), (dds_qos_t*)0))
|
|
#define get_qos_TRW(qt, q, n) ((qt == DDS_KIND_TOPIC) || (qt == DDS_KIND_READER) || (qt == DDS_KIND_WRITER) ? q : (inapplicable_qos((qt), n), (dds_qos_t*)0))
|
|
|
|
void qos_durability(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_TRW(qt, q, "durability");
|
|
if (qp == NULL)
|
|
return;
|
|
if (strcmp(arg, "v") == 0)
|
|
dds_qset_durability(qp, DDS_DURABILITY_VOLATILE);
|
|
else if (strcmp(arg, "tl") == 0)
|
|
dds_qset_durability(qp, DDS_DURABILITY_TRANSIENT_LOCAL);
|
|
else if (strcmp(arg, "t") == 0)
|
|
dds_qset_durability(qp, DDS_DURABILITY_TRANSIENT);
|
|
else if (strcmp(arg, "p") == 0)
|
|
dds_qset_durability(qp, DDS_DURABILITY_PERSISTENT);
|
|
else
|
|
error_exit("durability qos: %s: invalid\n", arg);
|
|
}
|
|
|
|
void qos_history(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_TRW(qt, q, "history");
|
|
int hist_depth, pos;
|
|
if (qp == NULL)
|
|
return;
|
|
if (strcmp(arg, "all") == 0) {
|
|
dds_qset_history(qp, DDS_HISTORY_KEEP_ALL, DDS_LENGTH_UNLIMITED);
|
|
} else if (sscanf(arg, "%d%n", &hist_depth, &pos) == 1 && arg[pos] == 0 && hist_depth > 0) {
|
|
dds_qset_history(qp, DDS_HISTORY_KEEP_LAST, hist_depth);
|
|
} else {
|
|
error_exit("history qos: %s: invalid\n", arg);
|
|
}
|
|
}
|
|
|
|
void qos_destination_order(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_TRW(qt, q, "destination_order");
|
|
if (qp == NULL)
|
|
return;
|
|
if (strcmp(arg, "r") == 0) {
|
|
dds_qset_destination_order(qp, DDS_DESTINATIONORDER_BY_RECEPTION_TIMESTAMP);
|
|
} else if (strcmp(arg, "s") == 0) {
|
|
dds_qset_destination_order(qp, DDS_DESTINATIONORDER_BY_SOURCE_TIMESTAMP);
|
|
} else {
|
|
error_exit("destination order qos: %s: invalid\n", arg);
|
|
}
|
|
}
|
|
|
|
void qos_ownership(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_TRW(qt, q, "ownership");
|
|
int strength, pos;
|
|
if (qp == NULL)
|
|
return;
|
|
if (strcmp(arg, "s") == 0) {
|
|
dds_qset_ownership(qp, DDS_OWNERSHIP_SHARED);
|
|
} else if (strcmp(arg, "x") == 0) {
|
|
dds_qset_ownership(qp, DDS_OWNERSHIP_EXCLUSIVE);
|
|
} else if (sscanf(arg, "x:%d%n", &strength, &pos) == 1 && arg[pos] == 0) {
|
|
dds_qos_t *qps = get_qos_W(qt, q, "ownership_strength");
|
|
dds_qset_ownership(qp, DDS_OWNERSHIP_EXCLUSIVE);
|
|
if(qps) {
|
|
dds_qset_ownership_strength(qps, strength);
|
|
}
|
|
} else {
|
|
error_exit("ownership qos: %s invalid\n", arg);
|
|
}
|
|
}
|
|
|
|
void qos_transport_priority(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_W(qt, q, "transport_priority");
|
|
int pos;
|
|
int value;
|
|
if (qp == NULL)
|
|
return;
|
|
if (sscanf(arg, "%d%n", &value, &pos) != 1 || arg[pos] != 0)
|
|
error_exit("transport_priority qos: %s invalid\n", arg);
|
|
dds_qset_transport_priority(qp, value);
|
|
}
|
|
|
|
static unsigned char gethexchar(const char **str) {
|
|
unsigned char v = 0;
|
|
int empty = 1;
|
|
while (**str) {
|
|
switch (**str) {
|
|
case '0': case '1': case '2': case '3': case '4':
|
|
case '5': case '6': case '7': case '8': case '9':
|
|
v = (unsigned char) (16 * v + (unsigned char) **str - '0');
|
|
(*str)++;
|
|
break;
|
|
case 'a': case 'b': case 'c': case 'd': case 'e': case 'f':
|
|
v = (unsigned char) (16 * v + (unsigned char) **str - 'a' + 10);
|
|
(*str)++;
|
|
break;
|
|
case 'A': case 'B': case 'C': case 'D': case 'E': case 'F':
|
|
v = (unsigned char) (16 * v + (unsigned char) **str - 'A' + 10);
|
|
(*str)++;
|
|
break;
|
|
default:
|
|
if (empty)
|
|
error_exit("empty \\x escape");
|
|
goto done;
|
|
}
|
|
empty = 0;
|
|
}
|
|
done:
|
|
return v;
|
|
}
|
|
|
|
static unsigned char getoctchar(const char **str) {
|
|
unsigned char v = 0;
|
|
int nseen = 0;
|
|
while (**str && nseen < 3) {
|
|
if (**str >= '0' && **str <= '7') {
|
|
v = (unsigned char) (8 * v + (unsigned char) **str - '0');
|
|
(*str)++;
|
|
nseen++;
|
|
} else {
|
|
if (nseen == 0)
|
|
error_exit("empty \\ooo escape");
|
|
break;
|
|
}
|
|
}
|
|
return v;
|
|
}
|
|
|
|
static void *unescape(const char *str, size_t *len) {
|
|
/* results in a blob without explicit terminator, i.e., can't get
|
|
* any longer than strlen(str) */
|
|
unsigned char *x = dds_alloc(strlen(str)), *p = x;
|
|
while (*str) {
|
|
if (*str != '\\')
|
|
*p++ = (unsigned char) *str++;
|
|
else {
|
|
str++;
|
|
switch (*str) {
|
|
case '\\': case ',': case '\'': case '"': case '?':
|
|
*p++ = (unsigned char) *str;
|
|
str++;
|
|
break;
|
|
case 'x':
|
|
str++;
|
|
*p++ = gethexchar(&str);
|
|
break;
|
|
case '0': case '1': case '2': case '3': case '4': case '5': case '6': case '7':
|
|
*p++ = getoctchar(&str);
|
|
break;
|
|
case 'a': *p++ = '\a'; str++; break;
|
|
case 'b': *p++ = '\b'; str++; break;
|
|
case 'f': *p++ = '\f'; str++; break;
|
|
case 'n': *p++ = '\n'; str++; break;
|
|
case 'r': *p++ = '\r'; str++; break;
|
|
case 't': *p++ = '\t'; str++; break;
|
|
case 'v': *p++ = '\v'; str++; break;
|
|
case 'e': *p++ = 0x1b; str++; break;
|
|
default:
|
|
error_exit("invalid escape string: %s\n", str);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
*len = (size_t) (p - x);
|
|
return x;
|
|
}
|
|
|
|
void qos_user_data(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_RW(qt, q, "user_data");
|
|
size_t len;
|
|
if (qp == NULL)
|
|
return;
|
|
|
|
void *unesc = unescape(arg, &len);
|
|
if(len==0) {
|
|
dds_qset_userdata(qp, NULL, 0);
|
|
} else {
|
|
dds_qset_userdata(qp, unesc, len);
|
|
}
|
|
|
|
dds_free(unesc);
|
|
}
|
|
|
|
int double_to_dds_duration(dds_duration_t *dd, double d) {
|
|
if (d < 0)
|
|
return -1;
|
|
double nanosec = d * 1e9;
|
|
if(nanosec > INT64_MAX) {
|
|
*dd = DDS_INFINITY;
|
|
} else {
|
|
*dd = (int64_t) nanosec;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void set_infinite_dds_duration(dds_duration_t *dd) {
|
|
*dd = DDS_INFINITY;
|
|
}
|
|
|
|
void qos_reliability(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_TRW(qt, q, "reliability");
|
|
const char *argp = arg;
|
|
dds_duration_t max_block_t = DDS_MSECS(100);
|
|
|
|
if (qp == NULL)
|
|
return;
|
|
|
|
switch (*argp++) {
|
|
case 'b':
|
|
case 'n':
|
|
dds_qset_reliability(qp, DDS_RELIABILITY_BEST_EFFORT, max_block_t);
|
|
break;
|
|
case 'r':
|
|
case 'y':
|
|
if (*argp == ':') {
|
|
double max_blocking_time;
|
|
int pos;
|
|
if (strcmp(argp, ":inf") == 0) {
|
|
set_infinite_dds_duration(&max_block_t);
|
|
argp += 4;
|
|
} else if (sscanf(argp, ":%lf%n", &max_blocking_time, &pos) == 1 && argp[pos] == 0) {
|
|
if (max_blocking_time <= 0 || double_to_dds_duration(&max_block_t, max_blocking_time) < 0)
|
|
error_exit("reliability qos: %s: max blocking time out of range\n", arg);
|
|
argp += pos;
|
|
} else {
|
|
error_exit("reliability qos: %s: invalid max_blocking_time\n", arg);
|
|
}
|
|
}
|
|
dds_qset_reliability(qp, DDS_RELIABILITY_RELIABLE, max_block_t);
|
|
break;
|
|
default:
|
|
error_exit("reliability qos: %s: invalid\n", arg);
|
|
}
|
|
|
|
if (*argp != 0) {
|
|
error_exit("reliability qos: %s: invalid\n", arg);
|
|
}
|
|
}
|
|
|
|
void qos_liveliness(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_duration_t dd = 0;
|
|
dds_qos_t *qp = get_qos_TRW(qt, q, "liveliness");
|
|
double lease_duration;
|
|
int pos;
|
|
|
|
if (qp == NULL)
|
|
return;
|
|
|
|
if (strcmp(arg, "a") == 0) {
|
|
dds_qset_liveliness(qp, DDS_LIVELINESS_AUTOMATIC, DDS_INFINITY);
|
|
} else if (sscanf(arg, "p:%lf%n", &lease_duration, &pos) == 1 && arg[pos] == 0) {
|
|
if (lease_duration <= 0 || double_to_dds_duration(&dd, lease_duration) < 0)
|
|
error_exit("liveliness qos: %s: lease duration out of range\n", arg);
|
|
dds_qset_liveliness(qp, DDS_LIVELINESS_MANUAL_BY_PARTICIPANT, dd);
|
|
} else if (sscanf(arg, "w:%lf%n", &lease_duration, &pos) == 1 && arg[pos] == 0) {
|
|
if (lease_duration <= 0 || double_to_dds_duration(&dd, lease_duration) < 0)
|
|
error_exit("liveliness qos: %s: lease duration out of range\n", arg);
|
|
dds_qset_liveliness(qp, DDS_LIVELINESS_MANUAL_BY_TOPIC, dd);
|
|
} else {
|
|
error_exit("liveliness qos: %s: invalid\n", arg);
|
|
}
|
|
}
|
|
|
|
static void qos_simple_duration(dds_duration_t *dd, const char *name, const char *arg) {
|
|
double duration;
|
|
int pos;
|
|
if (strcmp(arg, "inf") == 0) {
|
|
set_infinite_dds_duration(dd);
|
|
} else if (sscanf(arg, "%lf%n", &duration, &pos) == 1 && arg[pos] == 0) {
|
|
if (double_to_dds_duration(dd, duration) < 0)
|
|
error_exit("%s qos: %s: duration invalid\n", name, arg);
|
|
} else {
|
|
error_exit("%s qos: %s: invalid\n", name, arg);
|
|
}
|
|
}
|
|
|
|
void qos_latency_budget(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_TRW(qt, q, "latency_budget");
|
|
dds_duration_t duration = 0;
|
|
if (qp == NULL)
|
|
return;
|
|
qos_simple_duration(&duration, "latency_budget", arg);
|
|
dds_qset_latency_budget(qp, duration);
|
|
}
|
|
|
|
void qos_deadline(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_TRW(qt, q, "deadline");
|
|
dds_duration_t deadline = 0;
|
|
if (qp == NULL)
|
|
return;
|
|
qos_simple_duration(&deadline, "deadline", arg);
|
|
dds_qset_deadline(qp, deadline);
|
|
}
|
|
|
|
void qos_lifespan(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_TW(qt, q, "lifespan");
|
|
dds_duration_t duration = 0;
|
|
if (qp == NULL)
|
|
return;
|
|
qos_simple_duration(&duration, "lifespan", arg);
|
|
dds_qset_lifespan(qp, duration);
|
|
}
|
|
|
|
static int one_resource_limit(int32_t *val, const char **arg) {
|
|
int pos;
|
|
if (strncmp(*arg, "inf", 3) == 0) {
|
|
*val = DDS_LENGTH_UNLIMITED;
|
|
(*arg) += 3;
|
|
return 1;
|
|
} else if (sscanf(*arg, "%d%n", val, &pos) == 1) {
|
|
(*arg) += pos;
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
void qos_resource_limits(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_TRW(qt, q, "resource_limits");
|
|
const char *argp = arg;
|
|
int32_t max_samples = 0;
|
|
int32_t max_instances = 0;
|
|
int32_t max_samples_per_instance = 0;
|
|
if (qp == NULL)
|
|
return;
|
|
|
|
if (!one_resource_limit(&max_samples, &argp))
|
|
goto err;
|
|
if (*argp++ != '/')
|
|
goto err;
|
|
if (!one_resource_limit(&max_instances, &argp))
|
|
goto err;
|
|
if (*argp++ != '/')
|
|
goto err;
|
|
if (!one_resource_limit(&max_samples_per_instance, &argp))
|
|
goto err;
|
|
|
|
dds_qset_resource_limits(qp, max_samples, max_instances, max_samples_per_instance);
|
|
|
|
if (*argp != 0)
|
|
goto err;
|
|
return;
|
|
|
|
err:
|
|
error_exit("resource limits qos: %s: invalid\n", arg);
|
|
}
|
|
|
|
void qos_durability_service(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_T(qt, q, "durability_service");
|
|
const char *argp = arg;
|
|
double service_cleanup_delay_t;
|
|
int pos, hist_depth;
|
|
dds_duration_t service_cleanup_delay = 0;
|
|
dds_history_kind_t history_kind = DDS_HISTORY_KEEP_LAST;
|
|
int32_t history_depth = 1;
|
|
int32_t max_samples = DDS_LENGTH_UNLIMITED;
|
|
int32_t max_instances = DDS_LENGTH_UNLIMITED;
|
|
int32_t max_samples_per_instance = DDS_LENGTH_UNLIMITED;
|
|
|
|
if (qp == NULL)
|
|
return;
|
|
|
|
argp = arg;
|
|
if (strncmp(argp, "inf", 3) == 0) {
|
|
set_infinite_dds_duration(&service_cleanup_delay);
|
|
pos = 3;
|
|
} else if (sscanf(argp, "%lf%n", &service_cleanup_delay_t, &pos) == 1) {
|
|
if (service_cleanup_delay_t < 0 || double_to_dds_duration(&service_cleanup_delay, service_cleanup_delay_t) < 0)
|
|
error_exit("durability service qos: %s: service cleanup delay out of range\n", arg);
|
|
} else {
|
|
goto err;
|
|
}
|
|
|
|
if (argp[pos] == 0) {
|
|
dds_qset_durability_service(qp, service_cleanup_delay, history_kind, history_depth, max_samples, max_instances, max_samples_per_instance);
|
|
return;
|
|
} else if (argp[pos] != '/') goto err;
|
|
argp += pos + 1;
|
|
|
|
if (strncmp(argp, "all", 3) == 0) {
|
|
history_kind = DDS_HISTORY_KEEP_ALL;
|
|
pos = 3;
|
|
} else if (sscanf(argp, "%d%n", &hist_depth, &pos) == 1 && hist_depth > 0) {
|
|
history_depth = hist_depth;
|
|
} else {
|
|
goto err;
|
|
}
|
|
|
|
if (argp[pos] == 0) {
|
|
dds_qset_durability_service(qp, service_cleanup_delay, history_kind, history_depth, max_samples, max_instances, max_samples_per_instance);
|
|
return;
|
|
} else if (argp[pos] != '/') goto err;
|
|
argp += pos + 1;
|
|
|
|
if (!one_resource_limit(&max_samples, &argp))
|
|
goto err;
|
|
if (*argp++ != '/')
|
|
goto err;
|
|
if (!one_resource_limit(&max_instances, &argp))
|
|
goto err;
|
|
if (*argp++ != '/')
|
|
goto err;
|
|
if (!one_resource_limit(&max_samples_per_instance, &argp))
|
|
goto err;
|
|
|
|
dds_qset_durability_service(qp, service_cleanup_delay, history_kind, history_depth, max_samples, max_instances, max_samples_per_instance);
|
|
|
|
if (*argp != 0)
|
|
goto err;
|
|
return;
|
|
|
|
err:
|
|
error_exit("durability service qos: %s: invalid\n", arg);
|
|
}
|
|
|
|
void qos_presentation(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_PS(qt, q, "presentation");
|
|
if (qp == NULL)
|
|
return;
|
|
if (strcmp(arg, "i") == 0) {
|
|
dds_qset_presentation(qp, DDS_PRESENTATION_INSTANCE, 0, 0);
|
|
} else if (strcmp(arg, "t") == 0) {
|
|
dds_qset_presentation(qp, DDS_PRESENTATION_TOPIC, 1, 0);
|
|
} else if (strcmp(arg, "g") == 0) {
|
|
dds_qset_presentation(qp, DDS_PRESENTATION_GROUP, 1, 0);
|
|
} else {
|
|
error_exit("presentation qos: %s: invalid\n", arg);
|
|
}
|
|
}
|
|
|
|
void qos_autodispose_unregistered_instances(dds_entity_kind_t qt, dds_qos_t *q, const char *arg) {
|
|
dds_qos_t *qp = get_qos_W(qt, q, "writer_data_lifecycle");
|
|
if (qp == NULL)
|
|
return;
|
|
if (strcmp(arg, "n") == 0)
|
|
dds_qset_writer_data_lifecycle(qp, false);
|
|
else if (strcmp(arg, "y") == 0)
|
|
dds_qset_writer_data_lifecycle(qp, true);
|
|
else
|
|
error_exit("autodispose_unregistered_instances qos: %s: invalid\n", arg);
|
|
}
|
|
|
|
const char *qos_arg_usagestr = "\
|
|
QOS (not all are universally applicable):\n\
|
|
A={a|p:S|w:S} liveliness (automatic, participant or writer, S in seconds)\n\
|
|
d={v|tl|t|p} durability (default: v)\n\
|
|
D=P deadline P in seconds (default: inf)\n\
|
|
k={all|N} KEEP_ALL or KEEP_LAST N\n\
|
|
l=D latency budget in seconds (default: 0)\n\
|
|
L=D lifespan in seconds (default: inf)\n\
|
|
o=[r|s] order by reception or source timestamp (default: s)\n\
|
|
O=[s|x[:S]] ownership: shared or exclusive, strength S (default: s)\n\
|
|
p=PRIO transport priority (default: 0)\n\
|
|
P={i|t|g} instance, or {topic|group} + coherent updates\n\
|
|
r={r[:T]|b} reliability, T is max blocking time in seconds,\n\
|
|
(default: r:1)\n\
|
|
R=S/I/SpI resource limits (samples, insts, S/I; default: inf/inf/inf)\n\
|
|
S=C[/H[/S/I/SpI]] durability_service (cleanup delay, history, resource limits)\n\
|
|
u={y|n} autodispose unregistered instances (default: n)\n\
|
|
U=TEXT set user_data to TEXT\n\
|
|
";
|
|
|
|
void set_qosprovider(const char *arg) {
|
|
//Todo: There is no qosprovider_create in dds.h, yet
|
|
(void)arg;
|
|
// int result = DDS_RETCODE_OK;
|
|
// const char *p = strchr(arg, ',');
|
|
// const char *xs = strstr(arg, "://");
|
|
// char *profile = NULL;
|
|
// const char *uri;
|
|
// if (p == NULL || xs == NULL || p >= xs)
|
|
// uri = arg;
|
|
// else {
|
|
// uri = p+1;
|
|
// profile = dds_string_dup(arg);
|
|
// profile[p-arg] = 0;
|
|
// }
|
|
//
|
|
// if((result = dds_qosprovider_create(&qosprov, uri, profile)) != DDS_RETCODE_OK)
|
|
// error("dds_qosprovider_create(%s,%s) failed\n", uri, profile ? profile : "(null)");
|
|
// dds_free(profile);
|
|
}
|
|
|
|
void setqos_from_args(dds_entity_kind_t qt, dds_qos_t *q, int n, const char *args[]) {
|
|
int i;
|
|
for (i = 0; i < n; i++) {
|
|
char *args_copy = dds_string_dup(args[i]), *cursor = args_copy;
|
|
const char *arg;
|
|
while ((arg = os_strsep(&cursor, ",")) != NULL) {
|
|
if (arg[0] && arg[1] == '=') {
|
|
const char *a = arg + 2;
|
|
switch (arg[0]) {
|
|
case 'A': qos_liveliness(qt, q, a); break;
|
|
case 'd': qos_durability(qt, q, a); break;
|
|
case 'D': qos_deadline(qt, q, a); break;
|
|
case 'k': qos_history(qt, q, a); break;
|
|
case 'l': qos_latency_budget(qt, q, a); break;
|
|
case 'L': qos_lifespan(qt, q, a); break;
|
|
case 'o': qos_destination_order(qt, q, a); break;
|
|
case 'O': qos_ownership(qt, q, a); break;
|
|
case 'p': qos_transport_priority(qt, q, a); break;
|
|
case 'P': qos_presentation(qt, q, a); break;
|
|
case 'r': qos_reliability(qt, q, a); break;
|
|
case 'R': qos_resource_limits(qt, q, a); break;
|
|
case 'S': qos_durability_service(qt, q, a); break;
|
|
case 'u': qos_autodispose_unregistered_instances(qt, q, a); break;
|
|
case 'U': qos_user_data(qt, q, a); break;
|
|
default:
|
|
error_exit("%s: unknown QoS\n", arg);
|
|
}
|
|
} else if (!qosprov) {
|
|
error_exit("QoS specification %s requires a QoS provider but none set\n", arg);
|
|
} else {
|
|
printf ("Qos provider not supported\n"); //Todo: Commentted qos provider. Could not find in dds.h. Fix required.
|
|
// int result;
|
|
// if (*arg == 0)
|
|
// arg = NULL;
|
|
// switch (q->qt) {
|
|
// case QT_TOPIC:
|
|
// if ((result = dds_qosprovider_get_topic_qos(qosprov, q->u.topic.q, arg)) != DDS_RETCODE_OK)
|
|
// error ("dds_qosprovider_get_topic_qos(%s): error %d (%s)\n", arg, (int) result, dds_strerror(result));
|
|
// break;
|
|
// case QT_PUBLISHER:
|
|
// if ((result = dds_qosprovider_get_publisher_qos(qosprov, q->u.pub.q, arg)) != DDS_RETCODE_OK)
|
|
// error ("dds_qosprovider_get_publisher_qos(%s): error %d (%s)\n", arg, (int) result, dds_strerror(result));
|
|
// break;
|
|
// case QT_SUBSCRIBER:
|
|
// if ((result = dds_qosprovider_get_subscriber_qos(qosprov, q->u.sub.q, arg)) != DDS_RETCODE_OK)
|
|
// error ("dds_qosprovider_subscriber_qos(%s): error %d (%s)\n", arg, (int) result, dds_strerror(result));
|
|
// break;
|
|
// case QT_WRITER:
|
|
// if ((result = dds_qosprovider_get_writer_qos(qosprov, q->u.wr.q, arg)) != DDS_RETCODE_OK)
|
|
// error ("dds_qosprovider_get_writer_qos(%s): error %d (%s)\n", arg, (int) result, dds_strerror(result));
|
|
// break;
|
|
// case QT_READER:
|
|
// if ((result = dds_qosprovider_get_reader_qos(qosprov, q->u.rd.q, arg)) != DDS_RETCODE_OK)
|
|
// error ("dds_qosprovider_get_reader_qos(%s): error %d (%s)\n", arg, (int) result, dds_strerror(result));
|
|
// break;
|
|
// }
|
|
}
|
|
}
|
|
dds_free((char *)arg);
|
|
dds_free(args_copy);
|
|
}
|
|
}
|