diff --git a/network/BaseNetwork.cpp b/network/BaseNetwork.cpp
index 34db2ba5..9e70d420 100644
--- a/network/BaseNetwork.cpp
+++ b/network/BaseNetwork.cpp
@@ -226,14 +226,21 @@ uint8_t* BaseNetwork::readP25(bool& ret, p25::lc::LC& control, p25::data::LowSpe
uint8_t* data = NULL;
len = m_buffer[23U];
- if (len <= 24) {
- data = new uint8_t[len];
- ::memset(data, 0x00U, len);
+ if (duid == p25::P25_DUID_PDU) {
+ data = new uint8_t[length];
+ ::memset(data, 0x00U, length);
+ ::memcpy(data, m_buffer, length);
}
else {
- data = new uint8_t[len];
- ::memset(data, 0x00U, len);
- ::memcpy(data, m_buffer + 24U, len);
+ if (len <= 24) {
+ data = new uint8_t[len];
+ ::memset(data, 0x00U, len);
+ }
+ else {
+ data = new uint8_t[len];
+ ::memset(data, 0x00U, len);
+ ::memcpy(data, m_buffer + 24U, len);
+ }
}
ret = true;
@@ -359,12 +366,14 @@ bool BaseNetwork::writeP25TSDU(const p25::lc::TSBK& tsbk, const uint8_t* data)
///
/// Writes P25 PDU frame data to the network.
///
-///
-///
+///
+///
+///
///
///
///
-bool BaseNetwork::writeP25PDU(const uint32_t llId, const uint8_t dataType, const uint8_t* data, const uint32_t len)
+bool BaseNetwork::writeP25PDU(const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader, const uint8_t currentBlock,
+ const uint8_t* data, const uint32_t len)
{
if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING)
return false;
@@ -375,7 +384,7 @@ bool BaseNetwork::writeP25PDU(const uint32_t llId, const uint8_t dataType, const
m_streamId[0] = m_p25StreamId;
- return writeP25PDU(m_id, m_p25StreamId, llId, dataType, data, len);
+ return writeP25PDU(m_id, m_p25StreamId, header, secHeader, currentBlock, data, len);
}
///
@@ -483,7 +492,7 @@ bool BaseNetwork::writeDMR(const uint32_t id, const uint32_t streamId, const dmr
uint32_t dstId = data.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U);
- __SET_UINT32(id, buffer, 11U);
+ __SET_UINT32(id, buffer, 11U); // Peer ID
uint32_t slotNo = data.getSlotNo();
@@ -521,7 +530,7 @@ bool BaseNetwork::writeDMR(const uint32_t id, const uint32_t streamId, const dmr
buffer[4U] = data.getSeqNo(); // Sequence Number
- __SET_UINT32(streamId, buffer, 16U);
+ __SET_UINT32(streamId, buffer, 16U); // Stream ID
data.getData(buffer + 20U);
@@ -569,11 +578,11 @@ bool BaseNetwork::writeP25LDU1(const uint32_t id, const uint32_t streamId, const
uint32_t dstId = control.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U);
- __SET_UINT32(id, buffer, 11U);
+ __SET_UINT32(id, buffer, 11U); // Peer ID
buffer[15U] = control.getMFId(); // MFId
- __SET_UINT32(streamId, buffer, 16U);
+ __SET_UINT32(streamId, buffer, 16U); // Stream ID
buffer[20U] = lsd.getLSD1(); // LSD 1
buffer[21U] = lsd.getLSD2(); // LSD 2
@@ -670,11 +679,11 @@ bool BaseNetwork::writeP25LDU2(const uint32_t id, const uint32_t streamId, const
uint32_t dstId = control.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U);
- __SET_UINT32(id, buffer, 11U);
+ __SET_UINT32(id, buffer, 11U); // Peer ID
buffer[15U] = control.getMFId(); // MFId
- __SET_UINT32(streamId, buffer, 16U);
+ __SET_UINT32(streamId, buffer, 16U); // Stream ID
buffer[20U] = lsd.getLSD1(); // LSD 1
buffer[21U] = lsd.getLSD2(); // LSD 2
@@ -765,11 +774,11 @@ bool BaseNetwork::writeP25TDU(const uint32_t id, const uint32_t streamId, const
uint32_t dstId = control.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U);
- __SET_UINT32(id, buffer, 11U);
+ __SET_UINT32(id, buffer, 11U); // Peer ID
buffer[15U] = control.getMFId(); // MFId
- __SET_UINT32(streamId, buffer, 16U);
+ __SET_UINT32(streamId, buffer, 16U); // Stream ID
buffer[20U] = lsd.getLSD1(); // LSD 1
buffer[21U] = lsd.getLSD2(); // LSD 2
@@ -813,11 +822,11 @@ bool BaseNetwork::writeP25TSDU(const uint32_t id, const uint32_t streamId, const
uint32_t dstId = tsbk.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U);
- __SET_UINT32(id, buffer, 11U);
+ __SET_UINT32(id, buffer, 11U); // Peer ID
buffer[15U] = tsbk.getMFId(); // MFId
- __SET_UINT32(streamId, buffer, 16U);
+ __SET_UINT32(streamId, buffer, 16U); // Stream ID
buffer[20U] = 0U; // Reserved (LSD 1)
buffer[21U] = 0U; // Reserved (LSD 2)
@@ -844,17 +853,26 @@ bool BaseNetwork::writeP25TSDU(const uint32_t id, const uint32_t streamId, const
///
///
///
-///
-///
+///
+///
+///
///
///
///
-bool BaseNetwork::writeP25PDU(const uint32_t id, const uint32_t streamId, const uint32_t llId, const uint8_t dataType, const uint8_t* data,
- const uint32_t len)
+bool BaseNetwork::writeP25PDU(const uint32_t id, const uint32_t streamId, const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader,
+ const uint8_t currentBlock, const uint8_t* data, const uint32_t len)
{
if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING)
return false;
+ bool useSecondHeader = false;
+
+ // process second header if we're using enhanced addressing
+ if (header.getSAP() == p25::PDU_SAP_EXT_ADDR &&
+ header.getFormat() == p25::PDU_FMT_UNCONFIRMED) {
+ useSecondHeader = true;
+ }
+
assert(data != NULL);
uint8_t buffer[DATA_PACKET_LENGTH];
@@ -862,20 +880,24 @@ bool BaseNetwork::writeP25PDU(const uint32_t id, const uint32_t streamId, const
::memcpy(buffer + 0U, TAG_P25_DATA, 4U);
- buffer[4U] = dataType; // Data Type (LCO)
+ buffer[4U] = header.getSAP(); // Service Access Point
+ if (header.getFormat() == p25::PDU_FMT_CONFIRMED) {
+ buffer[4U] |= 0x80U;
+ }
- __SET_UINT16(llId, buffer, 5U); // Logical Link Address (Source Address)
+ uint32_t llId = (useSecondHeader) ? secHeader.getLLId() : header.getLLId();
+ __SET_UINT16(llId, buffer, 5U); // Logical Link Address
- __SET_UINT16(len, buffer, 8U); // PDU Length [bytes] (Target Address)
+ __SET_UINT16(len, buffer, 8U); // PDU Length [bytes]
- __SET_UINT32(id, buffer, 11U);
+ __SET_UINT32(id, buffer, 11U); // Peer ID
- buffer[15U] = p25::P25_MFG_STANDARD; // MFId
+ buffer[15U] = header.getMFId(); // MFId
- __SET_UINT32(streamId, buffer, 16U);
+ __SET_UINT32(streamId, buffer, 16U); // Stream ID
- buffer[20U] = 0U; // Reserved (LSD 1)
- buffer[21U] = 0U; // Reserved (LSD 2)
+ buffer[20U] = header.getBlocksToFollow(); // Blocks To Follow
+ buffer[21U] = currentBlock; // Current Block
buffer[22U] = p25::P25_DUID_PDU; // DUID
diff --git a/network/BaseNetwork.h b/network/BaseNetwork.h
index 7c60ca5c..a6254a66 100644
--- a/network/BaseNetwork.h
+++ b/network/BaseNetwork.h
@@ -141,7 +141,8 @@ namespace network
/// Writes P25 TSDU frame data to the network.
virtual bool writeP25TSDU(const p25::lc::TSBK& control, const uint8_t* data);
/// Writes P25 PDU frame data to the network.
- virtual bool writeP25PDU(const uint32_t llId, const uint8_t dataType, const uint8_t* data, const uint32_t len);
+ virtual bool writeP25PDU(const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader, const uint8_t currentBlock,
+ const uint8_t* data, const uint32_t len);
/// Writes the local activity log to the network.
virtual bool writeActLog(const char* message);
@@ -207,7 +208,8 @@ namespace network
/// Writes P25 TSDU frame data to the network.
bool writeP25TSDU(const uint32_t id, const uint32_t streamId, const p25::lc::TSBK& control, const uint8_t* data);
/// Writes P25 PDU frame data to the network.
- bool writeP25PDU(const uint32_t id, const uint32_t streamId, const uint32_t llId, const uint8_t dataType, const uint8_t* data, const uint32_t len);
+ bool writeP25PDU(const uint32_t id, const uint32_t streamId, const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader, const uint8_t currentBlock,
+ const uint8_t* data, const uint32_t len);
/// Writes data to the network.
virtual bool write(const uint8_t* data, uint32_t length);
diff --git a/p25/DataPacket.cpp b/p25/DataPacket.cpp
index cf6b2b77..14bf31c9 100644
--- a/p25/DataPacket.cpp
+++ b/p25/DataPacket.cpp
@@ -150,8 +150,6 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
m_p25->m_rfState = m_prevRfState;
return false;
}
-
- writeNetworkRF(P25_DT_DATA_HEADER, buffer, P25_PDU_FEC_LENGTH_BYTES);
}
if (m_p25->m_rfState == RS_RF_DATA) {
@@ -183,8 +181,6 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
m_rfSecondHeader.getHeaderOffset(), m_rfSecondHeader.getLLId());
}
- writeNetworkRF(P25_DT_DATA_SEC_HEADER, buffer, P25_PDU_FEC_LENGTH_BYTES);
-
m_rfUseSecondHeader = true;
offset += P25_PDU_FEC_LENGTH_BITS;
@@ -222,7 +218,6 @@ bool DataPacket::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;
- m_rfDataBlockCnt++;
// is this the last block?
if (m_rfData[i].getLastBlock()) {
@@ -232,7 +227,8 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
}
}
- writeNetworkRF(P25_DT_DATA, buffer, P25_PDU_FEC_LENGTH_BYTES);
+ writeNetworkRF(m_rfDataBlockCnt, m_pduUserData + dataOffset, (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES);
+ m_rfDataBlockCnt++;
}
else {
if (m_rfData[i].getFormat() == PDU_FMT_CONFIRMED)
@@ -392,117 +388,90 @@ bool DataPacket::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, da
switch (duid) {
case P25_DUID_PDU:
{
- uint32_t pduLen = control.getDstId(); // PDU's use dstId as the PDU len
- ::memset(m_netPDU, 0x00U, pduLen + 2U);
- ::memcpy(m_netPDU, data, pduLen);
-
- uint8_t dataType = control.getLCO();
- if (dataType == P25_DT_DATA_HEADER) {
- writeNet_PDU_Header();
- }
- else if (dataType == P25_DT_DATA_SEC_HEADER) {
- writeNet_PDU_Sec_Header();
- }
- else if (dataType == P25_DT_DATA) {
- writeNet_PDU();
- ::ActivityLog("P25", false, "network data transmission from %u to %u, %u blocks", m_netDataHeader.getLLId(), m_netDataHeader.getLLId(), m_netDataHeader.getBlocksToFollow());
- }
+ if (m_p25->m_netState != RS_NET_DATA) {
+ m_netDataHeader.reset();
+ m_netSecondHeader.reset();
+ m_netDataOffset = 0U;
+ m_netDataBlockCnt = 0U;
+ m_netPDUCount = 0U;
+
+ m_p25->m_netState = RS_NET_DATA;
+
+ uint8_t blocksToFollow = data[20U];
+ bool confirmed = (data[4U] & 0x80U) == 0x80U;
+ bool response = (data[4U] & 0x40U) == 0x40U;
+ uint8_t sap = data[4U] & 0x3FU;
+
+ m_netDataHeader.setAckNeeded(confirmed);
+ m_netDataHeader.setOutbound(true);
+ m_netDataHeader.setFormat((confirmed) ? PDU_FMT_CONFIRMED : PDU_FMT_UNCONFIRMED);
+ m_netDataHeader.setSAP(sap);
+ m_netDataHeader.setFullMessage(true);
+ m_netDataHeader.setBlocksToFollow(blocksToFollow);
- if (m_p25->m_netState == RS_NET_DATA) {
- if (m_netDataBlockCnt >= m_netBlocksToFollow) {
- if (m_dumpPDUData) {
- Utils::dump(1U, "PDU Packet", m_pduUserData, m_netDataOffset);
- }
+ if (m_verbose) {
+ LogMessage(LOG_NET, P25_PDU_STR ", ack = %u, outbound = %u, fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padCount = %u, n = %u, seqNo = %u, hdrOffset = %u",
+ m_netDataHeader.getAckNeeded(), m_netDataHeader.getOutbound(), m_netDataHeader.getFormat(), m_netDataHeader.getSAP(), m_netDataHeader.getFullMessage(),
+ m_netDataHeader.getBlocksToFollow(), m_netDataHeader.getPadCount(), m_netDataHeader.getNs(), m_netDataHeader.getFSN(),
+ m_netDataHeader.getHeaderOffset());
}
- // write data to RF interface?
- if (m_repeatPDU) {
- if (m_netDataBlockCnt >= m_netBlocksToFollow) {
- uint32_t bitLength = ((m_netDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
- uint32_t offset = P25_PREAMBLE_LENGTH_BITS;
-
- ::memset(m_netPDU, 0x00U, P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U);
-
- uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
- ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
-
- uint32_t blocksToFollow = m_netDataHeader.getBlocksToFollow();
-
- // Generate the PDU header and 1/2 rate Trellis
- m_netDataHeader.encode(buffer);
- Utils::setBitRange(buffer, m_netPDU, offset, P25_PDU_FEC_LENGTH_BITS);
- offset += P25_PDU_FEC_LENGTH_BITS;
+ // make sure we don't get a PDU with more blocks then we support
+ if (m_netDataHeader.getBlocksToFollow() >= P25_MAX_PDU_COUNT) {
+ LogError(LOG_NET, P25_PDU_STR ", too many PDU blocks to process, %u > %u", m_netDataHeader.getBlocksToFollow(), P25_MAX_PDU_COUNT);
- // Generate the second PDU header
- if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR) {
- m_netSecondHeader.encode(buffer);
- Utils::setBitRange(buffer, m_netPDU, offset, P25_PDU_FEC_LENGTH_BITS);
-
- offset += P25_PDU_FEC_LENGTH_BITS;
- blocksToFollow--;
- }
-
- // Generate the PDU data
- uint32_t dataOffset = 0U;
- for (uint32_t i = 0U; i < blocksToFollow; i++) {
- m_netData[i].setFormat((m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR) ? m_netSecondHeader : m_netDataHeader);
- m_netData[i].setSerialNo(i);
- m_netData[i].setData(m_pduUserData + dataOffset);
-
- ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
- m_netData[i].encode(buffer);
- Utils::setBitRange(buffer, m_netPDU, offset, P25_PDU_FEC_LENGTH_BITS);
-
- offset += P25_PDU_FEC_LENGTH_BITS;
- dataOffset += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
- }
-
- if (m_debug) {
- Utils::dump(2U, "!!! *Raw PDU Frame Data - P25_DUID_PDU", m_netPDU, bitLength / 8U);
- }
-
- uint8_t pdu[P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U];
-
- // Add the data
- uint32_t newBitLength = P25Utils::encode(m_netPDU, pdu + 2U, bitLength);
- uint32_t newByteLength = newBitLength / 8U;
- if ((newBitLength % 8U) > 0U)
- newByteLength++;
-
- // Regenerate Sync
- Sync::addP25Sync(pdu + 2U);
+ m_netDataHeader.reset();
+ m_netDataOffset = 0U;
+ m_netDataBlockCnt = 0U;
+ m_netPDUCount = 0U;
+ m_p25->m_netState = RS_NET_IDLE;
+ return false;
+ }
- // Regenerate NID
- m_p25->m_nid.encode(pdu + 2U, P25_DUID_PDU);
+ if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR &&
+ m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) {
+ LogWarning(LOG_NET, P25_PDU_STR ", unsupported confirmed enhanced addressing");
+
+ m_netDataHeader.reset();
+ m_netSecondHeader.reset();
+ m_netDataOffset = 0U;
+ m_netDataBlockCnt = 0U;
+ m_netPDUCount = 0U;
+ m_p25->m_netState = RS_NET_IDLE;
+ return false;
+ }
- // Add busy bits
- m_p25->addBusyBits(pdu + 2U, newBitLength, false, true);
+ ::ActivityLog("P25", false, "network data transmission from %u to %u, %u blocks", m_netDataHeader.getLLId(), m_netDataHeader.getLLId(), m_netDataHeader.getBlocksToFollow());
+ }
- ::memset(m_netPDU, 0x00U, P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U);
+ if (m_p25->m_netState == RS_NET_DATA) {
+ uint32_t pduLen = control.getDstId(); // PDU's use dstId as the PDU len
+ ::memset(m_netPDU, 0x00U, pduLen + 2U);
+ ::memcpy(m_netPDU, data, pduLen);
- if (m_p25->m_duplex) {
- pdu[0U] = TAG_DATA;
- pdu[1U] = 0x00U;
- m_p25->writeQueueNet(pdu, newByteLength + 2U);
- }
+ if (m_netDataBlockCnt >= m_netDataHeader.getBlocksToFollow()) {
+ if (m_dumpPDUData) {
+ Utils::dump(1U, "PDU Packet", m_pduUserData, m_netDataOffset);
+ }
- // add trailing null pad; only if control data isn't being transmitted
- if (!m_p25->m_ccRunning) {
- m_p25->writeRF_Nulls();
- }
+ writeNet_PDU_Buffered();
- ::ActivityLog("P25", true, "end of RF data transmission");
+ ::ActivityLog("P25", true, "end of RF data transmission");
- m_netDataHeader.reset();
- m_netSecondHeader.reset();
- m_netBlocksToFollow = 0U;
- m_netDataBlockCnt = 0U;
- m_netBitOffset = 0U;
- m_netDataOffset = 0U;
- m_p25->m_netState = RS_NET_IDLE;
- }
+ m_netDataHeader.reset();
+ m_netSecondHeader.reset();
+ m_netDataOffset = 0U;
+ m_netDataBlockCnt = 0U;
+ m_netPDUCount = 0U;
+ m_p25->m_netState = RS_NET_IDLE;
}
- } // if (m_netState == RS_NET_DATA)
+ else {
+ uint32_t len = __GET_UINT16(data, 8U);
+ ::memcpy(m_pduUserData, data + 24U, len);
+ m_netDataOffset += len;
+ m_netDataBlockCnt++;
+ }
+ }
}
break;
default:
@@ -560,11 +529,11 @@ DataPacket::DataPacket(Control* p25, network::BaseNetwork* network, bool dumpPDU
m_netData(NULL),
m_netDataHeader(),
m_netSecondHeader(),
- m_netBlocksToFollow(0U),
- m_netDataBlockCnt(0U),
- m_netBitOffset(0U),
+ m_netUseSecondHeader(false),
m_netDataOffset(0U),
+ m_netDataBlockCnt(0U),
m_netPDU(NULL),
+ m_netPDUCount(0U),
m_pduUserData(NULL),
m_pduUserDataLength(0U),
m_fneRegTable(),
@@ -602,10 +571,10 @@ DataPacket::~DataPacket()
///
/// Write data processed from RF to the network.
///
-///
+///
///
///
-void DataPacket::writeNetworkRF(const uint8_t dataType, const uint8_t *data, uint32_t len)
+void DataPacket::writeNetworkRF(const uint8_t currentBlock, const uint8_t *data, uint32_t len)
{
assert(data != NULL);
@@ -615,7 +584,7 @@ void DataPacket::writeNetworkRF(const uint8_t dataType, const uint8_t *data, uin
if (m_p25->m_rfTimeout.isRunning() && m_p25->m_rfTimeout.hasExpired())
return;
- m_network->writeP25PDU((m_rfUseSecondHeader) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId(), dataType, data, len);
+ m_network->writeP25PDU(m_rfDataHeader, m_rfSecondHeader, currentBlock, data, len);
}
///
@@ -664,6 +633,57 @@ void DataPacket::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNull
}
}
+///
+/// Helper to write a network P25 PDU packet.
+///
+/// This will take buffered network PDU data and repeat it over the air.
+void DataPacket::writeNet_PDU_Buffered()
+{
+ uint32_t bitLength = ((m_netDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
+ uint32_t offset = P25_PREAMBLE_LENGTH_BITS;
+
+ uint8_t* data = new uint8_t[bitLength / 8U];
+ ::memset(data, 0x00U, bitLength / 8U);
+ uint8_t block[P25_PDU_FEC_LENGTH_BYTES];
+ ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
+
+ uint32_t blocksToFollow = m_netDataHeader.getBlocksToFollow();
+
+ // 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
+ if (m_netUseSecondHeader) {
+ ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
+
+ m_netSecondHeader.encode(block);
+ Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
+
+ offset += P25_PDU_FEC_LENGTH_BITS;
+ blocksToFollow--;
+ }
+
+ // Generate the PDU data
+ uint32_t dataOffset = 0U;
+ for (uint32_t i = 0U; i < blocksToFollow; i++) {
+ m_netData[i].setFormat((m_netUseSecondHeader) ? m_netSecondHeader : m_netDataHeader);
+ m_netData[i].setSerialNo(i);
+ m_netData[i].setData(m_pduUserData + dataOffset);
+
+ ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
+ m_netData[i].encode(block);
+ Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
+
+ offset += P25_PDU_FEC_LENGTH_BITS;
+ dataOffset += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
+ }
+
+ writeRF_PDU(data, bitLength);
+ delete[] data;
+}
+
///
/// Helper to re-write a received P25 PDU packet.
///
@@ -822,166 +842,3 @@ void DataPacket::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uin
writeRF_PDU(data, bitLength);
delete[] data;
}
-
-///
-/// Helper to write a network P25 PDU header packet.
-///
-void DataPacket::writeNet_PDU_Header()
-{
- if (m_p25->m_netState != RS_NET_DATA) {
- m_netDataHeader.reset();
- m_netSecondHeader.reset();
- m_netBlocksToFollow = 0U;
- m_netDataBlockCnt = 0U;
- m_netBitOffset = 0U;
- m_netDataOffset = 0U;
-
- m_p25->m_netState = RS_NET_DATA;
-
- ::memset(m_pduUserData, 0x00U, P25_MAX_PDU_COUNT * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
-
- uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
- ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
- ::memcpy(buffer, m_netPDU, P25_PDU_FEC_LENGTH_BYTES);
-
- bool ret = m_netDataHeader.decode(buffer);
- if (!ret) {
- LogWarning(LOG_NET, P25_PDU_STR ", unfixable RF 1/2 rate header data");
- Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
-
- m_netDataHeader.reset();
- m_netSecondHeader.reset();
- m_netBlocksToFollow = 0U;
- m_netDataBlockCnt = 0U;
- m_netBitOffset = 0U;
- m_netDataOffset = 0U;
- m_p25->m_netState = RS_NET_IDLE;
- return;
- }
-
- if (m_verbose) {
- LogMessage(LOG_NET, P25_PDU_STR ", ack = %u, outbound = %u, fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padCount = %u, n = %u, seqNo = %u, hdrOffset = %u",
- m_netDataHeader.getAckNeeded(), m_netDataHeader.getOutbound(), m_netDataHeader.getFormat(), m_netDataHeader.getSAP(), m_netDataHeader.getFullMessage(),
- m_netDataHeader.getBlocksToFollow(), m_netDataHeader.getPadCount(), m_netDataHeader.getNs(), m_netDataHeader.getFSN(),
- m_netDataHeader.getHeaderOffset());
- }
-
- // make sure we don't get a PDU with more blocks then we support
- if (m_netDataHeader.getBlocksToFollow() >= P25_MAX_PDU_COUNT) {
- 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_netBlocksToFollow = 0U;
- m_netDataBlockCnt = 0U;
- m_netBitOffset = 0U;
- m_netDataOffset = 0U;
- return;
- }
-
- if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR &&
- m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) {
- LogWarning(LOG_NET, P25_PDU_STR ", unsupported confirmed enhanced addressing");
-
- m_netDataHeader.reset();
- m_netSecondHeader.reset();
- m_netBlocksToFollow = 0U;
- m_netDataBlockCnt = 0U;
- m_netBitOffset = 0U;
- m_netDataOffset = 0U;
- m_p25->m_netState = RS_NET_IDLE;
- return;
- }
-
- m_netBlocksToFollow = m_netDataHeader.getBlocksToFollow();
-
- //uint32_t bitLength = ((m_netDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
- m_netBitOffset = P25_PREAMBLE_LENGTH_BITS;
-
- ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
-
- // Generate the PDU header and 1/2 rate Trellis
- m_netDataHeader.encode(buffer);
- Utils::setBitRange(buffer, m_netPDU, m_netBitOffset, P25_PDU_FEC_LENGTH_BITS);
- m_netBitOffset += P25_PDU_FEC_LENGTH_BITS;
- }
-}
-
-///
-/// Helper to write a network P25 PDU secondary header packet.
-///
-void DataPacket::writeNet_PDU_Sec_Header()
-{
- if (m_p25->m_netState == RS_NET_DATA) {
- // process second header if we're using enhanced addressing
- if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR) {
- uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
- ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
- ::memcpy(buffer, m_netPDU, P25_PDU_FEC_LENGTH_BYTES);
-
- bool ret = m_netSecondHeader.decode(buffer);
- if (!ret) {
- LogWarning(LOG_NET, P25_PDU_STR ", unfixable RF 1/2 rate second header data");
- Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_HEADER_LENGTH_BYTES);
-
- m_netDataHeader.reset();
- m_netSecondHeader.reset();
- m_netBlocksToFollow = 0U;
- m_netDataBlockCnt = 0U;
- m_netBitOffset = 0U;
- m_netDataOffset = 0U;
- m_p25->m_netState = RS_NET_IDLE;
- return;
- }
-
- if (m_verbose) {
- LogMessage(LOG_NET, P25_PDU_STR ", fmt = $%02X, sap = $%02X, llId = %u",
- m_netSecondHeader.getFormat(), m_netSecondHeader.getSAP(), m_netSecondHeader.getLLId());
- }
-
- ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
-
- // Generate the PDU header and 1/2 rate Trellis
- m_netDataHeader.encode(buffer);
- Utils::setBitRange(buffer, m_netPDU, m_netBitOffset, P25_PDU_FEC_LENGTH_BITS);
- m_netBitOffset += P25_PDU_FEC_LENGTH_BITS;
-
- m_netBlocksToFollow--;
- }
- }
-}
-
-///
-/// Helper to write a network P25 PDU data packet.
-///
-void DataPacket::writeNet_PDU()
-{
- if (m_p25->m_netState == RS_NET_DATA) {
- uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
- ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
- ::memcpy(buffer, m_netPDU, P25_PDU_FEC_LENGTH_BYTES);
-
- bool ret = m_netData[m_netDataBlockCnt].decode(buffer, (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR) ? m_netSecondHeader : m_netDataHeader);
- if (ret) {
- if (m_verbose) {
- LogMessage(LOG_NET, P25_PDU_STR ", block %u, fmt = $%02X",
- (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? m_netData[m_netDataBlockCnt].getSerialNo() : m_netDataBlockCnt, m_netData[m_netDataBlockCnt].getFormat());
- }
-
- m_netData[m_netDataBlockCnt].getData(m_pduUserData + m_netDataOffset);
- m_netDataBlockCnt++;
- }
- else {
- if (m_netData[m_netDataBlockCnt].getFormat() == PDU_FMT_CONFIRMED)
- LogWarning(LOG_NET, P25_PDU_STR ", unfixable PDU data (3/4 rate or CRC)");
- else
- LogWarning(LOG_NET, P25_PDU_STR ", unfixable PDU data (1/2 rate or CRC)");
-
- if (m_dumpPDUData) {
- Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
- }
- }
-
- m_netBitOffset += P25_PDU_FEC_LENGTH_BITS;
- m_netDataOffset += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
- }
-}
diff --git a/p25/DataPacket.h b/p25/DataPacket.h
index 0f350d24..8a77c34e 100644
--- a/p25/DataPacket.h
+++ b/p25/DataPacket.h
@@ -88,11 +88,11 @@ namespace p25
data::DataBlock* m_netData;
data::DataHeader m_netDataHeader;
data::DataHeader m_netSecondHeader;
- uint8_t m_netBlocksToFollow;
- uint8_t m_netDataBlockCnt;
- uint32_t m_netBitOffset;
+ bool m_netUseSecondHeader;
uint32_t m_netDataOffset;
+ uint8_t m_netDataBlockCnt;
uint8_t* m_netPDU;
+ uint32_t m_netPDUCount;
uint8_t* m_pduUserData;
uint32_t m_pduUserDataLength;
@@ -111,23 +111,18 @@ namespace p25
~DataPacket();
/// Write data processed from RF to the network.
- void writeNetworkRF(const uint8_t dataType, const uint8_t* data, uint32_t len);
+ void writeNetworkRF(const uint8_t currentBlock, const uint8_t* data, uint32_t len);
/// Helper to write a P25 PDU packet.
void writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls = false);
+ /// Helper to write a network P25 PDU packet.
+ void writeNet_PDU_Buffered();
/// Helper to re-write a received P25 PDU packet.
void writeRF_PDU_Buffered();
/// Helper to write a PDU registration response.
void writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong64_t ipAddr);
/// Helper to write a PDU acknowledge response.
void writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t llId);
-
- /// Helper to write a network P25 PDU header packet.
- void writeNet_PDU_Header();
- /// Helper to write a network P25 PDU secondary header packet.
- void writeNet_PDU_Sec_Header();
- /// Helper to write a network P25 PDU data packet.
- void writeNet_PDU();
};
} // namespace p25
diff --git a/p25/P25Defines.h b/p25/P25Defines.h
index d2e403a5..91a69244 100644
--- a/p25/P25Defines.h
+++ b/p25/P25Defines.h
@@ -340,11 +340,6 @@ namespace p25
const uint8_t P25_DUID_LDU2 = 0x0AU; // Logical Link Data Unit 2
const uint8_t P25_DUID_PDU = 0x0CU; // Packet Data Unit
const uint8_t P25_DUID_TDULC = 0x0FU; // Terminator Data Unit with Link Control
-
- // Data Type(s)
- const uint8_t P25_DT_DATA_HEADER = 0x06U;
- const uint8_t P25_DT_DATA_SEC_HEADER = 0x07U;
- const uint8_t P25_DT_DATA = 0x08U;
} // namespace p25
// ---------------------------------------------------------------------------