bool modAOS_IN_SDU_Insert::_insertWaitingInSduUnit(AOS_Transfer_Frame* frame) {
bool success = false;
if ( ! getAuxQueue()->is_empty() ) {
std::pair<NetworkData*, int> auxQueueTop = getAuxData_();
if ( auxQueueTop.second < 0 ) {
MOD_ERROR("getAuxData_() call failed.");
}
else if ( ! auxQueueTop.first ) {
MOD_ERROR("getAuxData_() returned with null data.");
}
else {
ACE_Message_Block* mb = auxQueueTop.first;
NetworkData* inSdu = dynamic_cast<NetworkData*>(mb);
if ( ! inSdu || mb->length() != static_cast<size_t>(getInsertZoneSize()) ) {
MOD_WARNING("Unrecognized %d-octet, non-IN_SDU data unit received on auxInput queue (inSdu ptr: %X, IZ size: %d)",
mb->length(), inSdu, getInsertZoneSize());
ndSafeRelease(mb);
}
else {
MOD_DEBUG("%d-octet IN_SDU unit is waiting, attempting to copy to the %d-octet Insert Zone.",
inSdu->getUnitLength(), frame->getInSduLen());
_receivedInSduUnitCount += 1;
_receivedInSduOctetCount += inSdu->getUnitLength();
try {
frame->setInsertZone(inSdu);
success = true;
}
catch(const BufferOverflow& e) {
MOD_WARNING("%d-octet IN_SDU unit could not fit into %d-octet Insert Zone, dropping.",
inSdu->getUnitLength(), frame->getInSduLen());
}
ndSafeRelease(inSdu);
}
}
}
return success;
}
int modASM_Remove::svc() {
svcStart_();
ACE_UINT8* markerBuf = 0;
size_t markerLen = 0;
_rebuildMarker = true;
NetworkData* data = 0;
NetworkData* goodUnit = 0;
size_t goodUnitLength = 0;
size_t advanceLen = 0;
size_t remainingLen = 0;
size_t partialMarkerLen = 0;
size_t markerOffset = 0;
bool searching = false;
while ( continueService() ) {
std::pair<NetworkData*, int> queueTop = getData_();
if ( msg_queue()->deactivated() ) break;
if ( queueTop.second < 0 ) {
MOD_ERROR("getData_() call failed.");
continue;
}
else if ( ! queueTop.first ) {
MOD_ERROR("getData_() returned with null data.");
continue;
}
data = queueTop.first;
_updateMarker(markerBuf, markerLen);
MOD_DEBUG("Received a %d-octet unit to test for ASMs.", data->getUnitLength());
while ( data->getUnitLength() ) {
if ( ! goodUnit ) { // no unit to continue, need to find next ASM
partialMarkerLen = markerLen - markerOffset;
// determine if enough buffer left to find ASM
// if not, check what's left and check for remainder in next buffer
if ( data->getUnitLength() < partialMarkerLen ) {
if ( _markerMatch(data->ptrUnit(), markerBuf + markerOffset, data->getUnitLength()) ) {
// this portion is correct
markerOffset += data->getUnitLength();
}
else {
MOD_DEBUG("Missed partial ASM, resetting marker offset and bit error count.");
incPartialMismatchCount();
markerOffset = 0;
_currentBitErrors = 0;
}
advanceLen = data->getUnitLength();
}
// if so, look for ASM normally
else if ( _markerMatch(data->ptrUnit(), markerBuf + markerOffset, partialMarkerLen) ) {
MOD_DEBUG("Found ASM.");
_asmCount++;
if ( searching ) _asmDiscoveredCount++;
else _asmValidCount++;
searching = false;
advanceLen = partialMarkerLen;
markerOffset = 0;
_currentBitErrors = 0;
// determine if entire unit can be extracted from buffer
// if so, wrap and send
if ( data->getUnitLength() - partialMarkerLen >= getExpectedUnitLength() ) {
MOD_DEBUG("Found complete unit.");
goodUnit = data->wrapInnerPDU<NetworkData>(getExpectedUnitLength(),
data->ptrUnit() + partialMarkerLen);
_send(goodUnit);
advanceLen += getExpectedUnitLength();
}
// if not, create a new unit to hold it
else {
MOD_DEBUG("Holding partial unit for expected completion.");
goodUnitLength = data->getUnitLength() - partialMarkerLen;
goodUnit = new NetworkData(getExpectedUnitLength());
goodUnit->copyUnit(goodUnit->ptrUnit(), data->ptrUnit() + partialMarkerLen, goodUnitLength);
advanceLen += goodUnitLength;
};
}
else {
MOD_DEBUG("Missed ASM, advancing one octet at a time.");
if (! searching ) _asmMissedCount++; // Only increment for the first miss, or after another ASM was found.
searching = true;
incSearchCount();
// data->dump();
advanceLen = 1;
markerOffset = 0;
_currentBitErrors = 0;
}
}
else { // continuing previous unit, don't look for ASM
remainingLen = getExpectedUnitLength() - goodUnitLength;
// determine if remaining unit fits in buffer
// if so, append it and send, reset goodUnit to 0
if ( data->getUnitLength() >= remainingLen ) {
MOD_DEBUG("Completing partial unit with newly received data.");
goodUnit->copyUnit(goodUnit->ptrUnit() + goodUnitLength, data->ptrUnit(),
remainingLen);
_send(goodUnit);
advanceLen = remainingLen;
}
// if not, append it and don't send
else {
MOD_DEBUG("Adding to partial unit but will complete later.");
goodUnit->copyUnit(goodUnit->ptrUnit() + goodUnitLength, data->ptrUnit(),
data->getUnitLength());
goodUnitLength += data->getUnitLength();
advanceLen = data->getUnitLength();
}
}
// advance read pointer by length just used up
data->rd_ptr(advanceLen);
MOD_DEBUG("Buffer length is now %d.", data->getUnitLength());
}
ndSafeRelease(data);
}
return svcEnd_();
}
bool modAOS_OCF_Insert::_insertWaitingReport(AOS_Transfer_Frame* frame) {
bool success = false;
if ( ! getAuxQueue()->is_empty() ) {
std::pair<NetworkData*, int> auxQueueTop = getAuxData_();
if ( auxQueueTop.second < 0 ) {
MOD_ERROR("getAuxData_() call failed.");
}
else if ( ! auxQueueTop.first ) {
MOD_ERROR("getAuxData_() returned with null data.");
}
else {
ACE_Message_Block* mb = auxQueueTop.first;
TC_Comm_Link_Control_Word* clcw = dynamic_cast<TC_Comm_Link_Control_Word*>(mb);
if (clcw) {
MOD_DEBUG("Received a Communications Link Control Word.");
_receivedReportUnitCount += 1;
_receivedReportOctetCount += clcw->getUnitLength();
try {
frame->setCLCW(clcw);
success = true;
}
catch (const MissingField& e) {
MOD_WARNING("Attempt to insert CLCW into frame without an OCF, dropping the CLCW.");
}
delete clcw;
}
else {
NetworkData* ocf = dynamic_cast<NetworkData*>(mb);
if ( ! ocf || mb->length() != AOS_Transfer_Frame::spanOperationalControlField ) {
MOD_WARNING("Unrecognized %d-octet, non-Report data unit received on auxiliary queue.", mb->length());
ndSafeRelease(mb);
}
else {
MOD_DEBUG("Type-2 Report is waiting, attempting to copy to the Operational Control Field.");
_receivedReportUnitCount += 1;
_receivedReportOctetCount += ocf->getUnitLength();
try {
frame->setType2ReportData(ocf->ptrUnit());
success = true;
}
catch (const MissingField& e) {
MOD_WARNING("Attempt to insert Type-2 Report into AOS Transfer Frame without an OCF, dropping the report.");
}
catch (const NetworkData::MalformedPayload& e) {
MOD_WARNING("Attempt to insert malformed Type-2 Report into AOS Transfer Frame, dropping the report.");
}
ndSafeRelease(ocf);
}
}
}
}
return success;
}
