DDS_ReturnCode_t dcps_write (DDS_DataWriter wp,
const void *instance_data,
int dynamic,
const DDS_InstanceHandle_t handle,
const FTime_t *time,
DDS_InstanceHandleSeq *dests)
{
Change_t *cp;
unsigned char *keys;
unsigned char *dp;
DB *dbp;
HCI hci;
size_t tlen, ofs, size;
InstanceHandle h;
const TypeSupport_t *ts;
unsigned i;
DDS_ReturnCode_t ret;
unsigned char buf [16];
prof_start (dcps_write_p);
if (!writer_ptr (wp, 1, &ret))
return (ret);
if (dests) {
if (!dests->_length || !dests->_buffer) {
ret = DDS_RETCODE_BAD_PARAMETER;
goto done;
}
else if (dests->_length > MAX_DW_DESTS) {
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto done;
}
}
if (!wp->w_pm_status.current_count &&
wp->w_qos->qos.durability_kind == DDS_VOLATILE_DURABILITY_QOS &&
(wp->w_flags & EF_BUILTIN) == 0) {
ret = DDS_RETCODE_OK;
goto done;
}
h = handle;
ts = wp->w_topic->type->type_support;
if (!handle && ts->ts_keys) {
keys = dcps_key_data_get (wp->w_topic, instance_data, dynamic, ENC_DATA (&wp->w_lep), buf, &size, &ret);
if (!keys) {
warn_printf ("DDS_DataWriter_write: invalid parameters!");
goto done;
}
hci = hc_register (wp->w_cache, keys, size, time, &h);
if (!hci) {
warn_printf ("DDS_DataWriter_write: cache_register_instance() failed!");
if (size > sizeof (buf))
xfree (keys);
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto done;
}
if (size > sizeof (buf))
xfree (keys);
}
else
hci = NULL;
if (!hc_write_required (wp->w_cache)) {
ret = DDS_RETCODE_OK;
goto done;
}
/* Allocate a new change record. */
cp = hc_change_new ();
if (!cp) {
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto done;
}
#if defined (DDS_DEBUG) && defined (DUMP_DDATA)
if (dynamic) {
dbg_printf ("dcps_write:\r\n");
xd_dump (1, instance_data);
}
#endif
if (ts->ts_dynamic || ts->ts_length > 512) {
ofs = 0;
tlen = DDS_MarshalledDataSize (instance_data, dynamic, ts, &ret);
if (ret) {
log_printf (DCPS_ID, 0, "DDS_DataWriter_write({%u}): marshalled buffer size could not be determined (%d)!\r\n",
wp->w_handle, ret);
goto free_data;
}
if (tlen > dds_max_sample_size) {
log_printf (DCPS_ID, 0, "DDS_DataWriter_write({u}): marshalled buffer size exceeds system limits (%lu/%lu bytes)\r\n",
(unsigned long) tlen, (unsigned long) dds_max_sample_size);
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto free_data;
}
}
else {
if (ts->ts_length > dds_max_sample_size) {
log_printf (DCPS_ID, 0, "DDS_DataWriter_write({u}): sample size exceeds system limits (%lu/%lu bytes)\r\n",
(unsigned long) ts->ts_length, (unsigned long) dds_max_sample_size);
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto free_data;
}
ofs = 4;
tlen = ts->ts_length + 4;
if (tlen <= C_DSIZE) {
cp->c_db = NULL;
dp = cp->c_xdata;
memcpy (dp + ofs, instance_data, ts->ts_length);
dp [0] = dp [2] = dp [3] = 0;
dp [1] = (MODE_RAW << 1) | ENDIAN_CPU;
goto data_copied;
}
}
/* Allocate a container to store the data. */
if ((dbp = db_alloc_data (tlen, 1)) == NULL) {
warn_printf ("DDS_DataWriter_write({%u}): out of memory for data (%lu bytes)!\r\n",
wp->w_handle, (unsigned long) tlen);
ret = DDS_RETCODE_OUT_OF_RESOURCES;
goto free_data;
}
cp->c_db = dbp;
dp = dbp->data;
if (ts->ts_dynamic || ts->ts_length > 512) {
/* Add marshalled data to DB chain, prefixed with marshalling type. */
ret = DDS_MarshallData (dp, instance_data, dynamic, ts);
if (ret) {
log_printf (DCPS_ID, 0, "DDS_DataWriter_write({%u}): error %u marshalling data!\r\n",
wp->w_handle, ret);
goto free_data;
}
}
else {
/* Add raw data to DB chain, prefixed with RAW identifier. */
db_put_data (dbp, 4, instance_data, ts->ts_length);
dp [0] = dp [2] = dp [3] = 0;
dp [1] = (MODE_RAW << 1) | ENDIAN_CPU;
}
data_copied:
cp->c_data = dp;
/* cp->c_wack = 0; */
cp->c_kind = ALIVE;
cp->c_linear = 1;
cp->c_writer = wp->w_handle;
cp->c_time = *time;
cp->c_handle = h;
cp->c_length = tlen;
if (!dests)
cp->c_dests [0] = 0;
else {
for (i = 0; i < dests->_length; i++)
cp->c_dests [i] = dests->_buffer [i];
while (i < MAX_DW_DESTS)
cp->c_dests [i++] = 0;
}
ret = hc_add_inst (wp->w_cache, cp, hci, 0);
goto done;
free_data:
hc_change_free (cp);
done:
lock_release (wp->w_lock);
prof_stop (dcps_write_p, 1);
return (ret);
}
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
}
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_fragment (RemWriter_t *rwp,
CCREF *refp,
DataFragSMsg *fragp,
FragInfo_t **finfo,
const KeyHash_t *hp,
Change_t *cp,
int ooo,
int ignore)
{
Change_t *xcp, *ncp;
FragInfo_t *fip;
Reader_t *rp;
const TypeSupport_t *ts;
unsigned max_frags, size;
int all_key, error;
HCI hci;
InstanceHandle h;
RejectCause_t cause;
DBW walk;
size_t ofs = 0;
#if defined (DDS_SECURITY) && defined (DDS_NATIVE_SECURITY)
DB *dbp;
#endif
/* If this is the first fragment, create the fragments context. */
fip = refp->fragments;
if (!fip) {
if (refp->relevant) {
/* Replace change with empty one. */
ncp = hc_change_new ();
if (!ncp) {
refp->state = CS_MISSING;
return;
}
xcp = refp->u.c.change;
ncp->c_kind = xcp->c_kind;
ncp->c_writer = xcp->c_writer;
ncp->c_time = xcp->c_time;
ncp->c_seqnr = xcp->c_seqnr;
hc_change_free (xcp);
refp->u.c.change = ncp;
}
else {
refp->state = CS_RECEIVED;
goto done;
}
fip = *finfo;
if (fip) {
rcl_access (fip);
fip->nrefs++;
rcl_done (fip);
refp->fragments = fip;
}
else {
max_frags = (fragp->sample_size + fragp->frag_size - 1) / fragp->frag_size;
if (fragp->frag_start + fragp->num_fragments - 1 > max_frags) {
no_frag_mem:
refp->state = CS_MISSING;
if (refp->u.c.change)
hc_change_free (refp->u.c.change);
return;
}
*finfo = fip = rfraginfo_create (refp, fragp, max_frags);
if (!fip)
goto no_frag_mem;
}
}
/* If this is the first fragment and we already have a fragmentation
context, and the data needs to be ignored, we abort reception. */
else if (refp->relevant && fragp->frag_start == 1 && ignore) {
xcp = refp->u.c.change;
refp->state = CS_RECEIVED;
refp->relevant = 0;
refp->u.range.first = refp->u.range.last = xcp->c_seqnr;
hc_change_free (xcp);
goto done_clean_fip;
}
/* Update key info if present. */
if (hp) {
fip->hash = *hp;
fip->hp = &fip->hash;
}
/* Mark the fragment as correctly received. */
mark_fragment (fip, fragp, cp);
/* If more fragments pending, simply exit, waiting for the next. */
if (fip->num_na)
return;
/* Data complete! Derive key info and get a new instance if possible. */
/* If data type indicates multi-instance data, we need the actual keys
in order to lookup instances properly. */
/*log_printf (RTPS_ID, 0, "sfr_fragment: complete!\r\n");*/
rp = (Reader_t *) rwp->rw_reader->endpoint.endpoint;
ts = rp->r_topic->type->type_support;
if (!refp->relevant || !ts->ts_keys) {
if (!hc_accepts (rp->r_cache, ooo)) {
/* Don't see this as rejected -- since there is no ack
given, last fragment will simply be retransmitted!
dcps_sample_rejected ((Reader_t *) ep->endpoint,
DDS_REJECTED_BY_SAMPLES_LIMIT, 0);*/
if (!ooo)
rwp->rw_blocked = 1;
ignore_last_fragment:
fip->num_na = 1;
fip->first_na = fragp->frag_start + fragp->num_fragments - 2;
SET_REM (fip->bitmap, fip->first_na);
return;
}
else
goto no_keys;
}
#if defined (DDS_SECURITY) && defined (DDS_NATIVE_SECURITY)
/* Decrypt payload data if an encrypted payload is present. */
if (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)
goto cleanup;
fip->data = dbp;
}
#endif
/* Key somewhere in either marshalled data or marshalled key.
Derive key information if not yet done. */
if (!fip->key) {
if (fip->length) {
walk.dbp = fip->data;
walk.data = fip->data->data + ofs;
walk.left = walk.length = fip->length;
}
all_key = cp->c_kind != ALIVE;
size = ts->ts_mkeysize;
if (!size || !ts->ts_fksize) {
size = DDS_KeySizeFromMarshalled (walk, ts,
all_key, NULL);
if (!size)
goto cleanup;
}
fip->keylen = size;
if (ts->ts_mkeysize &&
ts->ts_fksize &&
size <= sizeof (KeyHash_t) &&
!ENC_DATA (&rp->r_lep)) {
if (fip->hp) {
fip->key = fip->hash.hash;
goto got_keys;
}
if (size < sizeof (KeyHash_t))
memset (fip->hash.hash + size, 0,
sizeof (KeyHash_t) - size);
}
else {
fip->key = xmalloc (size);
if (!fip->key)
goto ignore_last_fragment;
}
error = DDS_KeyFromMarshalled (fip->key, walk, ts, all_key,
ENC_DATA (&rp->r_lep));
if (error) {
if (fip->key != fip->hash.hash)
xfree (fip->key);
fip->key = NULL;
goto cleanup;
}
if (!fip->hp) {
error = DDS_HashFromKey (fip->hash.hash, fip->key, size,
ENC_DATA (&rp->r_lep), ts);
if (error) {
if (fip->key != fip->hash.hash)
xfree (fip->key);
fip->key = NULL;
goto cleanup;
}
fip->hp = &fip->hash;
}
}
got_keys:
/* Key information is present in the fragment context now.
Derive a new or existing cache instance context. */
hci = hc_lookup_hash (rp->r_cache,
fip->hp, fip->key, fip->keylen,
&h, 1, ooo, &cause);
if (!hci) {
/* Don't see this as rejected -- since there is no ack
given, last fragment will simply be retransmitted!
dcps_sample_rejected ((Reader_t *) ep->endpoint,
(DDS_SampleRejectedStatusKind) cause,
h);*/
if (!ooo)
rwp->rw_blocked = 1;
goto ignore_last_fragment;
}
refp->u.c.hci = hci;
refp->u.c.change->c_handle = h;
hc_inst_inform (rp->r_cache, hci);
no_keys:
/* Transform to a valid received sample, as if this was a normal DATA
submessage. */
if (refp->relevant) {
xcp = refp->u.c.change;
xcp->c_db = fip->data;
xcp->c_data = fip->data->data + ofs;
xcp->c_length = fip->length;
rcl_access (xcp);
xcp->c_db->nrefs++;
rcl_done (xcp);
}
done_clean_fip:
/* Cleanup fragmentation context. */
if (fip->nrefs == 1)
*finfo = NULL;
rfraginfo_delete (refp);
done:
/* If received sample is first, add samples to history cache. */
sfr_process_samples (rwp);
return;
cleanup:
refp->state = CS_MISSING;
xcp = refp->u.c.change;
refp->relevant = 0;
refp->fragments = NULL;
refp->u.range.first = refp->u.range.last = xcp->c_seqnr;
hc_change_free (xcp);
rfraginfo_delete (refp);
}