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); }