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.
/// </summary>
/// <param name="header"></param>
/// <param name="secHeader"></param>
/// <param name="currentBlock"></param>
/// <param name="data"></param>
/// <param name="len"></param>
/// <returns></returns>
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
/// </summary>
/// <param name="length"></param>
/// <param name="header"></param>
/// <param name="secHeader"></param>
/// <param name="currentBlock"></param>
/// <param name="data"></param>
/// <param name="len"></param>
/// <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)
{
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

8
src/network/BaseNetwork.h
Unescape View File

@ -221,8 +221,8 @@ namespace network
/// <summary>Writes P25 TSDU frame data to the network.</summary>
bool writeP25TSDU(const p25::lc::LC& control, const uint8_t* data);
/// <summary>Writes P25 PDU frame data to the network.</summary>
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);
/// <summary>Helper to test if the P25 ring buffer has data.</summary>
bool hasP25Data() const;
@ -297,8 +297,8 @@ namespace network
UInt8Array createP25_TSDUMessage(uint32_t& length, const p25::lc::LC& control, const uint8_t* data);
/// <summary>Creates an P25 PDU frame message.</summary>
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);
/// <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);

78
src/p25/Control.cpp
Unescape View File

@ -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<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];
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<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();
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;

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

@ -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());
// 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,18 +408,13 @@ bool Data::process(uint8_t* data, uint32_t len)
/// </summary>
/// <param name="data">Buffer containing data frame.</param>
/// <param name="len">Length of data frame.</param>
/// <param name="control"></param>
/// <param name="lsd"></param>
/// <param name="duid"></param>
/// <param name="blockLength"></param>
/// <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)
return false;
switch (duid) {
case P25_DUID_PDU:
{
if (m_p25->m_netState != RS_NET_DATA) {
m_netDataHeader.reset();
m_netSecondHeader.reset();
@ -402,23 +424,27 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::Lo
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;
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);
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);
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",
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.getHeaderOffset(), m_netDataHeader.getLLId());
}
// make sure we don't get a PDU with more blocks then we support
@ -433,35 +459,134 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::Lo
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_CONFIRMED) {
LogWarning(LOG_NET, P25_PDU_STR ", unsupported confirmed enhanced addressing");
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;
}
::ActivityLog("P25", false, "network data transmission from %u to %u, %u blocks", m_netDataHeader.getLLId(), m_netDataHeader.getLLId(), m_netDataHeader.getBlocksToFollow());
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());
}
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);
m_netUseSecondHeader = true;
offset += P25_PDU_FEC_LENGTH_BYTES;
blocksToFollow--;
}
m_netDataBlockCnt = 0U;
// 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());
}
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_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);
if (m_netDataBlockCnt >= m_netDataHeader.getBlocksToFollow()) {
if (m_dumpPDUData) {
Utils::dump(1U, "PDU Packet", m_pduUserData, m_netDataOffset);
Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
}
}
writeNet_PDU_Buffered();
offset += P25_PDU_FEC_LENGTH_BYTES;
dataOffset += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}
::ActivityLog("P25", true, "end of RF data transmission");
if (m_dumpPDUData && m_netDataBlockCnt > 0U) {
Utils::dump(1U, "PDU Packet", m_pduUserData, dataOffset);
}
if (m_netDataBlockCnt < blocksToFollow) {
LogWarning(LOG_NET, P25_PDU_STR ", incomplete PDU (%d / %d blocks)", m_netDataBlockCnt, blocksToFollow);
}
uint32_t srcId = (m_netUseSecondHeader || m_netExtendedAddress) ? m_netSecondHeader.getLLId() : m_netDataHeader.getLLId();
uint32_t dstId = m_netDataHeader.getLLId();
::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
}
::ActivityLog("P25", false, "end of Net data transmission");
m_netDataHeader.reset();
m_netSecondHeader.reset();
@ -470,17 +595,6 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::Lo
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++;
}
}
}
break;
default:
return false;
}
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);
}
/// <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>
bool process(uint8_t* data, uint32_t len);
/// <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>
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;

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.