diff --git a/src/p25/P25Defines.h b/src/p25/P25Defines.h
index 3560c5ea..20c0b089 100644
--- a/src/p25/P25Defines.h
+++ b/src/p25/P25Defines.h
@@ -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
diff --git a/src/p25/data/DataBlock.cpp b/src/p25/data/DataBlock.cpp
index bd39ff9a..7f20d41c 100644
--- a/src/p25/data/DataBlock.cpp
+++ b/src/p25/data/DataBlock.cpp
@@ -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);
diff --git a/src/p25/packet/Data.cpp b/src/p25/packet/Data.cpp
index 9a5bba27..d2be1c4a 100644
--- a/src/p25/packet/Data.cpp
+++ b/src/p25/packet/Data.cpp
@@ -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
///
///
///
+///
///
-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);
}
diff --git a/src/p25/packet/Data.h b/src/p25/packet/Data.h
index 905588ad..baaed00a 100644
--- a/src/p25/packet/Data.h
+++ b/src/p25/packet/Data.h
@@ -138,7 +138,7 @@ namespace p25
/// Helper to write a PDU registration response.
void writeRF_PDU_Reg_Response(uint8_t regType, uint8_t mfId, 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, 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