v_messageQos
in_messageQos_new(
in_connectivityPeerWriter writer, c_base base)
{
in_ddsiDiscoveredWriterData i =
in_connectivityPeerWriterGetInfo(writer);
/* inefficient, should be permenanent */
c_type type;
v_messageQos _this ;
c_long transportQosValue = 0; /* TODO */
#ifdef _FAST_ACCESS_
c_long offset = 10, /* byte0 + byte1 + transport_priority + 4*time_offsets */
#else
c_long offset = 6, /* byte0 + byte1 + transport_priority */
#endif
strength_offset = 0,
latency_offset = 0,
deadline_offset = 0,
liveliness_offset = 0,
lifespan_offset = 0;
c_octet byte0 = 0,
byte1 = 0;
c_octet *dst, *src;
type = c_metaArrayTypeNew(c_metaObject(base),
"C_ARRAY<c_octet>",
c_octet_t(base),
0);
_this = c_newArray((c_collectionType)type,offset);
c_free(type); /* remove if "type" becomes permenanent */
if (_this) {
byte0 |= _LSHIFT_(i->topicData.info.reliability.kind,
MQ_BYTE0_RELIABILITY_OFFSET);
byte0 |= _LSHIFT_(i->topicData.info.ownership.kind,
MQ_BYTE0_OWNERSHIP_OFFSET);
byte0 |= _LSHIFT_(i->topicData.info.destination_order.kind,
MQ_BYTE0_ORDERBY_OFFSET);
byte0 |= _LSHIFT_(i->topicData.info.lifecycle.autodispose_unregistered_instances,
MQ_BYTE0_AUTODISPOSE_OFFSET);
byte1 |= _LSHIFT_(i->topicData.info.durability.kind,
MQ_BYTE1_DURABILITY_OFFSET);
byte1 |= _LSHIFT_(i->topicData.info.liveliness.kind,
MQ_BYTE1_LIVELINESS_OFFSET);
byte1 |= _LSHIFT_(i->topicData.info.presentation.access_scope,
MQ_BYTE1_PRESENTATION_OFFSET);
byte1 |= _LSHIFT_(i->topicData.info.presentation.coherent_access,
MQ_BYTE1_ORDERED_ACCESS_OFFSET);
byte1 |= _LSHIFT_(i->topicData.info.presentation.ordered_access,
MQ_BYTE1_COHERENT_ACCESS_OFFSET);
if (i->topicData.info.ownership.kind == V_OWNERSHIP_EXCLUSIVE) {
strength_offset = offset;
offset += sizeof(i->topicData.info.ownership_strength.value);
}
if (c_timeIsZero(i->topicData.info.latency_budget.duration)) {
byte0 |= _LSHIFT_(1,MQ_BYTE0_LATENCY_OFFSET);
} else {
latency_offset = offset;
offset += sizeof(i->topicData.info.latency_budget.duration);
}
if (c_timeIsInfinite(i->topicData.info.deadline.period)) {
byte0 |= _LSHIFT_(1,MQ_BYTE0_DEADLINE_OFFSET);
} else {
deadline_offset = offset;
offset += sizeof(i->topicData.info.deadline.period);
}
if (c_timeIsInfinite(i->topicData.info.liveliness.lease_duration)) {
byte0 |= _LSHIFT_(1,MQ_BYTE0_LIVELINESS_OFFSET);
} else {
liveliness_offset = offset;
offset += sizeof(i->topicData.info.liveliness.lease_duration);
}
if (c_timeIsInfinite(i->topicData.info.lifespan.duration)) {
byte0 |= _LSHIFT_(1,MQ_BYTE0_LIFESPAN_OFFSET);
} else {
lifespan_offset = offset;
offset += sizeof(i->topicData.info.lifespan.duration);
}
((c_octet *)_this)[0] = byte0;
((c_octet *)_this)[1] = byte1;
src = (c_octet *)&(transportQosValue); /* TODO fix*/
dst = (c_octet *)&((c_octet *)_this)[2];
_COPY4_(dst,src);
if (strength_offset) {
src = (c_octet *)&(i->topicData.info.ownership_strength.value);
dst = (c_octet *)&((c_octet *)_this)[strength_offset];
_COPY4_(dst,src);
}
if (latency_offset) {
src = (c_octet *)&(i->topicData.info.latency_budget.duration);
dst = (c_octet *)&((c_octet *)_this)[latency_offset];
_COPY8_(dst,src);
}
if (deadline_offset) {
src = (c_octet *)&(i->topicData.info.deadline.period);
dst = (c_octet *)&((c_octet *)_this)[deadline_offset];
_COPY8_(dst,src);
}
if (liveliness_offset) {
src = (c_octet *)&(i->topicData.info.liveliness.lease_duration);
dst = (c_octet *)&((c_octet *)_this)[liveliness_offset];
_COPY8_(dst,src);
}
if (lifespan_offset) {
src = (c_octet *)&(i->topicData.info.lifespan.duration);
dst = (c_octet *)&((c_octet *)_this)[lifespan_offset];
_COPY8_(dst,src);
}
} else {
assert(FALSE);
}
return _this;
}
c_bool
v_networkQueueWrite(
v_networkQueue queue,
v_message msg,
v_networkReaderEntry entry,
c_long sequenceNumber,
v_gid sender,
c_bool sendTo, /* for p2p writing */
v_gid receiver)
{
c_bool result = TRUE;
c_bool wasEmpty;
c_bool found;
v_networkStatusMarker *currentMarkerPtr;
v_networkStatusMarker currentMarker;
v_networkStatusMarker marker;
v_networkQueueSample newHolder;
c_ulonglong msecsTime;
c_ulonglong msecsResult;
c_ulonglong msecsLeftOver;
c_time sendBeforeNoTrunc;
c_time sendBefore;
c_time now;
c_ulong priorityLookingFor;
c_equality eq;
c_bool newMarkerCreated = FALSE;
c_bool sendNow = FALSE;
V_MESSAGE_STAMP(msg,readerInsertTime);
c_mutexLock(&queue->mutex);
/* numberOfSamplesArrived statistics */
v_networkQueueStatisticsAdd(numberOfSamplesArrived,queue->statistics);
if (queue->currentMsgCount == queue->maxMsgCount) {
c_mutexUnlock(&queue->mutex);
/* numberOfSamplesRejected stat */
v_networkQueueStatisticsAdd(numberOfSamplesRejected,queue->statistics);
return FALSE;
}
currentMarkerPtr = &(queue->firstStatusMarker);
currentMarker = *currentMarkerPtr;
if (queue->threadWaiting) {
wasEmpty = !v_networkQueueHasExpiringData(queue);
} else {
wasEmpty = FALSE;
}
marker = NULL;
found = FALSE;
priorityLookingFor = v_messageQos_getTransportPriority(msg->qos);
if (queue->periodic) {
if (v_messageQos_isZeroLatency(msg->qos)) {
sendBefore = C_TIME_ZERO;
sendNow = TRUE;
} else {
#ifdef _NAT_
now = v_timeGet();
#else
now = msg->allocTime;
#endif
sendBeforeNoTrunc = c_timeAdd(now,
v_messageQos_getLatencyPeriod(msg->qos));
TIME_TO_MSEC(sendBeforeNoTrunc, msecsTime);
msecsLeftOver = (c_ulonglong)((msecsTime - queue->phaseMilliSeconds) %
queue->msecsResolution);
msecsResult = (c_ulonglong)(msecsTime - msecsLeftOver);
MSEC_TO_TIME(msecsResult, sendBefore);
}
while ((currentMarker != NULL) && (!found)) {
eq = c_timeCompare(sendBefore, currentMarker->sendBefore);
switch (eq) {
case C_GT:
currentMarkerPtr = ¤tMarker->next;
currentMarker = *currentMarkerPtr;
break;
case C_EQ:
if (priorityLookingFor < currentMarker->priority) {
currentMarkerPtr = ¤tMarker->next;
currentMarker = *currentMarkerPtr;
} else {
found = TRUE;
if (priorityLookingFor == currentMarker->priority) {
marker = currentMarker;
}
}
break;
case C_LT:
found = TRUE;
break;
default:
assert(FALSE);
break;
}
}
} else {
if (currentMarker) {
sendBefore = C_TIME_ZERO;
if (c_timeIsZero(currentMarker->sendBefore)) {
marker = currentMarker;
}
}
}
/* Insert after end of list */
if (marker == NULL) {
newMarkerCreated = TRUE;
if (queue->freeStatusMarkers == NULL) {
marker = v_networkStatusMarker(c_new(queue->statusMarkerType));
} else {
marker = queue->freeStatusMarkers;
queue->freeStatusMarkers = marker->next;
}
if (marker != NULL) {
marker->sendBefore = sendBefore;
marker->priority = priorityLookingFor;
marker->firstSample = NULL;
marker->lastSample = NULL;
marker->next = *currentMarkerPtr; /* no keep, transfer refCount */
if (marker->next == NULL) {
queue->lastStatusMarker = marker; /* no keep, not reference counted */
}
*currentMarkerPtr = marker; /* no keep, transfer refCount */
} else {
OS_REPORT(OS_ERROR,
"v_networkQueueWrite",0,
"Failed to send message.");
c_mutexUnlock(&queue->mutex);
return FALSE;
}
}
V_MESSAGE_STAMP(msg,readerLookupTime);
assert(marker != NULL);
if (queue->freeSamples == NULL) {
newHolder = c_new(queue->sampleType);
} else {
newHolder = queue->freeSamples;
queue->freeSamples = newHolder->next;
}
if (newHolder) {
queue->currentMsgCount++;
/* numberOfSamplesInserted & numberOfSamplesWaiting + stats*/
v_networkQueueStatisticsAdd(numberOfSamplesInserted,queue->statistics);
v_networkQueueStatisticsCounterInc(numberOfSamplesWaiting,queue->statistics);
newHolder->message = c_keep(msg);
newHolder->entry = c_keep(entry);
newHolder->sequenceNumber = sequenceNumber;
newHolder->sender = sender;
newHolder->sendTo = sendTo;
newHolder->receiver = receiver;
if (marker->lastSample != NULL) {
newHolder->next = v_networkQueueSample(marker->lastSample)->next; /* no keep, transfer refCount */
v_networkQueueSample(marker->lastSample)->next = newHolder; /* no keep, transfer refCount */
} else {
newHolder->next = marker->firstSample; /* no keep, transfer refCount */
marker->firstSample = newHolder; /* no keep, transfer refCount */
}
marker->lastSample = newHolder;
/* Write done, wake up waiters if needed */
if (wasEmpty && queue->threadWaiting) {
if (sendNow || v_networkQueueHasExpiringData(queue)) {
c_condBroadcast(&queue->cv);
}
}
} else {
OS_REPORT(OS_ERROR,
"v_networkQueueWrite",0,
"Failed to send message.");
result = FALSE;
}
c_mutexUnlock(&queue->mutex);
return result;
}
