Digital_Voice_Modem_Project
/
dvmhost
mirror of https://github.com/DVMProject/dvmhost.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

refactor P25 PDU network handling code; better handle PDU confirmed extended addressing;

Browse Source
pull/35/head
Bryan Biedenkapp 3 years ago
parent 41f590fb0f
commit 4daac87c1a
5 changed files with 276 additions and 159 deletions
Show Stats Download Patch File Download Diff File

23
src/network/BaseNetwork.cpp
Unescape View File

@ -495,13 +495,12 @@ bool BaseNetwork::writeP25TSDU(const p25::lc::LC& control, const uint8_t* data)
/// Writes P25 PDU frame data to the network. /// Writes P25 PDU frame data to the network.
/// </summary> /// </summary>
/// <param name="header"></param> /// <param name="header"></param>
/// <param name="secHeader"></param>
/// <param name="currentBlock"></param> /// <param name="currentBlock"></param>
/// <param name="data"></param> /// <param name="data"></param>
/// <param name="len"></param> /// <param name="len"></param>
/// <returns></returns> /// <returns></returns>
bool BaseNetwork::writeP25PDU(const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader, const uint8_t currentBlock, bool BaseNetwork::writeP25PDU(const p25::data::DataHeader& header, const uint8_t currentBlock, const uint8_t* data,
const uint8_t* data, const uint32_t len) const uint32_t len)
{ {
if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING) if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING)
return false; return false;
@ -513,7 +512,7 @@ bool BaseNetwork::writeP25PDU(const p25::data::DataHeader& header, const p25::da
} }
uint32_t messageLength = 0U; 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) { if (message == nullptr) {
return false; return false;
} }
@ -972,33 +971,24 @@ UInt8Array BaseNetwork::createP25_TSDUMessage(uint32_t& length, const p25::lc::L
/// </summary> /// </summary>
/// <param name="length"></param> /// <param name="length"></param>
/// <param name="header"></param> /// <param name="header"></param>
/// <param name="secHeader"></param>
/// <param name="currentBlock"></param> /// <param name="currentBlock"></param>
/// <param name="data"></param> /// <param name="data"></param>
/// <param name="len"></param> /// <param name="len"></param>
/// <returns></returns> /// <returns></returns>
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) 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); assert(data != nullptr);
uint8_t* buffer = new uint8_t[DATA_PACKET_LENGTH]; uint8_t* buffer = new uint8_t[DATA_PACKET_LENGTH];
::memset(buffer, 0x00U, 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 ** PDU packs different bytes into the P25 message header space from the rest of the
** P25 DUIDs ** P25 DUIDs
*/ */
// construct P25 message header
::memcpy(buffer + 0U, TAG_P25_DATA, 4U); ::memcpy(buffer + 0U, TAG_P25_DATA, 4U);
buffer[4U] = header.getSAP(); // Service Access Point buffer[4U] = header.getSAP(); // Service Access Point
@ -1006,9 +996,6 @@ UInt8Array BaseNetwork::createP25_PDUMessage(uint32_t& length, const p25::data::
buffer[4U] |= 0x80U; 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] __SET_UINT16(len, buffer, 8U); // PDU Length [bytes]
buffer[15U] = header.getMFId(); // MFId buffer[15U] = header.getMFId(); // MFId

8
src/network/BaseNetwork.h
Unescape View File

@ -221,8 +221,8 @@ namespace network
/// <summary>Writes P25 TSDU frame data to the network.</summary> /// <summary>Writes P25 TSDU frame data to the network.</summary>
bool writeP25TSDU(const p25::lc::LC& control, const uint8_t* data); bool writeP25TSDU(const p25::lc::LC& control, const uint8_t* data);
/// <summary>Writes P25 PDU frame data to the network.</summary> /// <summary>Writes P25 PDU frame data to the network.</summary>
bool writeP25PDU(const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader, bool writeP25PDU(const p25::data::DataHeader& header, const uint8_t currentBlock, const uint8_t* data,
const uint8_t currentBlock, const uint8_t* data, const uint32_t len); const uint32_t len);
/// <summary>Helper to test if the P25 ring buffer has data.</summary> /// <summary>Helper to test if the P25 ring buffer has data.</summary>
bool hasP25Data() const; bool hasP25Data() const;
@ -297,8 +297,8 @@ namespace network
UInt8Array createP25_TSDUMessage(uint32_t& length, const p25::lc::LC& control, const uint8_t* data); UInt8Array createP25_TSDUMessage(uint32_t& length, const p25::lc::LC& control, const uint8_t* data);
/// <summary>Creates an P25 PDU frame message.</summary> /// <summary>Creates an P25 PDU frame message.</summary>
UInt8Array createP25_PDUMessage(uint32_t& length, const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader, UInt8Array createP25_PDUMessage(uint32_t& length, const p25::data::DataHeader& header, const uint8_t currentBlock,
const uint8_t currentBlock, const uint8_t* data, const uint32_t len); const uint8_t* data, const uint32_t len);
/// <summary>Creates an NXDN frame message.</summary> /// <summary>Creates an NXDN frame message.</summary>
UInt8Array createNXDN_Message(uint32_t& length, const nxdn::lc::RTCH& lc, const uint8_t* data, const uint32_t len); UInt8Array createNXDN_Message(uint32_t& length, const nxdn::lc::RTCH& lc, const uint8_t* data, const uint32_t len);

78
src/p25/Control.cpp
Unescape View File

@ -1009,17 +1009,58 @@ void Control::processNetwork()
} }
// process network message header // 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<uint8_t[]>(new uint8_t[length]);
::memset(data.get(), 0x00U, length);
::memcpy(data.get(), buffer.get(), length);
}
else {
if (frameLength <= 24) {
data = std::unique_ptr<uint8_t[]>(new uint8_t[frameLength]);
::memset(data.get(), 0x00U, frameLength);
}
else {
data = std::unique_ptr<uint8_t[]>(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]; uint8_t lco = buffer[4U];
uint32_t srcId = __GET_UINT16(buffer, 5U); uint32_t srcId = __GET_UINT16(buffer, 5U);
uint32_t dstId = __GET_UINT16(buffer, 8U); uint32_t dstId = __GET_UINT16(buffer, 8U);
uint8_t MFId = buffer[15U];
uint8_t lsd1 = buffer[20U]; uint8_t lsd1 = buffer[20U];
uint8_t lsd2 = buffer[21U]; uint8_t lsd2 = buffer[21U];
uint8_t duid = buffer[22U];
uint8_t frameType = p25::P25_FT_DATA_UNIT; uint8_t frameType = p25::P25_FT_DATA_UNIT;
if (m_debug) { if (m_debug) {
@ -1068,27 +1109,6 @@ void Control::processNetwork()
lsd.setLSD1(lsd1); lsd.setLSD1(lsd1);
lsd.setLSD2(lsd2); 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<uint8_t[]>(new uint8_t[length]);
::memset(data.get(), 0x00U, length);
::memcpy(data.get(), buffer.get(), length);
}
else {
if (frameLength <= 24) {
data = std::unique_ptr<uint8_t[]>(new uint8_t[frameLength]);
::memset(data.get(), 0x00U, frameLength);
}
else {
data = std::unique_ptr<uint8_t[]>(new uint8_t[frameLength]);
::memset(data.get(), 0x00U, frameLength);
::memcpy(data.get(), buffer.get() + 24U, frameLength);
}
}
m_networkWatchdog.start(); m_networkWatchdog.start();
if (m_debug) { if (m_debug) {
@ -1108,16 +1128,6 @@ void Control::processNetwork()
m_voice->processNetwork(data.get(), frameLength, control, lsd, duid, frameType); m_voice->processNetwork(data.get(), frameLength, control, lsd, duid, frameType);
break; 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: case P25_DUID_TSDU:
m_trunk->processNetwork(data.get(), frameLength, control, lsd, duid); m_trunk->processNetwork(data.get(), frameLength, control, lsd, duid);
break; break;

322
src/p25/packet/Data.cpp
Unescape View File

@ -142,10 +142,10 @@ bool Data::process(uint8_t* data, uint32_t len)
} }
if (m_verbose) { 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.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.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 // 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; m_p25->m_rfState = m_prevRfState;
return false; 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) { if (m_p25->m_rfState == RS_RF_DATA) {
@ -192,6 +200,14 @@ bool Data::process(uint8_t* data, uint32_t len)
m_rfUseSecondHeader = true; 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; offset += P25_PDU_FEC_LENGTH_BITS;
m_rfPDUCount++; m_rfPDUCount++;
blocksToFollow--; 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); Utils::getBitRange(m_rfPDU, buffer, offset, P25_PDU_FEC_LENGTH_BITS);
bool ret = m_rfData[i].decode(buffer, (m_rfUseSecondHeader) ? m_rfSecondHeader : m_rfDataHeader); bool ret = m_rfData[i].decode(buffer, (m_rfUseSecondHeader) ? m_rfSecondHeader : m_rfDataHeader);
if (ret) { if (ret) {
if (m_verbose) { if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR && m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED &&
if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR && m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED && m_rfData[i].getSerialNo() == 0U) {
m_rfData[i].getSerialNo() == 0U) { // bryanb: HACK - workaround for some vendors setting lastBlock to true when it shouldn't be
LogMessage(LOG_RF, P25_PDU_STR ", block %u, fmt = $%02X, sap = $%02X, llId = %u", if (blocksToFollow > 1 && m_rfData[i].getLastBlock()) {
m_rfData[i].getSerialNo(), m_rfData[i].getFormat(), m_rfData[i].getSAP(), m_rfData[i].getLLId()); m_rfData[i].setLastBlock(false);
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());
} }
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_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_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? // 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); bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength);
if (!crcRet) { if (!crcRet) {
LogWarning(LOG_RF, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", blocksToFollow, m_pduUserDataLength); 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 { else {
if (m_rfData[i].getFormat() == PDU_FMT_CONFIRMED) 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 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) { if (m_dumpPDUData) {
Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES); 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; break;
default: 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_repeatPDU) {
if (m_verbose) { 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 writeRF_PDU_Buffered(); // re-generate buffered PDU and send it on
@ -381,106 +408,193 @@ bool Data::process(uint8_t* data, uint32_t len)
/// </summary> /// </summary>
/// <param name="data">Buffer containing data frame.</param> /// <param name="data">Buffer containing data frame.</param>
/// <param name="len">Length of data frame.</param> /// <param name="len">Length of data frame.</param>
/// <param name="control"></param> /// <param name="blockLength"></param>
/// <param name="lsd"></param>
/// <param name="duid"></param>
/// <returns></returns> /// <returns></returns>
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) if (m_p25->m_rfState != RS_RF_LISTENING && m_p25->m_netState == RS_NET_IDLE)
return false; return false;
switch (duid) { if (m_p25->m_netState != RS_NET_DATA) {
case P25_DUID_PDU: m_netDataHeader.reset();
{ m_netSecondHeader.reset();
if (m_p25->m_netState != RS_NET_DATA) { 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_netDataHeader.reset();
m_netSecondHeader.reset(); m_netSecondHeader.reset();
m_netDataOffset = 0U; m_netUseSecondHeader = false;
m_netDataBlockCnt = 0U; m_netDataBlockCnt = 0U;
m_netPDUCount = 0U; m_netPDUCount = 0U;
m_p25->m_netState = RS_NET_IDLE;
return false;
}
m_p25->m_netState = RS_NET_DATA; 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",
uint8_t blocksToFollow = data[20U]; m_netSecondHeader.getFormat(), m_netSecondHeader.getMFId(), m_netSecondHeader.getSAP(), m_netSecondHeader.getFullMessage(),
bool confirmed = (data[4U] & 0x80U) == 0x80U; m_netSecondHeader.getBlocksToFollow(), m_netSecondHeader.getPadCount(), m_netSecondHeader.getNs(), m_netSecondHeader.getFSN(), m_netSecondHeader.getLastFragment(),
//bool response = (data[4U] & 0x40U) == 0x40U; m_netSecondHeader.getHeaderOffset(), m_netSecondHeader.getLLId());
uint8_t sap = data[4U] & 0x3FU; }
m_netDataHeader.setAckNeeded(confirmed); m_netUseSecondHeader = true;
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_verbose) { offset += P25_PDU_FEC_LENGTH_BYTES;
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", blocksToFollow--;
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 m_netDataBlockCnt = 0U;
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(); // process all blocks in the data stream
m_netDataOffset = 0U; uint32_t dataOffset = 0U;
m_netDataBlockCnt = 0U; for (uint32_t i = 0U; i < blocksToFollow; i++) {
m_netPDUCount = 0U; ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_p25->m_netState = RS_NET_IDLE; ::memcpy(buffer, m_netPDU + offset, P25_PDU_FEC_LENGTH_BYTES);
return false;
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_netData[i].getData(m_pduUserData + dataOffset);
m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) { m_pduUserDataLength += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
LogWarning(LOG_NET, P25_PDU_STR ", unsupported confirmed enhanced addressing");
m_netDataHeader.reset(); m_netDataBlockCnt++;
m_netSecondHeader.reset();
m_netDataOffset = 0U; // is this the last block?
m_netDataBlockCnt = 0U; if (m_netData[i].getLastBlock() && m_netDataBlockCnt == blocksToFollow) {
m_netPDUCount = 0U; bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength);
m_p25->m_netState = RS_NET_IDLE; if (!crcRet) {
return false; 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) { offset += P25_PDU_FEC_LENGTH_BYTES;
uint32_t pduLen = control.getDstId(); // PDU's use dstId as the PDU len dataOffset += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
::memset(m_netPDU, 0x00U, pduLen + 2U); }
::memcpy(m_netPDU, data, pduLen);
if (m_netDataBlockCnt >= m_netDataHeader.getBlocksToFollow()) { if (m_dumpPDUData && m_netDataBlockCnt > 0U) {
if (m_dumpPDUData) { Utils::dump(1U, "PDU Packet", m_pduUserData, dataOffset);
Utils::dump(1U, "PDU Packet", m_pduUserData, m_netDataOffset); }
}
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(); ::ActivityLog("P25", false, "Net data transmission from %u to %u, %u blocks", srcId, dstId, m_netDataHeader.getBlocksToFollow());
m_netSecondHeader.reset();
m_netDataOffset = 0U; if (m_repeatPDU) {
m_netDataBlockCnt = 0U; if (m_verbose) {
m_netPDUCount = 0U; LogMessage(LOG_NET, P25_PDU_STR ", repeating PDU, llId = %u", (m_netUseSecondHeader || m_netExtendedAddress) ? m_netSecondHeader.getLLId() : m_netDataHeader.getLLId());
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++;
}
} }
writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on
} }
break;
default: ::ActivityLog("P25", false, "end of Net data transmission");
return false;
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netDataOffset = 0U;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
m_p25->m_netState = RS_NET_IDLE;
}
} }
return true; return true;
@ -617,6 +731,7 @@ Data::Data(Control* p25, network::BaseNetwork* network, bool dumpPDUData, bool r
m_rfDataHeader(), m_rfDataHeader(),
m_rfSecondHeader(), m_rfSecondHeader(),
m_rfUseSecondHeader(false), m_rfUseSecondHeader(false),
m_rfExtendedAddress(false),
m_rfDataBlockCnt(0U), m_rfDataBlockCnt(0U),
m_rfPDU(nullptr), m_rfPDU(nullptr),
m_rfPDUCount(0U), m_rfPDUCount(0U),
@ -625,6 +740,7 @@ Data::Data(Control* p25, network::BaseNetwork* network, bool dumpPDUData, bool r
m_netDataHeader(), m_netDataHeader(),
m_netSecondHeader(), m_netSecondHeader(),
m_netUseSecondHeader(false), m_netUseSecondHeader(false),
m_netExtendedAddress(false),
m_netDataOffset(0U), m_netDataOffset(0U),
m_netDataBlockCnt(0U), m_netDataBlockCnt(0U),
m_netPDU(nullptr), 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()) if (m_p25->m_rfTimeout.isRunning() && m_p25->m_rfTimeout.hasExpired())
return; 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);
} }
/// <summary> /// <summary>

4
src/p25/packet/Data.h
Unescape View File

@ -68,7 +68,7 @@ namespace p25
/// <summary>Process a data frame from the RF interface.</summary> /// <summary>Process a data frame from the RF interface.</summary>
bool process(uint8_t* data, uint32_t len); bool process(uint8_t* data, uint32_t len);
/// <summary>Process a data frame from the network.</summary> /// <summary>Process a data frame from the network.</summary>
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);
/// <summary>Helper to check if a logical link ID has registered with data services.</summary> /// <summary>Helper to check if a logical link ID has registered with data services.</summary>
bool hasLLIdFNEReg(uint32_t llId) const; bool hasLLIdFNEReg(uint32_t llId) const;
@ -91,6 +91,7 @@ namespace p25
data::DataHeader m_rfDataHeader; data::DataHeader m_rfDataHeader;
data::DataHeader m_rfSecondHeader; data::DataHeader m_rfSecondHeader;
bool m_rfUseSecondHeader; bool m_rfUseSecondHeader;
bool m_rfExtendedAddress;
uint8_t m_rfDataBlockCnt; uint8_t m_rfDataBlockCnt;
uint8_t* m_rfPDU; uint8_t* m_rfPDU;
uint32_t m_rfPDUCount; uint32_t m_rfPDUCount;
@ -100,6 +101,7 @@ namespace p25
data::DataHeader m_netDataHeader; data::DataHeader m_netDataHeader;
data::DataHeader m_netSecondHeader; data::DataHeader m_netSecondHeader;
bool m_netUseSecondHeader; bool m_netUseSecondHeader;
bool m_netExtendedAddress;
uint32_t m_netDataOffset; uint32_t m_netDataOffset;
uint8_t m_netDataBlockCnt; uint8_t m_netDataBlockCnt;
uint8_t* m_netPDU; uint8_t* m_netPDU;

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.