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;
// 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_NACK_ILLEGAL = 0x00U; // Illegal Format

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

@ -100,23 +100,20 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader& header)
Utils::dump(1U, "P25, DataBlock::decode(), PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES);
#endif
m_serialNo = (buffer[0] & 0xFEU) >> 1; // Confirmed Data Serial No.
uint16_t crc = ((buffer[0] & 0x01U) << 8) + buffer[1]; // CRC-9 Check Sum
m_serialNo = (buffer[0] & 0xFEU) >> 1; // Confirmed Data Serial No.
uint16_t crc = ((buffer[0] & 0x01U) << 8) + buffer[1]; // CRC-9 Check Sum
::memset(m_data, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
// 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) {
count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - 4U;
m_sap = buffer[5U] & 0x3FU; // Service Access Point
m_llId = (buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U]; // Logical Link ID
m_sap = buffer[5U] & 0x3FU; // Service Access Point
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 {
::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
@ -127,7 +124,7 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader& header)
#if DEBUG_P25_PDU_DATA
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
}
catch (...) {
@ -150,7 +147,7 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader& header)
Utils::dump(1U, "P25, DataBlock::decode(), PDU Unconfirmed Data Block", buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
#endif
::memcpy(m_data, buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES); // Payload Data
::memcpy(m_data, buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES); // Payload Data
}
catch (...) {
Utils::dump(2U, "P25, decoding excepted with input data", data, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
@ -177,13 +174,13 @@ void DataBlock::encode(uint8_t* data)
uint8_t buffer[P25_PDU_CONFIRMED_LENGTH_BYTES];
::memset(buffer, 0x00U, P25_PDU_CONFIRMED_LENGTH_BYTES);
buffer[0U] = ((m_serialNo << 1) & 0xFEU); // Confirmed Data Serial No.
buffer[0U] = ((m_serialNo << 1) & 0xFEU); // Confirmed Data Serial No.
// if this is extended addressing and the first block decode the SAP and LLId
if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) {
buffer[5U] = m_sap & 0x3FU; // Service Access Point
buffer[5U] = m_sap & 0x3FU; // Service Access Point
buffer[2U] = (m_llId >> 16) & 0xFFU; // Logical Link ID
buffer[2U] = (m_llId >> 16) & 0xFFU; // Logical Link ID
buffer[3U] = (m_llId >> 8) & 0xFFU;
buffer[4U] = (m_llId >> 0) & 0xFFU;
@ -194,8 +191,8 @@ void DataBlock::encode(uint8_t* data)
}
uint16_t crc = edac::CRC::crc9(buffer, 144U);
buffer[0U] = buffer[0U] + ((crc >> 8) & 0x01U); // CRC-9 Check Sum (b8)
buffer[1U] = (crc & 0xFFU); // CRC-9 Check Sum (b0 - b7)
buffer[0U] = buffer[0U] + ((crc >> 8) & 0x01U); // CRC-9 Check Sum (b8)
buffer[1U] = (crc & 0xFFU); // CRC-9 Check Sum (b0 - b7)
#if DEBUG_P25_PDU_DATA
Utils::dump(1U, "P25, DataBlock::encode(), PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES);

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

@ -215,6 +215,9 @@ bool Data::process(uint8_t* data, uint32_t len)
blocksToFollow--;
}
uint32_t srcId = m_rfDataHeader.getLLId();
uint32_t dstId = (m_rfUseSecondHeader || m_rfExtendedAddress) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId();
m_rfPDUCount++;
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.setLLId(m_rfData[i].getLLId());
m_rfSecondHeader.setSAP(m_rfData[i].getSAP());
dstId = m_rfSecondHeader.getLLId();
m_rfExtendedAddress = true;
}
else {
@ -262,7 +266,8 @@ bool Data::process(uint8_t* data, uint32_t len)
}
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,
// 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);
if (!crcRet) {
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;
dataOffset += (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}
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) {
@ -325,10 +350,6 @@ bool Data::process(uint8_t* data, uint32_t len)
ulong64_t ipAddr = (m_pduUserData[8U] << 24) + (m_pduUserData[9U] << 16) +
(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) {
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];
if (m_rfDataHeader.getAckNeeded()) {
writeRF_PDU_Ack_Response(PDU_ACK_CLASS_ACK, PDU_ACK_TYPE_ACK, llId);
}
if (m_verbose) {
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;
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());
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_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++;
@ -598,11 +613,10 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
}
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) {
Utils::dump(1U, "PDU Packet", m_pduUserData, dataOffset);
Utils::dump(1U, "PDU Packet", m_pduUserData, m_pduUserDataLength);
}
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="ackType"></param>
/// <param name="llId"></param>
/// <param name="srcLlId"></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)
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.setLLId(llId);
if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) {
rspHeader.setSrcLLId(P25_WUID_FNE);
rspHeader.setFullMessage(true);
rspHeader.setSrcLLId(srcLlId);
rspHeader.setFullMessage(false);
}
else {
rspHeader.setFullMessage(false);
rspHeader.setFullMessage(true);
}
rspHeader.setBlocksToFollow(0U);
@ -1102,5 +1117,10 @@ void Data::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t
rspHeader.encode(block);
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);
}

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

@ -138,7 +138,7 @@ namespace p25
/// <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);
/// <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 p25

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.