DDS_ReturnCode_t dds_seq_insert (void *seq, unsigned pos, void *value)
{
DDS_VoidSeq *sp = (DDS_VoidSeq *) seq;
char *cp;
int ret;
if (!pos)
return (dds_seq_prepend (seq, value));
if (pos > sp->_length)
return (DDS_RETCODE_OUT_OF_RESOURCES);
if (pos == sp->_length)
return (dds_seq_append (seq, value));
if (sp->_length == sp->_maximum) {
ret = dds_seq_extend (seq, 1);
if (ret)
return (ret);
}
cp = (char *) sp->_buffer + sp->_esize * pos;
memmove (cp + sp->_esize, cp, (sp->_length - pos) * sp->_esize);
memcpy (cp, value, sp->_esize);
sp->_length++;
return (DDS_RETCODE_OK);
}
static int check_matched_subscription (Writer_t *wp,
Endpoint_t *ep,
DDS_InstanceHandleSeq *handles,
Participant_t *pp)
{
int match;
DDS_InstanceHandle_t h;
int error;
if (pp && !same_participant (ep, pp))
match = 0;
else
#ifdef RTPS_USED
if ((ep->entity.flags & EF_LOCAL) != 0)
#endif
match = hc_matches (wp->w_cache, ((Reader_t *) ep)->r_cache);
#ifdef RTPS_USED
else if (rtps_used)
match = rtps_writer_matches (wp, (DiscoveredReader_t *) ep);
else
match = 0;
#endif
if (match) {
h = ep->entity.handle;
error = dds_seq_append (handles, &h);
}
else
error = DDS_RETCODE_OK;
return (error);
}
int strings_append_cstr (Strings_t *s, const char *cp)
{
String_t *p;
if (!cp) {
if (dds_seq_append (s, (String_t *) &cp))
return (DDS_RETCODE_OUT_OF_RESOURCES);
}
else {
p = str_new_cstr (cp);
if (!p)
return (DDS_RETCODE_OUT_OF_RESOURCES);
if (dds_seq_append (s, &p)) {
str_unref (p);
return (DDS_RETCODE_OUT_OF_RESOURCES);
}
}
return (DDS_RETCODE_OK);
}
static int participant_add_handle (Skiplist_t *list, void *node, void *arg)
{
const Participant_t *pp, **ppp = (const Participant_t **) node;
DDS_InstanceHandleSeq *handles = (DDS_InstanceHandleSeq *) arg;
DDS_InstanceHandle_t h;
ARG_NOT_USED (list)
pp = *ppp;
if (entity_ignored (pp->p_entity.flags))
return (1);
h = pp->p_handle;
return (dds_seq_append (handles, &h) == 0);
}
static int topic_add_handle (Skiplist_t *list, void *node, void *arg)
{
const Topic_t *tp, **tpp = (const Topic_t **) node;
DDS_InstanceHandleSeq *handles = (DDS_InstanceHandleSeq *) arg;
DDS_InstanceHandle_t h;
ARG_NOT_USED (list)
tp = *tpp;
if (!entity_discovered (tp->entity.flags) ||
entity_ignored (tp->entity.flags))
return (1);
h = tp->entity.handle;
return (dds_seq_append (handles, &h) == 0);
}
static int subscriber_get_reader (Skiplist_t *list, void *node, void *args)
{
Endpoint_t *ep, **epp = (Endpoint_t **) node;
SubGReaders_t *sp = (SubGReaders_t *) args;
Reader_t *rp;
ARG_NOT_USED (list)
ep = *epp;
if (!entity_reader (entity_type (&ep->entity)) || ep->u.subscriber != sp->sp)
return (1);
rp = (Reader_t *) ep;
if (!hc_avail (rp->r_cache, sp->skip))
return (1);
if (dds_seq_append (sp->rseq, (DDS_DataReader *) &rp)) {
sp->ret = DDS_RETCODE_OUT_OF_RESOURCES;
return (0);
}
return (1);
}
static qeo_retcode_t fwd_server_instance_publish(const qeo_factory_t *factory)
{
qeo_retcode_t rc = QEO_OK;
org_qeo_system_Forwarder_t fwd_data = {
.locator = DDS_SEQ_INITIALIZER(org_qeo_system_ForwarderLocator_t)
};
fwd_data.deviceId = factory->qeo_id.device_id;
/* add locator if available */
if (QMGMT_LOCATORTYPE_UNKNOWN != factory->fwd.locator->type) {
org_qeo_system_ForwarderLocator_t locator_data;
locator_data.type = factory->fwd.locator->type;
locator_data.port = factory->fwd.locator->port;
locator_data.address = factory->fwd.locator->address;
rc = ddsrc_to_qeorc(dds_seq_append(&fwd_data.locator, &locator_data));
}
if (QEO_OK == rc) {
rc = qeocore_writer_write(factory->fwd.u.server.writer, &fwd_data);
}
dds_seq_cleanup(&fwd_data.locator);
return rc;
}
static void test_qos (int enabled)
{
DDS_DomainParticipantFactoryQos qos;
DDS_DomainParticipant p;
DDS_Publisher pub;
DDS_Subscriber sub;
DDS_DataWriter dw;
DDS_DataReader dr;
DDS_Topic t;
DDS_TopicDescription td;
DDS_SubscriberQos psq, refsq;
DDS_ReturnCode_t r;
static char *buf [] = { "Hello", "World" };
static unsigned char data [] = { 0x77, 0x88, 0xaa, 0xbb, 0xcc };
unsigned char d2 [sizeof (data) + 1];
unsigned n;
size_t i;
int err;
v_printf (" - Set factory QoS to %sabled.\r\n", (enabled) ? "en" : "dis");
r = DDS_DomainParticipantFactory_get_qos (&qos);
fail_unless (r == DDS_RETCODE_OK);
qos.entity_factory.autoenable_created_entities = enabled;
r = DDS_DomainParticipantFactory_set_qos (&qos);
fail_unless (r == DDS_RETCODE_OK);
p = DDS_DomainParticipantFactory_create_participant (1, DDS_PARTICIPANT_QOS_DEFAULT, NULL, 0);
fail_unless (p != NULL);
r = register_HelloWorldData_type (p);
fail_unless (r == DDS_RETCODE_OK);
DDS_SubscriberQos__init (&refsq);
for (i = 0; (size_t) i < sizeof (buf) / sizeof (char *); i++) {
err = dds_seq_append (&refsq.partition.name, &buf [i]);
fail_unless (err == 0);
}
err = dds_seq_from_array (&refsq.group_data, data, sizeof (data));
fail_unless (err == 0);
v_printf (" - Create subscriber with specific QoS parameters.\r\n");
sub = DDS_DomainParticipant_create_subscriber (p, &refsq, NULL, 0);
fail_unless (sub != NULL);
memset (&psq, 0, sizeof (psq));
r = DDS_Subscriber_get_qos (sub, &psq);
fail_unless (r == DDS_RETCODE_OK &&
DDS_SEQ_LENGTH (refsq.partition.name) == DDS_SEQ_LENGTH (psq.partition.name) &&
DDS_SEQ_LENGTH (refsq.group_data.value) == sizeof (data));
DDS_SEQ_FOREACH (refsq.partition.name, i)
fail_unless (!strcmp (DDS_SEQ_ITEM (psq.partition.name, i),
DDS_SEQ_ITEM (refsq.partition.name, i)));
n = dds_seq_to_array (&psq.group_data, d2, sizeof (d2));
fail_unless (n == sizeof (data) && !memcmp (data, d2, sizeof (data)));
DDS_SubscriberQos__clear (&psq);
r = DDS_DomainParticipant_delete_subscriber (p, sub);
fail_unless (r == DDS_RETCODE_OK);
v_printf (" - Create topic/publisher/writer/subscriber/reader entities.\r\n");
t = DDS_DomainParticipant_create_topic (p, "HelloWorld", TYPE_NAME, DDS_TOPIC_QOS_DEFAULT, NULL, 0);
fail_unless (t != NULL);
td = DDS_DomainParticipant_lookup_topicdescription (p, "HelloWorld");
fail_unless (td != NULL);
pub = DDS_DomainParticipant_create_publisher (p, DDS_PUBLISHER_QOS_DEFAULT, NULL, 0);
fail_unless (pub != NULL);
dw = DDS_Publisher_create_datawriter (pub, t, DDS_DATAWRITER_QOS_DEFAULT, NULL, 0);
fail_unless (dw != NULL);
sub = DDS_DomainParticipant_create_subscriber (p, DDS_SUBSCRIBER_QOS_DEFAULT, NULL, 0);
fail_unless (sub != NULL);
dr = DDS_Subscriber_create_datareader (sub, td, DDS_DATAREADER_QOS_DEFAULT, NULL, 0);
fail_unless (dr != NULL);
v_printf (" - Update subscriber QoS.\r\n");
r = DDS_Subscriber_get_qos (sub, &psq);
fail_unless (r == DDS_RETCODE_OK);
DDS_SubscriberQos__clear (&refsq);
fail_unless (r == DDS_RETCODE_OK && !memcmp (&psq, &refsq, sizeof (refsq)));
for (i = 0; (size_t) i < sizeof (buf) / sizeof (char *); i++) {
err = dds_seq_append (&refsq.partition.name, &buf [i]);
fail_unless (err == 0);
}
err = dds_seq_from_array (&refsq.group_data, data, sizeof (data));
fail_unless (err == 0);
r = DDS_Subscriber_set_qos (sub, &refsq);
fail_unless (r == DDS_RETCODE_OK);
delay ();
r = DDS_Subscriber_get_qos (sub, &psq);
fail_unless (r == DDS_RETCODE_OK &&
DDS_SEQ_LENGTH (refsq.partition.name) == DDS_SEQ_LENGTH (psq.partition.name) &&
DDS_SEQ_LENGTH (psq.group_data.value) == sizeof (data));
DDS_SEQ_FOREACH (psq.partition.name, i)
fail_unless (!strcmp (DDS_SEQ_ITEM (psq.partition.name, i),
DDS_SEQ_ITEM (refsq.partition.name, i)));
n = dds_seq_to_array (&psq.group_data, d2, sizeof (d2));
fail_unless (n == sizeof (data) && !memcmp (data, d2, sizeof (data)));
DDS_SubscriberQos__clear (&refsq);
DDS_SubscriberQos__clear (&psq);
delay ();
if (!enabled) {
v_printf (" - Enable child entities.\r\n");
r = DDS_DomainParticipant_enable (p);
fail_unless (r == DDS_RETCODE_OK);
r = DDS_Topic_enable (t);
fail_unless (r == DDS_RETCODE_OK);
r = DDS_Publisher_enable (pub);
fail_unless (r == DDS_RETCODE_OK);
r = DDS_Subscriber_enable (sub);
fail_unless (r == DDS_RETCODE_OK);
r = DDS_DataWriter_enable (dw);
fail_unless (r == DDS_RETCODE_OK);
r = DDS_DataReader_enable (dr);
fail_unless (r == DDS_RETCODE_OK);
sleep (1);
}
v_printf (" - Delete child entities.\r\n");
r = DDS_Publisher_delete_datawriter (pub, dw);
fail_unless (r == DDS_RETCODE_OK);
r = DDS_Subscriber_delete_datareader (sub, dr);
fail_unless (r == DDS_RETCODE_OK);
r = DDS_DomainParticipant_delete_publisher (p, pub);
fail_unless (r == DDS_RETCODE_OK);
r = DDS_DomainParticipant_delete_subscriber (p, sub);
fail_unless (r == DDS_RETCODE_OK);
r = DDS_DomainParticipant_delete_topic (p, t);
fail_unless (r == DDS_RETCODE_OK);
unregister_HelloWorldData_type (p);
r = DDS_DomainParticipantFactory_delete_participant (p);
fail_unless (r == DDS_RETCODE_OK);
}
