static void reader_frag_to (uintptr_t user)
{
RemWriter_t *rwp = (RemWriter_t *) user;
READER *rp = rwp->rw_reader;
CCREF *refp;
log_printf (RTPS_ID, 0, "reader_frag_to: %p\r\n", (void *) rwp);
refp = LIST_HEAD (rwp->rw_changes);
hc_change_free (refp->u.c.change);
rfraginfo_delete (refp);
mds_pool_free (&rtps_mem_blocks [MB_CCREF], refp);
remote_writer_remove (rp, rwp);
mds_pool_free (&rtps_mem_blocks [MB_REM_WRITER], rwp);
}
static void sfr_be_frag_to (uintptr_t user)
{
RemWriter_t *rwp = (RemWriter_t *) user;
CCREF *refp;
log_printf (RTPS_ID, 0, "sfr_be_frag_to: %p\r\n", (void *) rwp);
refp = LIST_HEAD (rwp->rw_changes);
hc_change_free (refp->u.c.change);
rfraginfo_delete (refp);
mds_pool_free (&rtps_mem_blocks [MB_CCREF], refp);
}
void reader_add_fragment (READER *rp,
Change_t *cp,
KeyHash_t *hp,
DataFragSMsg *fragp)
{
Reader_t *r = (Reader_t *) rp->endpoint.endpoint;
RemWriter_t *rwp, *fwp;
CCREF *refp;
FragInfo_t *fip;
Change_t *ncp;
unsigned max_frags;
/*log_printf (RTPS_ID, 0, "reader_add_fragment: snr:%u, start:%u, num:%u, fsize:%u, tsize:%u\r\n",
fragp->writer_sn.low,
fragp->frag_start, fragp->num_fragments,
fragp->frag_size, fragp->sample_size);*/
/* Check if we already got fragments from writer. */
fwp = NULL;
LIST_FOREACH (rp->rem_writers, rwp)
if ((refp = LIST_HEAD (rwp->rw_changes)) != NULL &&
refp->u.c.change->c_writer == cp->c_writer) {
fwp = rwp;
break;
}
/* If this is the first, add a new RemWriter for the fragment,
allocate all relevant data, and start the fragment timer. */
if (!fwp) {
max_frags = (fragp->sample_size + fragp->frag_size - 1) / fragp->frag_size;
if (fragp->frag_start + fragp->num_fragments - 1 > max_frags)
return;
if ((rwp = mds_pool_alloc (&rtps_mem_blocks [MB_REM_WRITER])) == NULL) {
warn_printf ("reader_add_fragment: no memory for fragment!");
return;
}
memset (rwp, 0, sizeof (RemWriter_t));
rwp->rw_reader = rp;
LIST_INIT (rwp->rw_changes);
rp->rem_writers.count++;
fwp = rwp;
LIST_ADD_TAIL (rp->rem_writers, *rwp);
ncp = hc_change_new ();
if (!ncp) {
warn_printf ("reader_add_fragment: no memory for change!");
goto no_change_mem;
}
ncp->c_kind = cp->c_kind;
ncp->c_writer = cp->c_writer;
ncp->c_time = cp->c_time;
ncp->c_seqnr = cp->c_seqnr;
refp = ccref_add (&rwp->rw_changes, ncp, 0, 1, CS_RECEIVED);
if (!refp) {
warn_printf ("reader_add_fragment: no memory for list element!");
goto no_ref_mem;
}
fip = rfraginfo_create (refp, fragp, max_frags);
if (!fip) {
warn_printf ("reader_add_fragment: no memory for fragment info!");
goto no_frag_mem;
}
}
else {
refp = LIST_HEAD (rwp->rw_changes);
fip = refp->fragments;
ncp = refp->u.c.change;
if (fip->fsize != fragp->frag_size ||
fip->length != fragp->sample_size ||
!SEQNR_EQ (ncp->c_seqnr, fragp->writer_sn)) {
/* Incorrect fragment context: reset it. */
fip = rfraginfo_update (refp, fragp);
if (!fip)
goto cleanup;
}
else if (fragp->frag_start + fragp->num_fragments - 1 > fip->total)
return;
}
/* Update key info if present. */
if (hp) {
fip->hash = *hp;
fip->hp = &fip->hash;
fip->key = fip->hash.hash;
fip->keylen = 12;
}
/* Mark the fragment as correctly received. */
mark_fragment (fip, fragp, cp);
/* If all fragments received correctly, cleanup the context. */
if (!fip->num_na) {
ncp->c_db = fip->data;
ncp->c_length = fip->length;
ncp->c_data = fip->data->data;
rcl_access (fip->data);
fip->data->nrefs++;
rcl_done (fip->data);
rfraginfo_delete (refp);
reader_cache_add_key (rp, ncp, &fip->hash, fip->key, fip->keylen);
cleanup:
if (fip) {
FRAGSC_TMR_STOP (rwp, &fip->timer);
rfraginfo_delete (refp);
}
mds_pool_free (&rtps_mem_blocks [MB_CCREF], refp);
remote_writer_remove (rp, rwp);
mds_pool_free (&rtps_mem_blocks [MB_REM_WRITER], rwp);
}
else {
FRAGSC_TMR_START (rwp,
&fip->timer,
TICKS_PER_SEC * 2,
(uintptr_t) rwp,
reader_frag_to,
&r->r_lock);
}
return;
no_frag_mem:
mds_pool_free (&rtps_mem_blocks [MB_CCREF], refp);
no_ref_mem:
hc_change_free (ncp);
no_change_mem:
mds_pool_free (&rtps_mem_blocks [MB_REM_WRITER], rwp);
return;
}
static void sfr_be_data (RemWriter_t *rwp,
Change_t *cp,
SequenceNumber_t *cpsnr,
const KeyHash_t *hp,
const unsigned char *key,
size_t keylen,
#ifdef RTPS_FRAGMENTS
DataFragSMsg *fragp,
FragInfo_t **finfo,
#endif
int ignore)
{
Change_t *ncp;
READER *rp = rwp->rw_reader;
Reader_t *r = (Reader_t *) rp->endpoint.endpoint;
#ifdef RTPS_FRAGMENTS
CCREF *refp;
FragInfo_t *fip;
unsigned max_frags = 0;
#if defined (DDS_SECURITY) && defined (DDS_NATIVE_SECURITY)
DB *dbp;
DBW walk;
int error;
#endif
size_t ofs;
#endif
ctrc_printd (RTPS_ID, RTPS_SFR_BE_DATA, &rwp, sizeof (rwp));
prof_start (rtps_br_data);
#ifdef RTPS_FRAGMENTS
if (rwp->rw_changes.nchanges) { /* Lingering fragment? */
refp = LIST_HEAD (rwp->rw_changes);
fip = refp->fragments;
}
else {
refp = NULL;
fip = NULL;
}
if (fragp) {
STATS_INC (rwp->rw_ndatafrags);
if (!fip) {
max_frags = (fragp->sample_size + fragp->frag_size - 1) / fragp->frag_size;
if (fragp->frag_start != 1 ||
fragp->frag_start + fragp->num_fragments - 1 > max_frags)
return;
ncp = hc_change_new ();
if (!ncp) {
warn_printf ("sfr_be_data: no memory for change!");
return;
}
ncp->c_kind = cp->c_kind;
ncp->c_writer = cp->c_writer;
ncp->c_time = cp->c_time;
ncp->c_seqnr = cp->c_seqnr;
refp = ccref_add (&rwp->rw_changes, ncp, 0, 1, CS_RECEIVED);
if (!refp) {
warn_printf ("sfr_be_data: no memory for list element!");
goto no_ref_mem;
}
new_finfo:
if (*finfo) {
fip = *finfo;
rcl_access (fip);
fip->nrefs++;
rcl_done (fip);
}
else {
fip = *finfo = rfraginfo_create (refp, fragp, max_frags);
if (!fip) {
goto no_frag_mem;
}
}
}
else {
ncp = refp->u.c.change;
if (fip->fsize != fragp->frag_size ||
fip->length != fragp->sample_size ||
!SEQNR_EQ (ncp->c_seqnr, fragp->writer_sn)) {
if (fragp->frag_start != 1)
return;
/* Incorrect fragment context: reset it. */
if (fip->nrefs == 1) {
fip = rfraginfo_update (refp, fragp);
if (!fip)
goto no_frag_mem;
}
else {
rfraginfo_delete (refp);
goto new_finfo;
}
}
else if (fragp->frag_start != fip->first_na + 1 ||
fragp->frag_start + fragp->num_fragments - 1 >
fip->total)
return;
}
/* Update key info if present. */
if (hp) {
fip->hash = *hp;
fip->hp = &fip->hash;
fip->key = fip->hash.hash;
fip->keylen = 12;
}
/* Mark the fragment as correctly received. */
mark_fragment (fip, fragp, cp);
fip->first_na++;
/* Check if all fragments are received correctly. */
if (fip->num_na) {
FRAGSC_TMR_START (rwp,
&fip->timer,
TICKS_PER_SEC * 2,
(uintptr_t) rwp,
sfr_be_frag_to,
&r->r_lock);
return;
}
#if defined (DDS_SECURITY) && defined (DDS_NATIVE_SECURITY)
/* Decrypt payload data if an encrypted payload is present. */
if (rwp->rw_endpoint &&
rwp->rw_crypto &&
fip->length) {
walk.dbp = fip->data;
walk.data = fip->data->data;
walk.length = walk.left = fip->length;
dbp = sec_decode_serialized_data (&walk,
0,
rwp->rw_crypto,
&fip->length,
&ofs,
(DDS_ReturnCode_t *) &error);
if (!dbp)
return;
fip->data = dbp;
}
else
#endif
ofs = 0;
/* Cleanup the context. */
ncp->c_db = fip->data;
ncp->c_length = fip->length;
ncp->c_data = fip->data->data + ofs;
rcl_access (fip->data);
fip->data->nrefs++;
rcl_done (fip->data);
cp = ncp;
FRAGSC_TMR_STOP (rwp, &fip->timer);
rfraginfo_delete (refp);
mds_pool_free (&rtps_mem_blocks [MB_CCREF], refp);
LIST_INIT (rwp->rw_changes);
rwp->rw_changes.nchanges = 0;
}
else {
#endif
STATS_INC (rwp->rw_ndata);
#ifdef RTPS_FRAGMENTS
if (fip) { /* Lingering fragment? Cleanup context. */
FRAGSC_TMR_STOP (rwp, &fip->timer);
rfraginfo_delete (refp);
mds_pool_free (&rtps_mem_blocks [MB_CCREF], refp);
LIST_INIT (rwp->rw_changes);
rwp->rw_changes.nchanges = 0;
}
}
#endif
RW_SIGNAL (rwp, "BE-Data");
#ifdef RTPS_MARKERS
if (rp->endpoint.mark_data)
rtps_marker_notify (r, EM_DATA, "sfr_be_data");
#endif
if (SEQNR_LT (*cpsnr, rwp->rw_seqnr_next))
return;
if (!SEQNR_EQ (*cpsnr, rwp->rw_seqnr_next))
dcps_samples_lost (r, SEQNR_DELTA (rwp->rw_seqnr_next, *cpsnr));
if (!ignore) {
if (cp->c_nrefs > 1) {
ncp = hc_change_clone (cp);
if (!ncp) {
warn_printf ("sfr_be_data: out of memory for change clone!\r\n");
return;
}
}
else {
rcl_access (cp);
cp->c_nrefs++;
rcl_done (cp);
ncp = cp;
}
ncp->c_seqnr = *cpsnr;
reader_cache_add_key (rp, ncp, hp, key, keylen);
}
rwp->rw_seqnr_next = cp->c_seqnr;
SEQNR_INC (rwp->rw_seqnr_next);
prof_stop (rtps_br_data, 1);
RW_SNR_TRACE (rwp, "BE-data: update");
#ifdef RTPS_FRAGMENTS
return;
no_frag_mem:
warn_printf ("sfr_be_data: no memory for fragment info!");
LIST_INIT (rwp->rw_changes);
rwp->rw_changes.nchanges = 0;
hc_change_free (refp->u.c.change);
mds_pool_free (&rtps_mem_blocks [MB_CCREF], refp);
no_ref_mem:
hc_change_free (ncp);
#endif
}
DDS_Topic DDS_DomainParticipant_find_topic (DDS_DomainParticipant dp,
const char *topic_name,
DDS_Duration_t *timeout)
{
Topic_t *tp;
#ifdef THREADS_USED
int ret;
struct timespec ts;
TopicWait_t *wp, *xp, *prev_wp;
#else
Ticks_t d, now, end_time; /* *10ms */
#endif
ctrc_begind (DCPS_ID, DCPS_DP_F_TOP, &dp, sizeof (dp));
ctrc_contd (topic_name, strlen (topic_name) + 1);
ctrc_contd (timeout, sizeof (DDS_Duration_t));
ctrc_endd ();
/* Get Domain Participant. */
if (!domain_ptr (dp, 1, NULL)) {
log_printf (DCPS_ID, 0, "find_topic(): domain participant not found!\r\n");
return (NULL);
}
tp = topic_lookup (&dp->participant, topic_name);
if (tp) {
if (!lock_take (tp->lock)) {
tp->entity.flags |= EF_LOCAL;
tp->nlrefs++;
lock_release (tp->lock);
}
lock_release (dp->lock);
return ((DDS_Topic) tp);
}
#ifdef THREADS_USED
for (wp = dp->topic_wait; wp; wp = wp->next)
if (!strcmp (topic_name, wp->name))
break;
if (wp)
wp->nthreads++;
else {
wp = mds_pool_alloc (&dcps_mem_blocks [MB_TOPIC_WAIT]);
if (!wp) {
lock_release (dp->lock);
return (NULL);
}
wp->next = dp->topic_wait;
cond_init (wp->condition);
wp->name = topic_name;
wp->topic = NULL;
wp->nthreads = 1;
dp->topic_wait = wp;
}
duration2timespec (timeout, &ts);
do {
if (ts.tv_sec || ts.tv_nsec)
ret = cond_wait_to (wp->condition, dp->lock, ts);
else
ret = cond_wait (wp->condition, dp->lock);
}
while (!wp->topic && !ret);
tp = wp->topic;
if (!--wp->nthreads) {
for (xp = dp->topic_wait, prev_wp = NULL;
xp != NULL && xp != wp;
prev_wp = xp, xp = xp->next)
;
if (prev_wp)
prev_wp->next = wp->next;
else
dp->topic_wait = wp->next;
cond_destroy (wp->condition);
mds_pool_free (&dcps_mem_blocks [MB_TOPIC_WAIT], wp);
}
lock_release (dp->lock);
#else
if (dds_listener_state) {
lock_release (dp->lock);
return (NULL);
}
/* Wait until timeout elapsed for discovery to add the topic. */
now = sys_getticks ();
if (timeout->sec == DDS_DURATION_INFINITE_SEC ||
timeout->nanosec == DDS_DURATION_INFINITE_NSEC)
end_time = now + 0x7ffffffe;
else
end_time = now + duration2ticks ((Duration_t *) timeout);
for (;;) {
d = end_time - now;
if (d >= 0x7fffffffUL)
break;
DDS_schedule (d * TMR_UNIT_MS);
tp = topic_lookup (&dp->participant, topic_name);
if (tp) {
tp->entity.flags |= EF_LOCAL;
tp->nlrefs++;
break;
}
now = sys_getticks ();
}
#endif
return (tp);
}
void sfr_process_samples (RemWriter_t *rwp)
{
Change_t *cp;
CCREF *rp;
ChangeState_t state;
SequenceNumber_t snr;
int error;
PROF_ITER (n);
ctrc_printd (RTPS_ID, RTPS_SFR_PROCESS, &rwp, sizeof (rwp));
prof_start (rtps_rr_proc);
/* Remove valid entries from the changes list and if relevant, store
them in the history cache. */
while ((rp = LIST_HEAD (rwp->rw_changes)) != NULL &&
((state = rp->state) == CS_LOST ||
(state == CS_RECEIVED
#ifdef RTPS_FRAGMENTS
&& !rp->fragments
#endif
))) {
PROF_INC (n);
snr = (rp->relevant) ? rp->u.c.change->c_seqnr : rp->u.range.first;
if (!SEQNR_EQ (snr, rwp->rw_seqnr_next))
break;
/* Check if we can add the sample to the cache. */
if (rp->relevant) {
cp = rp->u.c.change;
snr = cp->c_seqnr;
if (state == CS_RECEIVED) {
if (rwp->rw_reader->endpoint.cache_acks) {
rcl_access (rp->u.c.change);
rp->u.c.change->c_nrefs++;
rcl_done (rp->u.c.change);
}
error = reader_cache_add_inst (rwp->rw_reader,
rp->u.c.change,
rp->u.c.hci,
1);
/* If cache is full, try again later. */
if (error == DDS_RETCODE_NO_DATA) {
rwp->rw_blocked = 1;
break;
}
else if (rwp->rw_reader->endpoint.cache_acks)
hc_change_free (rp->u.c.change);
}
rwp->rw_reader->data_queued--;
}
else
snr = rp->u.range.last;
/* Remove change from changes list. */
LIST_REMOVE (rwp->rw_changes, *rp);
rwp->rw_changes.nchanges--;
/* Free the reference. */
mds_pool_free (&rtps_mem_blocks [MB_CCREF], rp);
/* Update the sequence number. */
SEQNR_INC (snr);
rwp->rw_seqnr_next = snr;
RW_SNR_TRACE (rwp, "process_samples");
}
prof_stop (rtps_rr_proc, n);
RANGE_CHECK (&rwp->rw_changes, "sfr_process_samples");
}
LocatorNode_t *locator_list_add (LocatorList_t *list,
LocatorKind_t kind,
const unsigned char *addr,
uint32_t port,
uint32_t scope_id,
Scope_t scope,
unsigned flags,
unsigned sproto)
{
LocSearchData data;
LocatorNode_t **npp, *np;
LocatorRef_t *rp, *p;
int is_new;
data.kind = kind;
data.addr = addr;
data.port = port;
lock_take (loc_lock);
npp = sl_insert (&loc_list, &data, &is_new, loc_cmp);
if (!npp) {
warn_printf ("locator_list_add: not enough memory for list node!");
lock_release (loc_lock);
return (NULL);
}
if (is_new) {
np = mds_pool_alloc (&mem_blocks [MB_LOCATOR]);
if (!np) {
warn_printf ("locator_list_add: not enough memory for locator node!");
#ifdef LIST_DELETE_NODE
sl_delete_node (&loc_list, npp);
#else
sl_delete (&loc_list, &data, loc_cmp);
#endif
lock_release (loc_lock);
return (NULL);
}
np->users = 0;
np->locator.kind = kind;
np->locator.port = port;
memcpy (np->locator.address, addr, sizeof (np->locator.address));
np->locator.scope_id = scope_id;
np->locator.scope = scope;
np->locator.flags = flags;
np->locator.sproto = sproto;
np->locator.intf = 0;
np->locator.handle = 0;
*npp = np;
}
else {
np = *npp;
if (np->locator.scope_id != scope_id ||
(np->locator.scope && scope && np->locator.scope != scope) ||
/*(np->locator.flags && flags &&
(np->locator.flags & LOCF_MFLAGS) != (flags & LOCF_MFLAGS)) ||*/
(np->locator.sproto && sproto && np->locator.sproto != sproto))
log_printf (LOC_ID, 0, "locator_list_add: incompatible locator attributes for %s, "
"%u:%u, %u:%u, 0x%x:0x%x, %u:%u!\r\n",
locator_str (&np->locator),
np->locator.scope_id, scope_id,
np->locator.scope, scope,
np->locator.flags, flags,
np->locator.sproto, sproto);
if (!np->locator.scope_id && scope_id)
np->locator.scope_id = scope_id;
if (!np->locator.scope && scope)
np->locator.scope = scope;
if (flags && np->locator.flags != flags)
np->locator.flags |= flags;
if (!np->locator.sproto && sproto)
np->locator.sproto = sproto;
/* Check if already in list. */
for (rp = *list; rp; rp = rp->next)
if (rp->data == np) { /* Already there! */
lock_release (loc_lock);
return (0);
}
}
#ifdef LOG_LOCATORS
log_printf (LOC_ID, 0, "LOC: locator_list_add (list=%p, %s)\r\n", (void *) list, locator_str (&np->locator));
#endif
rp = mds_pool_alloc (&mem_blocks [MB_LOCREF]);
if (!rp) {
warn_printf ("locator_list_add: not enough memory for locator reference!\r\n");
if (is_new) {
mds_pool_free (&mem_blocks [MB_LOCATOR], np);
#ifdef LIST_DELETE_NODE
sl_delete_node (&loc_list, npp);
#else
sl_delete (&loc_list, &data, loc_cmp);
#endif
}
lock_release (loc_lock);
return (NULL);
}
rp->next = NULL;
rp->data = np;
np->users++;
lock_release (loc_lock);
if (*list) {
for (p = *list; p->next; p = p->next)
;
p->next = rp;
}
else
*list = rp;
return (np);
}