static void sfr_rel_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)
{
ENDPOINT *ep = &rwp->rw_reader->endpoint;
CCREF *rp, *gap_rp;
HCI hci;
Change_t *ncp;
InstanceHandle h;
SequenceNumber_t gap_first, gap_last, seqnr_first, seqnr_last;
RejectCause_t cause;
/*unsigned max;*/
int error, ooo;
ctrc_printd (RTPS_ID, RTPS_SFR_REL_DATA, &rwp, sizeof (rwp));
prof_start (rtps_rr_data);
#if defined (RTPS_FRAGMENTS) && defined (EXTRA_STATS)
if (fragp)
STATS_INC (rwp->rw_ndatafrags);
else
#endif
STATS_INC (rwp->rw_ndata);
RW_SIGNAL (rwp, "REL-Data");
#ifdef RTPS_MARKERS
if (rwp->rw_reader->endpoint.mark_data)
rtps_marker_notify (rwp->rw_reader->endpoint.endpoint, EM_DATA, "sfr_rel_data");
#endif
#ifdef RTPS_INIT_ACKNACK
/* If first Data: accept it and become alive. */
if (!rwp->rw_peer_alive)
sfr_rel_alive (rwp);
#endif
rwp->rw_hb_no_data = 0;
/* If seqnr already in cache: ignore sample. */
if (SEQNR_LT (*cpsnr, rwp->rw_seqnr_next)) {
RW_SIGNAL (rwp, "REL-Data: ignore (SNR < hcache.SNR)");
return;
}
ooo = !SEQNR_EQ (rwp->rw_seqnr_next, *cpsnr);
/* Create a new instance already, if possible. */
if (ignore
#ifdef RTPS_FRAGMENTS
|| fragp
#endif
)
hci = NULL;
else if (ep->multi_inst) {
hci = hc_lookup_hash (ep->endpoint->cache, hp, key, keylen,
&h, 1, ooo, &cause);
if (!hci) {
/* Don't see this as rejected -- since there is no ack
given, sample will be retransmitted!
dcps_sample_rejected ((Reader_t *) ep->endpoint,
(DDS_SampleRejectedStatusKind) cause,
h);*/
if (!ooo)
rwp->rw_blocked = 1;
return;
}
}
else {
if (!hc_accepts (ep->endpoint->cache, ooo)) {
/* Don't see this as rejected -- since there is no ack
given, sample will be retransmitted!
dcps_sample_rejected ((Reader_t *) ep->endpoint,
DDS_REJECTED_BY_SAMPLES_LIMIT, 0);*/
if (!ooo)
rwp->rw_blocked = 1;
return;
}
h = 0;
hci = NULL;
}
/* Add to changes list of proxy writer context. */
if (LIST_NONEMPTY (rwp->rw_changes)) { /* Already some samples queued. */
RW_SIGNAL (rwp, "REL-Data: changes-queued");
if (hci)
hc_inst_inform (ep->endpoint->cache, hci);
if (rwp->rw_changes.head->relevant)
seqnr_first = rwp->rw_changes.head->u.c.change->c_seqnr;
else
seqnr_first = rwp->rw_changes.head->u.range.first;
if (rwp->rw_changes.tail->relevant)
seqnr_last = rwp->rw_changes.tail->u.c.change->c_seqnr;
else
seqnr_last = rwp->rw_changes.tail->u.range.last;
if (SEQNR_GT (*cpsnr, seqnr_last)) { /* Seqnr > last in list! */
/* Add new data sample node after tail of list. */
RW_SIGNAL (rwp, "REL-Data: add-missing>last");
gap_first = seqnr_last;
SEQNR_INC (gap_first);
if (!SEQNR_EQ (gap_first, *cpsnr)) {
/* Gap between tail and data: add MISSING sample
node between current tail of list and to be
added data sample node. */
gap_last = *cpsnr;
SEQNR_DEC (gap_last);
if (!rwp->rw_changes.tail->relevant &&
rwp->rw_changes.tail->state == CS_MISSING) {
/* Extend previous gap node. */
rwp->rw_changes.tail->u.range.last = gap_last;
RW_SIGNAL (rwp, "REL-Data: extend-gap");
}
else { /* Add new gap node. */
gap_rp = ccref_add_gap (&rwp->rw_changes,
&gap_first,
&gap_last,
1,
CS_MISSING);
if (!gap_rp)
return;
RW_SIGNAL (rwp, "REL-Data: add-gap");
}
}
/* Append data sample node. */
if (ignore)
rp = ccref_add_gap (&rwp->rw_changes, cpsnr,
cpsnr, 1, CS_RECEIVED);
else
rp = ccref_add_received (&rwp->rw_changes, cp,
cpsnr, hci, h, 1);
if (!rp)
return;
rwp->rw_reader->data_queued++;
#ifdef RTPS_FRAGMENTS
if (fragp) {
sfr_fragment (rwp, rp, fragp, finfo, hp, cp, ooo, ignore);
return;
}
#endif
RANGE_CHECK (&rwp->rw_changes, "REL-Data: >last");
}
else if (SEQNR_LT (*cpsnr, seqnr_first)) { /* Seqnr < first! */
/* Add new data sample node before head of list. */
RW_SIGNAL (rwp, "REL-Data: add-missing<first");
gap_last = seqnr_first;
SEQNR_DEC (gap_last);
if (!SEQNR_EQ (gap_last, *cpsnr)) {
/* Gap between data and head: add MISSING sample
node between to be added data sample and
current head of list. */
gap_first = *cpsnr;
SEQNR_INC (gap_first);
if (!rwp->rw_changes.head->relevant &&
rwp->rw_changes.tail->state == CS_MISSING) {
/* Extended following gap node. */
rwp->rw_changes.head->u.range.first = gap_first;
RW_SIGNAL (rwp, "REL-Data: extend-gap");
}
else { /* Add new gap node. */
gap_rp = ccref_add_gap (&rwp->rw_changes,
&gap_first,
&gap_last,
0,
CS_MISSING);
if (!gap_rp)
return;
RW_SIGNAL (rwp, "REL-Data: add-gap");
}
}
/* Prepend data sample node. */
if (ignore)
rp = ccref_add_gap (&rwp->rw_changes, cpsnr,
cpsnr, 0, CS_RECEIVED);
else
rp = ccref_add_received (&rwp->rw_changes, cp,
cpsnr, hci, h, 0);
if (!rp)
return;
rwp->rw_reader->data_queued++;
#ifdef RTPS_FRAGMENTS
if (fragp) {
sfr_fragment (rwp, rp, fragp, finfo, hp, cp, ooo, ignore);
return;
}
#endif
RANGE_CHECK (&rwp->rw_changes, "REL-Data: <first");
}
else { /* Seqnr somewhere in list - lets find it. */
LIST_FOREACH (rwp->rw_changes, rp)
if (rp->relevant) {
if (SEQNR_EQ (rp->u.c.change->c_seqnr, *cpsnr))
break;
}
else if (!SEQNR_LT (*cpsnr, rp->u.range.first) &&
!SEQNR_GT (*cpsnr, rp->u.range.last))
break;
if (LIST_END (rwp->rw_changes, rp) ||
(rp->state != CS_MISSING
#ifdef RTPS_FRAGMENTS
&& !rp->fragments
#endif
))
return;
#ifdef RTPS_FRAGMENTS
if (rp->fragments) {
sfr_fragment (rwp, rp, fragp, finfo, hp, cp, ooo, ignore);
return;
}
#endif
RW_SIGNAL (rwp, "REL-Data: in-range");
RANGE_CHECK (&rwp->rw_changes, "REL-Data: in-range");
if (SEQNR_GT (*cpsnr, rp->u.range.first)) {
/* Prepend gap node: range.first .. *cpsnr -1 */
gap_first = rp->u.range.first;
gap_last = *cpsnr;
SEQNR_DEC (gap_last);
gap_rp = ccref_insert_gap (&rwp->rw_changes,
rp->prev,
&gap_first,
&gap_last,
CS_MISSING);
if (!gap_rp)
return;
RW_SIGNAL (rwp, "REL-Data: prepend-gap");
}
if (SEQNR_LT (*cpsnr, rp->u.range.last)) {
/* Append gap node: *cpsnr + 1 .. range.last */
gap_first = *cpsnr;
SEQNR_INC (gap_first);
gap_last = rp->u.range.last;
gap_rp = ccref_insert_gap (&rwp->rw_changes,
rp,
&gap_first,
&gap_last,
CS_MISSING);
if (!gap_rp)
return;
RW_SIGNAL (rwp, "REL-Data: append-gap");
}
/* Reuse gap node for data. */
#ifdef RTPS_FRAGMENTS
rp->fragments = NULL;
#endif
if (ignore) {
rp->relevant = 0;
rp->u.range.first = *cpsnr;
rp->u.range.last = *cpsnr;
}
else {
if (cp->c_nrefs > 1) {
ncp = hc_change_clone (cp);
if (!ncp) {
warn_printf ("sfr_rel_data (): out of memory for change clone!\r\n");
return;
}
}
else {
rcl_access (cp);
cp->c_nrefs++;
rcl_done (cp);
ncp = cp;
}
ncp->c_handle = h;
ncp->c_seqnr = *cpsnr;
rp->relevant = 1;
rp->u.c.hci = hci;
rp->u.c.change = ncp;
rwp->rw_reader->data_queued++;
}
rp->state = CS_RECEIVED;
#ifdef RTPS_FRAGMENTS
if (fragp) {
sfr_fragment (rwp, rp, fragp, finfo, hp, cp, ooo, ignore);
return;
}
#endif
RW_SIGNAL (rwp, "REL-Data: missing::received");
RANGE_CHECK (&rwp->rw_changes, "REL-Data: missing::received");
}
/* If the received data sample caused some samples to be valid,
add these samples to the history cache and remove them from
the changes list. */
rp = LIST_HEAD (rwp->rw_changes);
if (rp &&
(rp->state == CS_RECEIVED || rp->state == CS_LOST) &&
#ifdef RTPS_FRAGMENTS
!rp->fragments &&
#endif
rwp->rw_heartbeats) {
RW_SIGNAL (rwp, "REL-Data: process-samples");
RANGE_CHECK (&rwp->rw_changes, "REL-Data: process-samples");
sfr_process_samples (rwp);
}
}
int setup_hierarchy(Msmpot *msm) {
const int nu = INTERP_PARAMS[msm->interp].nu;
const int omega = INTERP_PARAMS[msm->interp].omega;
const int split = msm->split;
int level, maxlevels;
int err = 0;
const float a = msm->a;
const float hx = msm->hx;
const float hy = msm->hy;
const float hz = msm->hz;
/* maximum extent of epotmap */
float xm1 = msm->xm0 + msm->dx * (msm->mx - 1);
float ym1 = msm->ym0 + msm->dy * (msm->my - 1);
float zm1 = msm->zm0 + msm->dz * (msm->mz - 1);
/* smallest possible extent of finest spaced MSM lattice */
float xlo = (msm->xmin < msm->xm0 ? msm->xmin : msm->xm0);
float ylo = (msm->ymin < msm->ym0 ? msm->ymin : msm->ym0);
float zlo = (msm->zmin < msm->zm0 ? msm->zmin : msm->zm0);
float xhi = (msm->xmax > xm1 ? msm->xmax : xm1);
float yhi = (msm->ymax > ym1 ? msm->ymax : ym1);
float zhi = (msm->zmax > zm1 ? msm->zmax : zm1);
/* indexes for MSM lattice */
long ia = ((long) floorf((xlo - msm->xm0) / hx)) - nu;
long ja = ((long) floorf((ylo - msm->ym0) / hy)) - nu;
long ka = ((long) floorf((zlo - msm->zm0) / hz)) - nu;
long ib = ((long) floorf((xhi - msm->xm0) / hx)) + 1 + nu;
long jb = ((long) floorf((yhi - msm->ym0) / hy)) + 1 + nu;
long kb = ((long) floorf((zhi - msm->zm0) / hz)) + 1 + nu;
long ni = ib - ia + 1;
long nj = jb - ja + 1;
long nk = kb - ka + 1;
long omega3 = omega * omega * omega;
long nhalf = (long) sqrtf(ni * nj * nk);
long lastnelems = (nhalf > omega3 ? nhalf : omega3);
long nelems, n;
long i, j, k;
MsmpotLattice *p = NULL;
float scaling;
n = ni;
if (n < nj) n = nj;
if (n < nk) n = nk;
for (maxlevels = 1; n > 0; n >>= 1) maxlevels++;
if (msm->maxlevels < maxlevels) {
MsmpotLattice **t;
t = (MsmpotLattice **) realloc(msm->qh, maxlevels*sizeof(MsmpotLattice *));
if (NULL == t) return ERROR(MSMPOT_ERROR_ALLOC);
msm->qh = t;
t = (MsmpotLattice **) realloc(msm->eh, maxlevels*sizeof(MsmpotLattice *));
if (NULL == t) return ERROR(MSMPOT_ERROR_ALLOC);
msm->eh = t;
t = (MsmpotLattice **) realloc(msm->gc, maxlevels*sizeof(MsmpotLattice *));
if (NULL == t) return ERROR(MSMPOT_ERROR_ALLOC);
msm->gc = t;
for (level = msm->maxlevels; level < maxlevels; level++) {
msm->qh[level] = Msmpot_lattice_create();
if (NULL == msm->qh[level]) return ERROR(MSMPOT_ERROR_ALLOC);
msm->eh[level] = Msmpot_lattice_create();
if (NULL == msm->eh[level]) return ERROR(MSMPOT_ERROR_ALLOC);
msm->gc[level] = Msmpot_lattice_create();
if (NULL == msm->gc[level]) return ERROR(MSMPOT_ERROR_ALLOC);
}
msm->maxlevels = maxlevels;
}
level = 0;
do {
err = Msmpot_lattice_setup(msm->qh[level], ia, ib, ja, jb, ka, kb);
if (err) return ERROR(err);
err = Msmpot_lattice_setup(msm->eh[level], ia, ib, ja, jb, ka, kb);
if (err) return ERROR(err);
nelems = ni * nj * nk;
ia = -((-ia+1)/2) - nu;
ja = -((-ja+1)/2) - nu;
ka = -((-ka+1)/2) - nu;
ib = (ib+1)/2 + nu;
jb = (jb+1)/2 + nu;
kb = (kb+1)/2 + nu;
ni = ib - ia + 1;
nj = jb - ja + 1;
nk = kb - ka + 1;
level++;
} while (nelems > lastnelems);
msm->nlevels = level;
/* ellipsoid axes for lattice cutoff weights */
ni = (long) ceilf(2*a/hx) - 1;
nj = (long) ceilf(2*a/hy) - 1;
nk = (long) ceilf(2*a/hz) - 1;
scaling = 1;
for (level = 0; level < msm->nlevels - 1; level++) {
p = msm->gc[level];
err = Msmpot_lattice_setup(p, -ni, ni, -nj, nj, -nk, nk);
if (err) return ERROR(err);
for (k = -nk; k <= nk; k++) {
for (j = -nj; j <= nj; j++) {
for (i = -ni; i <= ni; i++) {
float s, t, gs, gt, g;
s = ( (i*hx)*(i*hx) + (j*hy)*(j*hy) + (k*hz)*(k*hz) ) / (a*a);
t = 0.25f * s;
if (t >= 1) {
g = 0;
}
else if (s >= 1) {
gs = 1/sqrtf(s);
SPOLY(>, t, split);
g = scaling * (gs - 0.5f * gt) / a;
}
else {
SPOLY(&gs, s, split);
SPOLY(>, t, split);
g = scaling * (gs - 0.5f * gt) / a;
}
RANGE_CHECK(p, i, j, k);
*ELEM(p, i, j, k) = g;
}
}
} /* end loops over k-j-i */
scaling *= 0.5f;
} /* end loop over levels */
/* calculate coarsest level weights, ellipsoid axes are length of lattice */
ni = (msm->qh[level])->ib - (msm->qh[level])->ia;
nj = (msm->qh[level])->jb - (msm->qh[level])->ja;
nk = (msm->qh[level])->kb - (msm->qh[level])->ka;
p = msm->gc[level];
err = Msmpot_lattice_setup(p, -ni, ni, -nj, nj, -nk, nk);
for (k = -nk; k <= nk; k++) {
for (j = -nj; j <= nj; j++) {
for (i = -ni; i <= ni; i++) {
float s, gs;
s = ( (i*hx)*(i*hx) + (j*hy)*(j*hy) + (k*hz)*(k*hz) ) / (a*a);
if (s >= 1) {
gs = 1/sqrtf(s);
}
else {
SPOLY(&gs, s, split);
}
RANGE_CHECK(p, i, j, k);
*ELEM(p, i, j, k) = scaling * gs/a;
}
}
} /* end loops over k-j-i for coarsest level weights */
return OK;
}
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");
}
