From 9b43411376367173eb6dc2bb1811b9b46974c71f Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Mon, 10 Jul 2023 13:42:55 -0400 Subject: [PATCH] when decoding a PDU data block ensure class variables are set to reasonable defaults (since the actual DataBlock class tends to be reused); ensure DataHeader is passed to DataBlock decode() and setFormat() by reference; implement supporton DataHeader to retreive the raw byte contents of a decoded PDU header; ensure the PDU Rx process resets the second header data, irregardless of use; if we are using a PDU second header, ensure its data contents are added to pduUserData as the second header contents are CRC-32'ed along side all the data blocks (infact only the initial header is excluded from CRC-32 all other blocks following initial header *must* be CRC-32'ed with the data blocks); refactor PDU last block detection; ensure writeRF_PDU_Buffered and writeNet_PDU_Buffered regenerate the full packet CRC-32; --- src/p25/data/DataBlock.cpp | 40 ++++++++------- src/p25/data/DataBlock.h | 4 +- src/p25/data/DataHeader.cpp | 98 +++++++++++++++++++++++-------------- src/p25/data/DataHeader.h | 5 ++ src/p25/packet/Data.cpp | 62 ++++++++++++++++++++--- 5 files changed, 145 insertions(+), 64 deletions(-) diff --git a/src/p25/data/DataBlock.cpp b/src/p25/data/DataBlock.cpp index 1e477a12..bd39ff9a 100644 --- a/src/p25/data/DataBlock.cpp +++ b/src/p25/data/DataBlock.cpp @@ -71,7 +71,7 @@ DataBlock::~DataBlock() /// /// Instance of the DataHeader class. /// True, if data block was decoded, otherwise false. -bool DataBlock::decode(const uint8_t* data, const DataHeader header) +bool DataBlock::decode(const uint8_t* data, const DataHeader& header) { assert(data != nullptr); assert(m_data != nullptr); @@ -82,6 +82,11 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) m_fmt = header.getFormat(); m_headerSap = header.getSAP(); + // set these to reasonable defaults + m_serialNo = 0U; + m_lastBlock = false; + m_llId = 0U; + if (m_fmt == PDU_FMT_CONFIRMED) { // decode 3/4 rate Trellis try { @@ -91,16 +96,6 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) return false; } - // determine if this is the last data block - uint32_t count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; - if (m_serialNo == (header.getBlocksToFollow() - 1) && header.getBlocksToFollow() > 1) { - m_lastBlock = true; - } else { - if (header.getBlocksToFollow() <= 1) { - m_lastBlock = true; - } - } - #if DEBUG_P25_PDU_DATA Utils::dump(1U, "P25, DataBlock::decode(), PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); #endif @@ -111,6 +106,7 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) ::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; @@ -225,28 +221,36 @@ void DataBlock::encode(uint8_t* data) } } -/// Sets the data format. +/// +/// Sets the data format. +/// /// void DataBlock::setFormat(const uint8_t fmt) { m_fmt = fmt; } -/// Sets the data format from the data header. +/// +/// Sets the data format from the data header. +/// /// -void DataBlock::setFormat(const DataHeader header) +void DataBlock::setFormat(const DataHeader& header) { m_fmt = header.getFormat(); } -/// Gets the data format. +/// +/// Gets the data format. +/// /// uint8_t DataBlock::getFormat() const { return m_fmt; } -/// Sets the raw data stored in the data block. +/// +/// Sets the raw data stored in the data block. +/// /// void DataBlock::setData(const uint8_t* buffer) { @@ -264,7 +268,9 @@ void DataBlock::setData(const uint8_t* buffer) } } -/// Gets the raw data stored in the data block. +/// +/// Gets the raw data stored in the data block. +/// /// uint32_t DataBlock::getData(uint8_t* buffer) const { diff --git a/src/p25/data/DataBlock.h b/src/p25/data/DataBlock.h index 9b8b4f07..2817975a 100644 --- a/src/p25/data/DataBlock.h +++ b/src/p25/data/DataBlock.h @@ -49,14 +49,14 @@ namespace p25 ~DataBlock(); /// Decodes P25 PDU data block. - bool decode(const uint8_t* data, const DataHeader header); + bool decode(const uint8_t* data, const DataHeader& header); /// Encodes a P25 PDU data block. void encode(uint8_t* data); /// Sets the data format. void setFormat(const uint8_t fmt); /// Sets the data format from the data header. - void setFormat(const DataHeader header); + void setFormat(const DataHeader& header); /// Gets the data format. uint8_t getFormat() const; diff --git a/src/p25/data/DataHeader.cpp b/src/p25/data/DataHeader.cpp index 856aa0a5..d23a5c70 100644 --- a/src/p25/data/DataHeader.cpp +++ b/src/p25/data/DataHeader.cpp @@ -67,9 +67,11 @@ DataHeader::DataHeader() : m_trellis(), m_blocksToFollow(0U), m_padCount(0U), - m_dataOctets(0U) + m_dataOctets(0U), + m_data(nullptr) { - /* stub */ + m_data = new uint8_t[P25_PDU_HEADER_LENGTH_BYTES]; + ::memset(m_data, 0x00U, P25_PDU_HEADER_LENGTH_BYTES); } /// @@ -77,7 +79,7 @@ DataHeader::DataHeader() : /// DataHeader::~DataHeader() { - /* stub */ + delete[] m_data; } /// @@ -89,35 +91,32 @@ bool DataHeader::decode(const uint8_t* data) { assert(data != nullptr); - uint8_t header[P25_PDU_HEADER_LENGTH_BYTES]; - ::memset(header, 0x00U, P25_PDU_HEADER_LENGTH_BYTES); - // decode 1/2 rate Trellis & check CRC-CCITT 16 - bool valid = m_trellis.decode12(data, header); + bool valid = m_trellis.decode12(data, m_data); if (valid) - valid = edac::CRC::checkCCITT162(header, P25_PDU_HEADER_LENGTH_BYTES); + valid = edac::CRC::checkCCITT162(m_data, P25_PDU_HEADER_LENGTH_BYTES); if (!valid) { return false; } #if DEBUG_P25_PDU_DATA - Utils::dump(1U, "P25, DataHeader::decode(), PDU Header Data", header, P25_PDU_HEADER_LENGTH_BYTES); + Utils::dump(1U, "P25, DataHeader::decode(), PDU Header Data", m_data, P25_PDU_HEADER_LENGTH_BYTES); #endif - m_ackNeeded = (header[0U] & 0x40U) == 0x40U; // Acknowledge Needed - m_outbound = (header[0U] & 0x20U) == 0x20U; // Inbound/Outbound - m_fmt = header[0U] & 0x1FU; // Packet Format + m_ackNeeded = (m_data[0U] & 0x40U) == 0x40U; // Acknowledge Needed + m_outbound = (m_data[0U] & 0x20U) == 0x20U; // Inbound/Outbound + m_fmt = m_data[0U] & 0x1FU; // Packet Format - m_sap = header[1U] & 0x3FU; // Service Access Point + m_sap = m_data[1U] & 0x3FU; // Service Access Point - m_mfId = header[2U]; // Mfg Id. + m_mfId = m_data[2U]; // Mfg Id. - m_llId = (header[3U] << 16) + (header[4U] << 8) + header[5U]; // Logical Link ID + m_llId = (m_data[3U] << 16) + (m_data[4U] << 8) + m_data[5U]; // Logical Link ID - m_F = (header[6U] & 0x80U) == 0x80U; // Full Message Flag - m_blocksToFollow = header[6U] & 0x7FU; // Block Frames to Follow + m_F = (m_data[6U] & 0x80U) == 0x80U; // Full Message Flag + m_blocksToFollow = m_data[6U] & 0x7FU; // Block Frames to Follow - m_padCount = header[7U] & 0x1FU; // Pad Count + m_padCount = m_data[7U] & 0x1FU; // Pad Count if (m_fmt == PDU_FMT_RSP || m_fmt == PDU_FMT_AMBT) { m_padCount = 0; } @@ -131,28 +130,28 @@ bool DataHeader::decode(const uint8_t* data) switch (m_fmt) { case PDU_FMT_CONFIRMED: - m_S = (header[8U] & 0x80U) == 0x80U; // Re-synchronize Flag + m_S = (m_data[8U] & 0x80U) == 0x80U; // Re-synchronize Flag - m_Ns = (header[8U] >> 4) & 0x07U; // Packet Sequence No. - m_fsn = header[8U] & 0x07U; // Fragment Sequence No. - m_lastFragment = (header[8U] & 0x08U) == 0x08U; // Last Fragment Flag + m_Ns = (m_data[8U] >> 4) & 0x07U; // Packet Sequence No. + m_fsn = m_data[8U] & 0x07U; // Fragment Sequence No. + m_lastFragment = (m_data[8U] & 0x08U) == 0x08U; // Last Fragment Flag - m_headerOffset = header[9U] & 0x3FU; // Data Header Offset + m_headerOffset = m_data[9U] & 0x3FU; // Data Header Offset break; case PDU_FMT_RSP: m_ackNeeded = false; m_sap = PDU_SAP_USER_DATA; - m_rspClass = (header[1U] >> 6) & 0x03U; // Response Class - m_rspType = (header[1U] >> 3) & 0x07U; // Response Type - m_rspStatus = header[1U] & 0x07U; // Response Status + m_rspClass = (m_data[1U] >> 6) & 0x03U; // Response Class + m_rspType = (m_data[1U] >> 3) & 0x07U; // Response Type + m_rspStatus = m_data[1U] & 0x07U; // Response Status if (!m_F) { - m_srcLlId = (header[7U] << 16) + (header[8U] << 8) + header[9U]; // Source Logical Link ID + m_srcLlId = (m_data[7U] << 16) + (m_data[8U] << 8) + m_data[9U]; // Source Logical Link ID } break; case PDU_FMT_AMBT: - m_ambtOpcode = header[7U] & 0x3FU; // AMBT Opcode - m_ambtField8 = header[8U]; // AMBT Field 8 - m_ambtField9 = header[9U]; // AMBT Field 9 + m_ambtOpcode = m_data[7U] & 0x3FU; // AMBT Opcode + m_ambtField8 = m_data[8U]; // AMBT Field 8 + m_ambtField9 = m_data[9U]; // AMBT Field 9 // fall-thru case PDU_FMT_UNCONFIRMED: default: @@ -207,9 +206,8 @@ void DataHeader::encode(uint8_t* data) switch (m_fmt) { case PDU_FMT_CONFIRMED: header[7U] = (m_padCount & 0x1FU); // Pad Count - header[8U] = (m_S ? 0x80U : 0x00U) + // Re-synchronize Flag - ((m_Ns << 4) & 0x07U) + // Packet Sequence No. + ((m_Ns & 0x07U) << 4) + // Packet Sequence No. (m_lastFragment ? 0x08U : 0x00U) + // Last Fragment Flag (m_fsn & 0x07); // Fragment Sequence No. @@ -232,6 +230,7 @@ void DataHeader::encode(uint8_t* data) break; case PDU_FMT_UNCONFIRMED: default: + header[7U] = (m_padCount & 0x1FU); // Pad Count header[8U] = 0x00U; header[9U] = m_headerOffset & 0x3FU; // Data Header Offset break; @@ -284,17 +283,36 @@ void DataHeader::reset() m_ambtOpcode = 0U; m_ambtField8 = 0U; m_ambtField9 = 0U; + + ::memset(m_data, 0x00U, P25_PDU_HEADER_LENGTH_BYTES); } -/// Gets the total number of data octets. +/// +/// Gets the total number of data octets. +/// /// uint32_t DataHeader::getDataOctets() const { return m_dataOctets; } +/// +/// Gets the raw header data. +/// +/// +uint32_t DataHeader::getData(uint8_t* buffer) const +{ + assert(buffer != nullptr); + assert(m_data != nullptr); + + ::memcpy(buffer, m_data, P25_PDU_HEADER_LENGTH_BYTES); + return P25_PDU_HEADER_LENGTH_BYTES; +} + /** Common Data */ -/// Sets the total number of blocks to follow this header. +/// +/// Sets the total number of blocks to follow this header. +/// /// void DataHeader::setBlocksToFollow(uint8_t blocksToFollow) { @@ -309,14 +327,18 @@ void DataHeader::setBlocksToFollow(uint8_t blocksToFollow) } } -/// Gets the total number of blocks to follow this header. +/// +/// Gets the total number of blocks to follow this header. +/// /// uint8_t DataHeader::getBlocksToFollow() const { return m_blocksToFollow; } -/// Sets the count of block padding. +/// +/// Sets the count of block padding. +/// /// void DataHeader::setPadCount(uint8_t padCount) { @@ -331,7 +353,9 @@ void DataHeader::setPadCount(uint8_t padCount) } } -/// Gets the count of block padding. +/// +/// Gets the count of block padding. +/// /// uint8_t DataHeader::getPadCount() const { diff --git a/src/p25/data/DataHeader.h b/src/p25/data/DataHeader.h index bb013c39..dc8f4ee9 100644 --- a/src/p25/data/DataHeader.h +++ b/src/p25/data/DataHeader.h @@ -58,6 +58,9 @@ namespace p25 /// Gets the total number of data octets. uint32_t getDataOctets() const; + /// Gets the raw header data. + uint32_t getData(uint8_t* buffer) const; + /** Common Data */ /// Sets the total number of blocks to follow this header. void setBlocksToFollow(uint8_t blocksToFollow); @@ -119,6 +122,8 @@ namespace p25 uint8_t m_blocksToFollow; uint8_t m_padCount; uint32_t m_dataOctets; + + uint8_t* m_data; }; } // namespace data } // namespace p25 diff --git a/src/p25/packet/Data.cpp b/src/p25/packet/Data.cpp index b35d83a8..9a5bba27 100644 --- a/src/p25/packet/Data.cpp +++ b/src/p25/packet/Data.cpp @@ -134,6 +134,7 @@ bool Data::process(uint8_t* data, uint32_t len) Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES); m_rfDataHeader.reset(); + m_rfSecondHeader.reset(); m_rfDataBlockCnt = 0U; m_rfPDUCount = 0U; m_rfPDUBits = 0U; @@ -153,6 +154,7 @@ bool Data::process(uint8_t* data, uint32_t len) LogError(LOG_RF, P25_PDU_STR ", too many PDU blocks to process, %u > %u", m_rfDataHeader.getBlocksToFollow(), P25_MAX_PDU_COUNT); m_rfDataHeader.reset(); + m_rfSecondHeader.reset(); m_rfDataBlockCnt = 0U; m_rfPDUCount = 0U; m_rfPDUBits = 0U; @@ -219,11 +221,29 @@ bool Data::process(uint8_t* data, uint32_t len) if (m_rfPDUBits >= bitLength) { // process all blocks in the data stream uint32_t dataOffset = 0U; + + // if we are using a secondary header place it in the PDU user data buffer + if (m_rfUseSecondHeader) { + m_rfSecondHeader.getData(m_pduUserData + dataOffset); + dataOffset += P25_PDU_HEADER_LENGTH_BYTES; + m_pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES; + } + + // decode data blocks for (uint32_t i = 0U; i < blocksToFollow; i++) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); Utils::getBitRange(m_rfPDU, buffer, offset, P25_PDU_FEC_LENGTH_BITS); bool ret = m_rfData[i].decode(buffer, (m_rfUseSecondHeader) ? m_rfSecondHeader : m_rfDataHeader); if (ret) { + // if we are getting unconfirmed or confirmed blocks, and if we've reached the total number of blocks + // set this block as the last block for full packet CRC + if ((m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) || (m_rfDataHeader.getFormat() == PDU_FMT_UNCONFIRMED)) { + if ((m_rfDataBlockCnt + 1U) == blocksToFollow) { + m_rfData[i].setLastBlock(true); + } + } + + // are we processing extended address data from the first block? if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR && m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED && m_rfData[i].getSerialNo() == 0U) { LogMessage(LOG_RF, P25_PDU_STR ", block %u, fmt = $%02X, lastBlock = %u, sap = $%02X, llId = %u", @@ -430,6 +450,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES); m_netDataHeader.reset(); + m_netSecondHeader.reset(); m_netDataBlockCnt = 0U; m_netPDUCount = 0U; m_p25->m_netState = RS_NET_IDLE; @@ -448,6 +469,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) LogError(LOG_NET, P25_PDU_STR ", too many PDU blocks to process, %u > %u", m_netDataHeader.getBlocksToFollow(), P25_MAX_PDU_COUNT); m_netDataHeader.reset(); + m_netSecondHeader.reset(); m_netDataOffset = 0U; m_netDataBlockCnt = 0U; m_netPDUCount = 0U; @@ -510,12 +532,30 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) // process all blocks in the data stream uint32_t dataOffset = 0U; + + // if we are using a secondary header place it in the PDU user data buffer + if (m_netUseSecondHeader) { + m_netSecondHeader.getData(m_pduUserData + dataOffset); + dataOffset += P25_PDU_HEADER_LENGTH_BYTES; + m_pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES; + } + + // decode data blocks for (uint32_t i = 0U; i < blocksToFollow; i++) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); ::memcpy(buffer, m_netPDU + offset, P25_PDU_FEC_LENGTH_BYTES); bool ret = m_netData[i].decode(buffer, (m_netUseSecondHeader) ? m_netSecondHeader : m_netDataHeader); if (ret) { + // if we are getting unconfirmed or confirmed blocks, and if we've reached the total number of blocks + // set this block as the last block for full packet CRC + if ((m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) || (m_netDataHeader.getFormat() == PDU_FMT_UNCONFIRMED)) { + if ((m_netDataBlockCnt + 1U) == blocksToFollow) { + m_netData[i].setLastBlock(true); + } + } + + // are we processing extended address data from the first block? if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR && m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED && m_netData[i].getSerialNo() == 0U) { LogMessage(LOG_NET, P25_PDU_STR ", block %u, fmt = $%02X, lastBlock = %u, sap = $%02X, llId = %u", @@ -867,12 +907,15 @@ void Data::writeNet_PDU_Buffered() uint32_t blocksToFollow = m_netDataHeader.getBlocksToFollow(); - // Generate the PDU header and 1/2 rate Trellis + // generate the PDU header and 1/2 rate Trellis m_netDataHeader.encode(block); Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS); offset += P25_PDU_FEC_LENGTH_BITS; - // Generate the second PDU header + uint32_t dataOffset = 0U; + edac::CRC::addCRC32(m_pduUserData, m_pduUserDataLength); + + // generate the second PDU header if (m_netUseSecondHeader) { ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); @@ -880,11 +923,11 @@ void Data::writeNet_PDU_Buffered() Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS); offset += P25_PDU_FEC_LENGTH_BITS; + dataOffset += P25_PDU_HEADER_LENGTH_BYTES; blocksToFollow--; } - // Generate the PDU data - uint32_t dataOffset = 0U; + // generate the PDU data for (uint32_t i = 0U; i < blocksToFollow; i++) { m_netData[i].setFormat((m_netUseSecondHeader) ? m_netSecondHeader : m_netDataHeader); m_netData[i].setSerialNo(i); @@ -917,12 +960,15 @@ void Data::writeRF_PDU_Buffered() uint32_t blocksToFollow = m_rfDataHeader.getBlocksToFollow(); - // Generate the PDU header and 1/2 rate Trellis + // generate the PDU header and 1/2 rate Trellis m_rfDataHeader.encode(block); Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS); offset += P25_PDU_FEC_LENGTH_BITS; - // Generate the second PDU header + uint32_t dataOffset = 0U; + edac::CRC::addCRC32(m_pduUserData, m_pduUserDataLength); + + // generate the second PDU header if (m_rfUseSecondHeader) { ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); @@ -930,11 +976,11 @@ void Data::writeRF_PDU_Buffered() Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS); offset += P25_PDU_FEC_LENGTH_BITS; + dataOffset += P25_PDU_HEADER_LENGTH_BYTES; blocksToFollow--; } - // Generate the PDU data - uint32_t dataOffset = 0U; + // generate the PDU data for (uint32_t i = 0U; i < blocksToFollow; i++) { m_rfData[i].setFormat((m_rfUseSecondHeader) ? m_rfSecondHeader : m_rfDataHeader); m_rfData[i].setSerialNo(i);