diff --git a/src/network/BaseNetwork.cpp b/src/network/BaseNetwork.cpp
index eb7c4581..f530310a 100644
--- a/src/network/BaseNetwork.cpp
+++ b/src/network/BaseNetwork.cpp
@@ -495,13 +495,12 @@ bool BaseNetwork::writeP25TSDU(const p25::lc::LC& control, const uint8_t* data)
/// Writes P25 PDU frame data to the network.
///
///
-///
///
///
///
///
-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)
+bool BaseNetwork::writeP25PDU(const p25::data::DataHeader& header, 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;
@@ -513,7 +512,7 @@ bool BaseNetwork::writeP25PDU(const p25::data::DataHeader& header, const p25::da
}
uint32_t messageLength = 0U;
- UInt8Array message = createP25_PDUMessage(messageLength, header, secHeader, currentBlock, data, len);
+ UInt8Array message = createP25_PDUMessage(messageLength, header, currentBlock, data, len);
if (message == nullptr) {
return false;
}
@@ -972,33 +971,24 @@ UInt8Array BaseNetwork::createP25_TSDUMessage(uint32_t& length, const p25::lc::L
///
///
///
-///
///
///
///
///
-UInt8Array BaseNetwork::createP25_PDUMessage(uint32_t& length, const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader,
+UInt8Array BaseNetwork::createP25_PDUMessage(uint32_t& length, const p25::data::DataHeader& header,
const uint8_t currentBlock, const uint8_t* data, const uint32_t len)
{
- 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 != nullptr);
uint8_t* buffer = new uint8_t[DATA_PACKET_LENGTH];
::memset(buffer, 0x00U, DATA_PACKET_LENGTH);
- // construct P25 message header
/*
** PDU packs different bytes into the P25 message header space from the rest of the
** P25 DUIDs
*/
+ // construct P25 message header
::memcpy(buffer + 0U, TAG_P25_DATA, 4U);
buffer[4U] = header.getSAP(); // Service Access Point
@@ -1006,9 +996,6 @@ UInt8Array BaseNetwork::createP25_PDUMessage(uint32_t& length, const p25::data::
buffer[4U] |= 0x80U;
}
- uint32_t llId = (useSecondHeader) ? secHeader.getLLId() : header.getLLId();
- __SET_UINT16(llId, buffer, 5U); // Logical Link Address
-
__SET_UINT16(len, buffer, 8U); // PDU Length [bytes]
buffer[15U] = header.getMFId(); // MFId
diff --git a/src/network/BaseNetwork.h b/src/network/BaseNetwork.h
index a8def202..c895520e 100644
--- a/src/network/BaseNetwork.h
+++ b/src/network/BaseNetwork.h
@@ -221,8 +221,8 @@ namespace network
/// Writes P25 TSDU frame data to the network.
bool writeP25TSDU(const p25::lc::LC& control, const uint8_t* data);
/// Writes P25 PDU frame data to the network.
- bool writeP25PDU(const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader,
- const uint8_t currentBlock, const uint8_t* data, const uint32_t len);
+ bool writeP25PDU(const p25::data::DataHeader& header, const uint8_t currentBlock, const uint8_t* data,
+ const uint32_t len);
/// Helper to test if the P25 ring buffer has data.
bool hasP25Data() const;
@@ -297,8 +297,8 @@ namespace network
UInt8Array createP25_TSDUMessage(uint32_t& length, const p25::lc::LC& control, const uint8_t* data);
/// Creates an P25 PDU frame message.
- UInt8Array createP25_PDUMessage(uint32_t& length, const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader,
- const uint8_t currentBlock, const uint8_t* data, const uint32_t len);
+ UInt8Array createP25_PDUMessage(uint32_t& length, const p25::data::DataHeader& header, const uint8_t currentBlock,
+ const uint8_t* data, const uint32_t len);
/// Creates an NXDN frame message.
UInt8Array createNXDN_Message(uint32_t& length, const nxdn::lc::RTCH& lc, const uint8_t* data, const uint32_t len);
diff --git a/src/p25/Control.cpp b/src/p25/Control.cpp
index db8a22d4..28a2b7b4 100644
--- a/src/p25/Control.cpp
+++ b/src/p25/Control.cpp
@@ -1009,17 +1009,58 @@ void Control::processNetwork()
}
// process network message header
+ uint8_t duid = buffer[22U];
+ uint8_t MFId = buffer[15U];
+
+ // process raw P25 data bytes
+ UInt8Array data;
+ uint8_t frameLength = buffer[23U];
+ if (duid == p25::P25_DUID_PDU) {
+ frameLength = length;
+ data = std::unique_ptr(new uint8_t[length]);
+ ::memset(data.get(), 0x00U, length);
+ ::memcpy(data.get(), buffer.get(), length);
+ }
+ else {
+ if (frameLength <= 24) {
+ data = std::unique_ptr(new uint8_t[frameLength]);
+ ::memset(data.get(), 0x00U, frameLength);
+ }
+ else {
+ data = std::unique_ptr(new uint8_t[frameLength]);
+ ::memset(data.get(), 0x00U, frameLength);
+ ::memcpy(data.get(), buffer.get() + 24U, frameLength);
+ }
+ }
+
+ // is this a PDU?
+ if (duid == P25_DUID_PDU) {
+ uint32_t blockLength = __GET_UINT16(buffer, 8U);
+
+ if (m_debug) {
+ LogDebug(LOG_NET, "P25, duid = $%02X, MFId = $%02X, blockLength = %u, len = %u", duid, MFId, blockLength, length);
+ }
+
+ if (!m_dedicatedControl)
+ ret = m_data->processNetwork(data.get(), frameLength, blockLength);
+ else {
+ if (m_voiceOnControl) {
+ ret = m_data->processNetwork(data.get(), frameLength, blockLength);
+ }
+ }
+
+ return;
+ }
+
+ // handle LDU, TDU or TSDU frame
uint8_t lco = buffer[4U];
uint32_t srcId = __GET_UINT16(buffer, 5U);
uint32_t dstId = __GET_UINT16(buffer, 8U);
- uint8_t MFId = buffer[15U];
-
uint8_t lsd1 = buffer[20U];
uint8_t lsd2 = buffer[21U];
- uint8_t duid = buffer[22U];
uint8_t frameType = p25::P25_FT_DATA_UNIT;
if (m_debug) {
@@ -1068,27 +1109,6 @@ void Control::processNetwork()
lsd.setLSD1(lsd1);
lsd.setLSD2(lsd2);
- // process raw P25 data bytes
- UInt8Array data;
- uint8_t frameLength = buffer[23U];
- if (duid == p25::P25_DUID_PDU) {
- frameLength = length;
- data = std::unique_ptr(new uint8_t[length]);
- ::memset(data.get(), 0x00U, length);
- ::memcpy(data.get(), buffer.get(), length);
- }
- else {
- if (frameLength <= 24) {
- data = std::unique_ptr(new uint8_t[frameLength]);
- ::memset(data.get(), 0x00U, frameLength);
- }
- else {
- data = std::unique_ptr(new uint8_t[frameLength]);
- ::memset(data.get(), 0x00U, frameLength);
- ::memcpy(data.get(), buffer.get() + 24U, frameLength);
- }
- }
-
m_networkWatchdog.start();
if (m_debug) {
@@ -1108,16 +1128,6 @@ void Control::processNetwork()
m_voice->processNetwork(data.get(), frameLength, control, lsd, duid, frameType);
break;
- case P25_DUID_PDU:
- if (!m_dedicatedControl)
- ret = m_data->processNetwork(data.get(), frameLength, control, lsd, duid);
- else {
- if (m_voiceOnControl) {
- ret = m_voice->processNetwork(data.get(), frameLength, control, lsd, duid, frameType);
- }
- }
- break;
-
case P25_DUID_TSDU:
m_trunk->processNetwork(data.get(), frameLength, control, lsd, duid);
break;
diff --git a/src/p25/packet/Data.cpp b/src/p25/packet/Data.cpp
index 988f7397..f4878ffb 100644
--- a/src/p25/packet/Data.cpp
+++ b/src/p25/packet/Data.cpp
@@ -142,10 +142,10 @@ bool Data::process(uint8_t* data, uint32_t len)
}
if (m_verbose) {
- LogMessage(LOG_RF, P25_PDU_STR ", ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padCount = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u",
+ LogMessage(LOG_RF, P25_PDU_STR ", ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padCount = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u",
m_rfDataHeader.getAckNeeded(), m_rfDataHeader.getOutbound(), m_rfDataHeader.getFormat(), m_rfDataHeader.getMFId(), m_rfDataHeader.getSAP(), m_rfDataHeader.getFullMessage(),
m_rfDataHeader.getBlocksToFollow(), m_rfDataHeader.getPadCount(), m_rfDataHeader.getNs(), m_rfDataHeader.getFSN(), m_rfDataHeader.getLastFragment(),
- m_rfDataHeader.getHeaderOffset());
+ m_rfDataHeader.getHeaderOffset(), m_rfDataHeader.getLLId());
}
// make sure we don't get a PDU with more blocks then we support
@@ -159,6 +159,14 @@ bool Data::process(uint8_t* data, uint32_t len)
m_p25->m_rfState = m_prevRfState;
return false;
}
+
+ // only send data blocks across the network, if we're not an AMBT,
+ // an RSP or a registration service
+ if ((m_rfDataHeader.getFormat() != PDU_FMT_AMBT) &&
+ (m_rfDataHeader.getFormat() != PDU_FMT_RSP) &&
+ (m_rfDataHeader.getSAP() != PDU_SAP_REG)) {
+ writeNetwork(0, buffer, P25_PDU_FEC_LENGTH_BYTES);
+ }
}
if (m_p25->m_rfState == RS_RF_DATA) {
@@ -192,6 +200,14 @@ bool Data::process(uint8_t* data, uint32_t len)
m_rfUseSecondHeader = true;
+ // only send data blocks across the network, if we're not an AMBT,
+ // an RSP or a registration service
+ if ((m_rfDataHeader.getFormat() != PDU_FMT_AMBT) &&
+ (m_rfDataHeader.getFormat() != PDU_FMT_RSP) &&
+ (m_rfDataHeader.getSAP() != PDU_SAP_REG)) {
+ writeNetwork(1, buffer, P25_PDU_FEC_LENGTH_BYTES);
+ }
+
offset += P25_PDU_FEC_LENGTH_BITS;
m_rfPDUCount++;
blocksToFollow--;
@@ -208,46 +224,54 @@ bool Data::process(uint8_t* data, uint32_t len)
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 (m_verbose) {
- 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, sap = $%02X, llId = %u",
- m_rfData[i].getSerialNo(), m_rfData[i].getFormat(), m_rfData[i].getSAP(), m_rfData[i].getLLId());
- m_rfSecondHeader.reset();
- m_rfSecondHeader.setFormat(m_rfData[i].getFormat());
- m_rfSecondHeader.setLLId(m_rfData[i].getLLId());
- m_rfSecondHeader.setSAP(m_rfData[i].getSAP());
- }
- else {
- LogMessage(LOG_RF, P25_PDU_STR ", block %u, fmt = $%02X, lastBlock = %u",
- (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? m_rfData[i].getSerialNo() : m_rfDataBlockCnt, m_rfData[i].getFormat(),
- m_rfData[i].getLastBlock());
+ if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR && m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED &&
+ m_rfData[i].getSerialNo() == 0U) {
+ // bryanb: HACK - workaround for some vendors setting lastBlock to true when it shouldn't be
+ if (blocksToFollow > 1 && m_rfData[i].getLastBlock()) {
+ m_rfData[i].setLastBlock(false);
}
+
+ LogMessage(LOG_RF, P25_PDU_STR ", block %u, fmt = $%02X, lastBlock = %u, sap = $%02X, llId = %u",
+ m_rfData[i].getSerialNo(), m_rfData[i].getFormat(), m_rfData[i].getLastBlock(), m_rfData[i].getSAP(), m_rfData[i].getLLId());
+ m_rfSecondHeader.reset();
+ m_rfSecondHeader.setAckNeeded(true);
+ m_rfSecondHeader.setFormat(m_rfData[i].getFormat());
+ m_rfSecondHeader.setLLId(m_rfData[i].getLLId());
+ m_rfSecondHeader.setSAP(m_rfData[i].getSAP());
+ m_rfExtendedAddress = true;
+ }
+ else {
+ LogMessage(LOG_RF, P25_PDU_STR ", block %u, fmt = $%02X, lastBlock = %u",
+ (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? m_rfData[i].getSerialNo() : m_rfDataBlockCnt, m_rfData[i].getFormat(),
+ m_rfData[i].getLastBlock());
}
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;
+ // only send data blocks across the network, if we're not an AMBT,
+ // an RSP or a registration service
+ if ((m_rfDataHeader.getFormat() != PDU_FMT_AMBT) &&
+ (m_rfDataHeader.getFormat() != PDU_FMT_RSP) &&
+ (m_rfDataHeader.getSAP() != PDU_SAP_REG)) {
+ writeNetwork(m_rfDataBlockCnt, buffer, P25_PDU_FEC_LENGTH_BYTES);
+ }
+
+ m_rfDataBlockCnt++;
+
// is this the last block?
- if (m_rfData[i].getLastBlock()) {
+ if (m_rfData[i].getLastBlock() && m_rfDataBlockCnt == blocksToFollow) {
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);
}
}
-
- // only repeat data blocks if we're not an AMBT
- if (m_rfDataHeader.getFormat() != PDU_FMT_AMBT) {
- writeNetwork(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)
- LogWarning(LOG_RF, P25_PDU_STR ", unfixable PDU data (3/4 rate or CRC)");
+ LogWarning(LOG_RF, P25_PDU_STR ", unfixable PDU data (3/4 rate or CRC), block %u", i);
else
- LogWarning(LOG_RF, P25_PDU_STR ", unfixable PDU data (1/2 rate or CRC)");
+ LogWarning(LOG_RF, P25_PDU_STR ", unfixable PDU data (1/2 rate or CRC), block %u", i);
if (m_dumpPDUData) {
Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
@@ -341,11 +365,14 @@ bool Data::process(uint8_t* data, uint32_t len)
}
break;
default:
- ::ActivityLog("P25", true, "RF data transmission from %u to %u, %u blocks", m_rfDataHeader.getLLId(), m_rfDataHeader.getLLId(), m_rfDataHeader.getBlocksToFollow());
+ 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) {
if (m_verbose) {
- LogMessage(LOG_RF, P25_PDU_STR ", repeating PDU, llId = %u", (m_rfUseSecondHeader) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId());
+ LogMessage(LOG_RF, P25_PDU_STR ", repeating PDU, llId = %u", (m_rfUseSecondHeader || m_rfExtendedAddress) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId());
}
writeRF_PDU_Buffered(); // re-generate buffered PDU and send it on
@@ -381,106 +408,193 @@ bool Data::process(uint8_t* data, uint32_t len)
///
/// Buffer containing data frame.
/// Length of data frame.
-///
-///
-///
+///
///
-bool Data::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::LowSpeedData& lsd, uint8_t& duid)
+bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
{
if (m_p25->m_rfState != RS_RF_LISTENING && m_p25->m_netState == RS_NET_IDLE)
return false;
- switch (duid) {
- case P25_DUID_PDU:
- {
- if (m_p25->m_netState != RS_NET_DATA) {
+ 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 buffer[P25_PDU_FEC_LENGTH_BYTES];
+ ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
+ ::memcpy(buffer, data + 24U, 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_netDataBlockCnt = 0U;
+ m_netPDUCount = 0U;
+ m_p25->m_netState = RS_NET_IDLE;
+ return false;
+ }
+
+ 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, llId = %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(), m_netDataHeader.getLLId());
+ }
+
+ // 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_netDataOffset = 0U;
+ m_netDataBlockCnt = 0U;
+ m_netPDUCount = 0U;
+ m_p25->m_netState = RS_NET_IDLE;
+ return false;
+ }
+
+ m_netPDUCount++;
+ return true;
+ }
+
+ if (m_p25->m_netState == RS_NET_DATA) {
+ // Utils::dump(1U, "Incoming Network PDU Frame", data + 24U, blockLength);
+
+ ::memcpy(m_netPDU + m_netDataOffset, data + 24U, blockLength);
+ m_netDataOffset += blockLength;
+ m_netPDUCount++;
+ m_netDataBlockCnt++;
+
+ if (m_netDataBlockCnt >= m_netDataHeader.getBlocksToFollow()) {
+ uint32_t blocksToFollow = m_netDataHeader.getBlocksToFollow();
+ uint32_t offset = 0U;
+
+ uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
+
+ // process second header if we're using enhanced addressing
+ if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR &&
+ m_netDataHeader.getFormat() == PDU_FMT_UNCONFIRMED) {
+ ::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_netDataOffset = 0U;
+ m_netUseSecondHeader = false;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
+ m_p25->m_netState = RS_NET_IDLE;
+ return false;
+ }
- 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;
+ if (m_verbose) {
+ LogMessage(LOG_NET, P25_PDU_STR ", fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padCount = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u",
+ m_netSecondHeader.getFormat(), m_netSecondHeader.getMFId(), m_netSecondHeader.getSAP(), m_netSecondHeader.getFullMessage(),
+ m_netSecondHeader.getBlocksToFollow(), m_netSecondHeader.getPadCount(), m_netSecondHeader.getNs(), m_netSecondHeader.getFSN(), m_netSecondHeader.getLastFragment(),
+ m_netSecondHeader.getHeaderOffset(), m_netSecondHeader.getLLId());
+ }
- 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);
+ m_netUseSecondHeader = true;
- 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());
- }
+ offset += P25_PDU_FEC_LENGTH_BYTES;
+ blocksToFollow--;
+ }
- // 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_netDataBlockCnt = 0U;
- m_netDataHeader.reset();
- m_netDataOffset = 0U;
- m_netDataBlockCnt = 0U;
- m_netPDUCount = 0U;
- m_p25->m_netState = RS_NET_IDLE;
- return false;
+ // process all blocks in the data stream
+ uint32_t dataOffset = 0U;
+ 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 (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",
+ m_netData[i].getSerialNo(), m_netData[i].getFormat(), m_netData[i].getLastBlock(), m_netData[i].getSAP(), m_netData[i].getLLId());
+ m_netSecondHeader.reset();
+ m_netSecondHeader.setAckNeeded(true);
+ m_netSecondHeader.setFormat(m_netData[i].getFormat());
+ m_netSecondHeader.setLLId(m_netData[i].getLLId());
+ m_netSecondHeader.setSAP(m_netData[i].getSAP());
+ m_netExtendedAddress = true;
+ }
+ else {
+ LogMessage(LOG_NET, P25_PDU_STR ", block %u, fmt = $%02X, lastBlock = %u",
+ (m_netSecondHeader.getFormat() == PDU_FMT_CONFIRMED) ? m_netData[i].getSerialNo() : m_netDataBlockCnt, m_netData[i].getFormat(),
+ m_netData[i].getLastBlock());
}
- 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_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;
- m_netDataHeader.reset();
- m_netSecondHeader.reset();
- m_netDataOffset = 0U;
- m_netDataBlockCnt = 0U;
- m_netPDUCount = 0U;
- m_p25->m_netState = RS_NET_IDLE;
- return false;
+ m_netDataBlockCnt++;
+
+ // is this the last block?
+ if (m_netData[i].getLastBlock() && m_netDataBlockCnt == blocksToFollow) {
+ bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength);
+ if (!crcRet) {
+ LogWarning(LOG_NET, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", blocksToFollow, m_pduUserDataLength);
+ }
}
+ }
+ else {
+ if (m_netData[i].getFormat() == PDU_FMT_CONFIRMED)
+ LogWarning(LOG_NET, P25_PDU_STR ", unfixable PDU data (3/4 rate or CRC), block %u", i);
+ else
+ LogWarning(LOG_NET, P25_PDU_STR ", unfixable PDU data (1/2 rate or CRC), block %u", i);
- ::ActivityLog("P25", false, "network data transmission from %u to %u, %u blocks", m_netDataHeader.getLLId(), m_netDataHeader.getLLId(), m_netDataHeader.getBlocksToFollow());
+ if (m_dumpPDUData) {
+ Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
+ }
}
- 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);
+ 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_netDataBlockCnt >= m_netDataHeader.getBlocksToFollow()) {
- if (m_dumpPDUData) {
- Utils::dump(1U, "PDU Packet", m_pduUserData, m_netDataOffset);
- }
+ if (m_dumpPDUData && m_netDataBlockCnt > 0U) {
+ Utils::dump(1U, "PDU Packet", m_pduUserData, dataOffset);
+ }
- writeNet_PDU_Buffered();
+ if (m_netDataBlockCnt < blocksToFollow) {
+ LogWarning(LOG_NET, P25_PDU_STR ", incomplete PDU (%d / %d blocks)", m_netDataBlockCnt, blocksToFollow);
+ }
- ::ActivityLog("P25", true, "end of RF data transmission");
+ uint32_t srcId = (m_netUseSecondHeader || m_netExtendedAddress) ? m_netSecondHeader.getLLId() : m_netDataHeader.getLLId();
+ uint32_t dstId = m_netDataHeader.getLLId();
- m_netDataHeader.reset();
- m_netSecondHeader.reset();
- m_netDataOffset = 0U;
- m_netDataBlockCnt = 0U;
- m_netPDUCount = 0U;
- m_p25->m_netState = RS_NET_IDLE;
- }
- else {
- uint32_t len = __GET_UINT16(data, 8U);
- ::memcpy(m_pduUserData, data + 24U, len);
- m_netDataOffset += len;
- m_netDataBlockCnt++;
- }
+ ::ActivityLog("P25", false, "Net data transmission from %u to %u, %u blocks", srcId, dstId, m_netDataHeader.getBlocksToFollow());
+
+ if (m_repeatPDU) {
+ if (m_verbose) {
+ LogMessage(LOG_NET, P25_PDU_STR ", repeating PDU, llId = %u", (m_netUseSecondHeader || m_netExtendedAddress) ? m_netSecondHeader.getLLId() : m_netDataHeader.getLLId());
}
+
+ writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on
}
- break;
- default:
- return false;
+
+ ::ActivityLog("P25", false, "end of Net data transmission");
+
+ m_netDataHeader.reset();
+ m_netSecondHeader.reset();
+ m_netDataOffset = 0U;
+ m_netDataBlockCnt = 0U;
+ m_netPDUCount = 0U;
+ m_p25->m_netState = RS_NET_IDLE;
+ }
}
return true;
@@ -617,6 +731,7 @@ Data::Data(Control* p25, network::BaseNetwork* network, bool dumpPDUData, bool r
m_rfDataHeader(),
m_rfSecondHeader(),
m_rfUseSecondHeader(false),
+ m_rfExtendedAddress(false),
m_rfDataBlockCnt(0U),
m_rfPDU(nullptr),
m_rfPDUCount(0U),
@@ -625,6 +740,7 @@ Data::Data(Control* p25, network::BaseNetwork* network, bool dumpPDUData, bool r
m_netDataHeader(),
m_netSecondHeader(),
m_netUseSecondHeader(false),
+ m_netExtendedAddress(false),
m_netDataOffset(0U),
m_netDataBlockCnt(0U),
m_netPDU(nullptr),
@@ -685,7 +801,9 @@ void Data::writeNetwork(const uint8_t currentBlock, const uint8_t *data, uint32_
if (m_p25->m_rfTimeout.isRunning() && m_p25->m_rfTimeout.hasExpired())
return;
- m_network->writeP25PDU(m_rfDataHeader, m_rfSecondHeader, currentBlock, data, len);
+ // Utils::dump(1U, "Outgoing Network PDU Frame", data, len);
+
+ m_network->writeP25PDU(m_rfDataHeader, currentBlock, data, len);
}
///
diff --git a/src/p25/packet/Data.h b/src/p25/packet/Data.h
index 38a9a625..905588ad 100644
--- a/src/p25/packet/Data.h
+++ b/src/p25/packet/Data.h
@@ -68,7 +68,7 @@ namespace p25
/// Process a data frame from the RF interface.
bool process(uint8_t* data, uint32_t len);
/// Process a data frame from the network.
- bool processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::LowSpeedData& lsd, uint8_t& duid);
+ bool processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength);
/// Helper to check if a logical link ID has registered with data services.
bool hasLLIdFNEReg(uint32_t llId) const;
@@ -91,6 +91,7 @@ namespace p25
data::DataHeader m_rfDataHeader;
data::DataHeader m_rfSecondHeader;
bool m_rfUseSecondHeader;
+ bool m_rfExtendedAddress;
uint8_t m_rfDataBlockCnt;
uint8_t* m_rfPDU;
uint32_t m_rfPDUCount;
@@ -100,6 +101,7 @@ namespace p25
data::DataHeader m_netDataHeader;
data::DataHeader m_netSecondHeader;
bool m_netUseSecondHeader;
+ bool m_netExtendedAddress;
uint32_t m_netDataOffset;
uint8_t m_netDataBlockCnt;
uint8_t* m_netPDU;