Digital_Voice_Modem_Project
/
dvmhost
mirror of https://github.com/DVMProject/dvmhost.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

refactor how confirmed data blocks are handled; better handle determining src and dst LLIds during PDU processing; better handle data offset and PDU data length calculations; handle properly acknowledging confirmed packets with A/N flag set;

Browse Source
pull/39/head
Bryan Biedenkapp 3 years ago
parent 9b43411376
commit 9a71eb344f
4 changed files with 57 additions and 38 deletions
Show Stats Download Patch File Download Diff File

2
src/p25/P25Defines.h
Unescape View File

@ -238,6 +238,8 @@ namespace p25
const uint8_t PDU_ACK_CLASS_ACK_RETRY = 0x02U; const uint8_t PDU_ACK_CLASS_ACK_RETRY = 0x02U;
// PDU ACK Type(s) // PDU ACK Type(s)
const uint8_t PDU_ACK_TYPE_RETRY = 0x00U;
const uint8_t PDU_ACK_TYPE_ACK = 0x01U; const uint8_t PDU_ACK_TYPE_ACK = 0x01U;
const uint8_t PDU_ACK_TYPE_NACK_ILLEGAL = 0x00U; // Illegal Format const uint8_t PDU_ACK_TYPE_NACK_ILLEGAL = 0x00U; // Illegal Format

9
src/p25/data/DataBlock.cpp
Unescape View File

@ -106,17 +106,14 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader& header)
::memset(m_data, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); ::memset(m_data, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
// if this is extended addressing and the first block decode the SAP and LLId // if this is extended addressing and the first block decode the SAP and LLId
uint32_t count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES;
if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) { if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) {
count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - 4U;
m_sap = buffer[5U] & 0x3FU; // Service Access Point m_sap = buffer[5U] & 0x3FU; // Service Access Point
m_llId = (buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U]; // Logical Link ID m_llId = (buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U]; // Logical Link ID
::memcpy(m_data, buffer + 6U, count); // Payload Data ::memcpy(m_data, buffer + 2U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); // Payload Data
} }
else { else {
::memcpy(m_data, buffer + 2U, count); // Payload Data ::memcpy(m_data, buffer + 2U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); // Payload Data
} }
// compute CRC-9 for the packet // compute CRC-9 for the packet
@ -127,7 +124,7 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader& header)
#if DEBUG_P25_PDU_DATA #if DEBUG_P25_PDU_DATA
LogDebug(LOG_P25, "P25_DUID_PDU, fmt = $%02X, crc = $%04X, calculated = $%04X", m_fmt, crc, calculated); LogDebug(LOG_P25, "P25_DUID_PDU, fmt = $%02X, crc = $%04X, calculated = $%04X", m_fmt, crc, calculated);
Utils::dump(1U, "P25, DataBlock::decode(), Confirmed PDU Block Data", m_data, count); Utils::dump(1U, "P25, DataBlock::decode(), Confirmed PDU Block Data", m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
#endif #endif
} }
catch (...) { catch (...) {

62
src/p25/packet/Data.cpp
Unescape View File

@ -215,6 +215,9 @@ bool Data::process(uint8_t* data, uint32_t len)
blocksToFollow--; blocksToFollow--;
} }
uint32_t srcId = m_rfDataHeader.getLLId();
uint32_t dstId = (m_rfUseSecondHeader || m_rfExtendedAddress) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId();
m_rfPDUCount++; m_rfPDUCount++;
uint32_t bitLength = ((blocksToFollow + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; uint32_t bitLength = ((blocksToFollow + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
@ -253,6 +256,7 @@ bool Data::process(uint8_t* data, uint32_t len)
m_rfSecondHeader.setFormat(m_rfData[i].getFormat()); m_rfSecondHeader.setFormat(m_rfData[i].getFormat());
m_rfSecondHeader.setLLId(m_rfData[i].getLLId()); m_rfSecondHeader.setLLId(m_rfData[i].getLLId());
m_rfSecondHeader.setSAP(m_rfData[i].getSAP()); m_rfSecondHeader.setSAP(m_rfData[i].getSAP());
dstId = m_rfSecondHeader.getLLId();
m_rfExtendedAddress = true; m_rfExtendedAddress = true;
} }
else { else {
@ -262,7 +266,8 @@ bool Data::process(uint8_t* data, uint32_t len)
} }
m_rfData[i].getData(m_pduUserData + dataOffset); m_rfData[i].getData(m_pduUserData + dataOffset);
m_pduUserDataLength += (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; dataOffset += (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
m_pduUserDataLength = dataOffset;
// only send data blocks across the network, if we're not an AMBT, // only send data blocks across the network, if we're not an AMBT,
// an RSP or a registration service // an RSP or a registration service
@ -279,6 +284,27 @@ bool Data::process(uint8_t* data, uint32_t len)
bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength); bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength);
if (!crcRet) { if (!crcRet) {
LogWarning(LOG_RF, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", blocksToFollow, m_pduUserDataLength); LogWarning(LOG_RF, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", blocksToFollow, m_pduUserDataLength);
// does the packet require ack?
if (m_rfDataHeader.getAckNeeded()) {
if (m_rfExtendedAddress) {
writeRF_PDU_Ack_Response(PDU_ACK_CLASS_NACK, PDU_ACK_TYPE_NACK_PACKET_CRC, dstId, srcId);
}
else {
writeRF_PDU_Ack_Response(PDU_ACK_CLASS_NACK, PDU_ACK_TYPE_NACK_PACKET_CRC, srcId);
}
}
}
else {
// does the packet require ack?
if (m_rfDataHeader.getAckNeeded()) {
if (m_rfExtendedAddress) {
writeRF_PDU_Ack_Response(PDU_ACK_CLASS_ACK, PDU_ACK_TYPE_ACK, dstId, srcId);
}
else {
writeRF_PDU_Ack_Response(PDU_ACK_CLASS_ACK, PDU_ACK_TYPE_ACK, srcId);
}
}
} }
} }
} }
@ -294,11 +320,10 @@ bool Data::process(uint8_t* data, uint32_t len)
} }
offset += P25_PDU_FEC_LENGTH_BITS; offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
} }
if (m_dumpPDUData && m_rfDataBlockCnt > 0U) { if (m_dumpPDUData && m_rfDataBlockCnt > 0U) {
Utils::dump(1U, "PDU Packet", m_pduUserData, dataOffset); Utils::dump(1U, "PDU Packet", m_pduUserData, m_pduUserDataLength);
} }
if (m_rfDataBlockCnt < blocksToFollow) { if (m_rfDataBlockCnt < blocksToFollow) {
@ -325,10 +350,6 @@ bool Data::process(uint8_t* data, uint32_t len)
ulong64_t ipAddr = (m_pduUserData[8U] << 24) + (m_pduUserData[9U] << 16) + ulong64_t ipAddr = (m_pduUserData[8U] << 24) + (m_pduUserData[9U] << 16) +
(m_pduUserData[10U] << 8) + m_pduUserData[11U]; (m_pduUserData[10U] << 8) + m_pduUserData[11U];
if (m_rfDataHeader.getAckNeeded()) {
writeRF_PDU_Ack_Response(PDU_ACK_CLASS_ACK, PDU_ACK_TYPE_ACK, llId);
}
if (m_verbose) { if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_REQ_CNCT (Registration Request Connect), llId = %u, ipAddr = %s", llId, __IP_FROM_ULONG(ipAddr).c_str()); LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_REQ_CNCT (Registration Request Connect), llId = %u, ipAddr = %s", llId, __IP_FROM_ULONG(ipAddr).c_str());
} }
@ -343,10 +364,6 @@ bool Data::process(uint8_t* data, uint32_t len)
{ {
uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U]; uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U];
if (m_rfDataHeader.getAckNeeded()) {
writeRF_PDU_Ack_Response(PDU_ACK_CLASS_ACK, PDU_ACK_TYPE_ACK, llId);
}
if (m_verbose) { if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_REQ_DISCNCT (Registration Request Disconnect), llId = %u", llId); LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_REQ_DISCNCT (Registration Request Disconnect), llId = %u", llId);
} }
@ -380,9 +397,6 @@ bool Data::process(uint8_t* data, uint32_t len)
} }
break; break;
default: default:
uint32_t srcId = (m_rfUseSecondHeader || m_rfExtendedAddress) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId();
uint32_t dstId = m_rfDataHeader.getLLId();
::ActivityLog("P25", true, "RF data transmission from %u to %u, %u blocks", srcId, dstId, m_rfDataHeader.getBlocksToFollow()); ::ActivityLog("P25", true, "RF data transmission from %u to %u, %u blocks", srcId, dstId, m_rfDataHeader.getBlocksToFollow());
if (m_repeatPDU) { if (m_repeatPDU) {
@ -574,7 +588,8 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
} }
m_netData[i].getData(m_pduUserData + dataOffset); m_netData[i].getData(m_pduUserData + dataOffset);
m_pduUserDataLength += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; dataOffset += (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
m_pduUserDataLength = dataOffset;
m_netDataBlockCnt++; m_netDataBlockCnt++;
@ -598,11 +613,10 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
} }
offset += P25_PDU_FEC_LENGTH_BYTES; offset += P25_PDU_FEC_LENGTH_BYTES;
dataOffset += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
} }
if (m_dumpPDUData && m_netDataBlockCnt > 0U) { if (m_dumpPDUData && m_netDataBlockCnt > 0U) {
Utils::dump(1U, "PDU Packet", m_pduUserData, dataOffset); Utils::dump(1U, "PDU Packet", m_pduUserData, m_pduUserDataLength);
} }
if (m_netDataBlockCnt < blocksToFollow) { if (m_netDataBlockCnt < blocksToFollow) {
@ -1067,8 +1081,9 @@ void Data::writeRF_PDU_Reg_Response(uint8_t regType, uint8_t mfId, uint32_t llId
/// <param name="ackClass"></param> /// <param name="ackClass"></param>
/// <param name="ackType"></param> /// <param name="ackType"></param>
/// <param name="llId"></param> /// <param name="llId"></param>
/// <param name="srcLlId"></param>
/// <param name="noNulls"></param> /// <param name="noNulls"></param>
void Data::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t llId, bool noNulls) void Data::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t llId, uint32_t srcLlId, bool noNulls)
{ {
if (ackClass == PDU_ACK_CLASS_ACK && ackType != PDU_ACK_TYPE_ACK) if (ackClass == PDU_ACK_CLASS_ACK && ackType != PDU_ACK_TYPE_ACK)
return; return;
@ -1090,11 +1105,11 @@ void Data::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t
rspHeader.setResponseStatus(m_rfDataHeader.getNs()); rspHeader.setResponseStatus(m_rfDataHeader.getNs());
rspHeader.setLLId(llId); rspHeader.setLLId(llId);
if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) { if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) {
rspHeader.setSrcLLId(P25_WUID_FNE); rspHeader.setSrcLLId(srcLlId);
rspHeader.setFullMessage(true); rspHeader.setFullMessage(false);
} }
else { else {
rspHeader.setFullMessage(false); rspHeader.setFullMessage(true);
} }
rspHeader.setBlocksToFollow(0U); rspHeader.setBlocksToFollow(0U);
@ -1102,5 +1117,10 @@ void Data::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t
rspHeader.encode(block); rspHeader.encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS); Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", response, ackClass = $%02X, ackType = $%02X, llId = %u, srcLLId = %u",
rspHeader.getResponseClass(), rspHeader.getResponseType(), rspHeader.getLLId(), rspHeader.getSrcLLId());
}
writeRF_PDU(data, bitLength, noNulls); writeRF_PDU(data, bitLength, noNulls);
} }

2
src/p25/packet/Data.h
Unescape View File

@ -138,7 +138,7 @@ namespace p25
/// <summary>Helper to write a PDU registration response.</summary> /// <summary>Helper to write a PDU registration response.</summary>
void writeRF_PDU_Reg_Response(uint8_t regType, uint8_t mfId, uint32_t llId, ulong64_t ipAddr); void writeRF_PDU_Reg_Response(uint8_t regType, uint8_t mfId, uint32_t llId, ulong64_t ipAddr);
/// <summary>Helper to write a PDU acknowledge response.</summary> /// <summary>Helper to write a PDU acknowledge response.</summary>
void writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t llId, bool noNulls = false); void writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t llId, uint32_t srcLlId = 0U, bool noNulls = false);
}; };
} // namespace packet } // namespace packet
} // namespace p25 } // namespace p25

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.