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);