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 extended address handling (this loosely gets a lot of conventional data things working, Moto TMS for example will pass in conventional [although buggy]); pass PDU response data headers (with no block followers) across the network;

Browse Source
pull/61/head
Bryan Biedenkapp 2 years ago
parent 197ce833ac
commit f3c3cc95ce
9 changed files with 496 additions and 353 deletions
Show Stats Download Patch File Download Diff File

33
src/common/p25/data/DataBlock.cpp
Unescape View File

@ -4,7 +4,7 @@
* GPLv2 Open Source. Use is subject to license terms.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* Copyright (C) 2018,2022,2024 Bryan Biedenkapp, N2PLL
* Copyright (C) 2018-2024 Bryan Biedenkapp, N2PLL
*
*/
#include "Defines.h"
@ -30,8 +30,6 @@ using namespace p25::data;
DataBlock::DataBlock() :
m_serialNo(0U),
m_lastBlock(false),
m_llId(0U),
m_sap(0U),
m_trellis(),
m_fmt(PDUFormatType::CONFIRMED),
m_headerSap(0U),
@ -63,7 +61,6 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader& header)
// set these to reasonable defaults
m_serialNo = 0U;
m_lastBlock = false;
m_llId = 0U;
if (m_fmt == PDUFormatType::CONFIRMED) {
// decode 3/4 rate Trellis
@ -82,17 +79,7 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader& header)
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
if (m_headerSap == PDUSAP::EXT_ADDR && m_serialNo == 0U) {
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 + 2U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); // Payload Data
}
else {
::memcpy(m_data, buffer + 2U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); // Payload Data
}
::memcpy(m_data, buffer + 2U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); // Payload Data
uint8_t crcBuffer[18U];
::memset(crcBuffer, 0x00U, 18U);
@ -115,7 +102,7 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader& header)
#if DEBUG_P25_PDU_DATA
LogDebug(LOG_P25, "PDU, fmt = $%02X, crc = $%04X, calculated = $%04X", m_fmt, crc, calculated);
Utils::dump(1U, "P25, DataBlock::decode(), Confirmed PDU Block Data", m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
Utils::dump(1U, "P25, DataBlock::decode(), Confirmed PDU Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES);
#endif
}
catch (...) {
@ -165,19 +152,7 @@ void DataBlock::encode(uint8_t* data)
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 == PDUSAP::EXT_ADDR && m_serialNo == 0U) {
buffer[5U] = m_sap & 0x3FU; // Service Access Point
buffer[2U] = (m_llId >> 16) & 0xFFU; // Logical Link ID
buffer[3U] = (m_llId >> 8) & 0xFFU;
buffer[4U] = (m_llId >> 0) & 0xFFU;
::memcpy(buffer + 6U, m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - 4U);
}
else {
::memcpy(buffer + 2U, m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
::memcpy(buffer + 2U, m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); // Payload Data
uint8_t crcBuffer[18U];
::memset(crcBuffer, 0x00U, 18U);

11
src/common/p25/data/DataBlock.h
Unescape View File

@ -4,7 +4,7 @@
* GPLv2 Open Source. Use is subject to license terms.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* Copyright (C) 2018,2022 Bryan Biedenkapp, N2PLL
* Copyright (C) 2018-2024 Bryan Biedenkapp, N2PLL
*
*/
/**
@ -97,15 +97,6 @@ namespace p25
*/
__PROPERTY(bool, lastBlock, LastBlock);
/**
* @brief Logical link ID.
*/
__PROPERTY(uint32_t, llId, LLId);
/**
* @brief Service access point.
*/
__PROPERTY(uint8_t, sap, SAP);
private:
edac::Trellis m_trellis;

133
src/common/p25/data/DataHeader.cpp
Unescape View File

@ -61,10 +61,13 @@ DataHeader::DataHeader() :
m_ambtField8(0U),
m_ambtField9(0U),
m_trellis(),
m_data(nullptr)
m_data(nullptr),
m_extAddrData(nullptr)
{
m_data = new uint8_t[P25_PDU_HEADER_LENGTH_BYTES];
::memset(m_data, 0x00U, P25_PDU_HEADER_LENGTH_BYTES);
m_extAddrData = new uint8_t[P25_PDU_HEADER_LENGTH_BYTES];
::memset(m_extAddrData, 0x00U, P25_PDU_HEADER_LENGTH_BYTES);
}
/* Finalizes a instance of the DataHeader class. */
@ -72,6 +75,7 @@ DataHeader::DataHeader() :
DataHeader::~DataHeader()
{
delete[] m_data;
delete[] m_extAddrData;
}
/* Decodes P25 PDU data header. */
@ -254,6 +258,122 @@ void DataHeader::encode(uint8_t* data, bool noTrellis)
}
}
/* Decodes P25 PDU extended addressing header. */
bool DataHeader::decodeExtAddr(const uint8_t* data, bool noTrellis)
{
assert(data != nullptr);
::memset(m_extAddrData, 0x00U, P25_PDU_HEADER_LENGTH_BYTES);
if (m_sap != PDUSAP::EXT_ADDR)
return false;
if (m_fmt == PDUFormatType::CONFIRMED) {
::memcpy(m_extAddrData, data, 4U);
#if DEBUG_P25_PDU_DATA
Utils::dump(1U, "P25, DataHeader::decodeExtAddr(), PDU Extended Address Data", m_extAddrData, 4U);
#endif
m_exSap = m_extAddrData[3U] & 0x3FU; // Service Access Point
m_srcLlId = (m_extAddrData[0U] << 16) + (m_extAddrData[1U] << 8) + // Source Logical Link ID
m_extAddrData[2U];
} else if (m_fmt == PDUFormatType::UNCONFIRMED) {
// decode 1/2 rate Trellis & check CRC-CCITT 16
bool valid = true;
if (noTrellis) {
::memcpy(m_extAddrData, data, P25_PDU_HEADER_LENGTH_BYTES);
}
else {
valid = m_trellis.decode12(data, m_extAddrData);
}
if (valid) {
valid = edac::CRC::checkCCITT162(m_extAddrData, P25_PDU_HEADER_LENGTH_BYTES);
if (!valid) {
if (m_warnCRC) {
// if we're already warning instead of erroring CRC, don't announce invalid CRC in the
// case where no CRC is defined
if ((m_extAddrData[P25_PDU_HEADER_LENGTH_BYTES - 2U] != 0x00U) && (m_extAddrData[P25_PDU_HEADER_LENGTH_BYTES - 1U] != 0x00U)) {
LogWarning(LOG_P25, "DataHeader::decodeExtAddr(), failed CRC CCITT-162 check");
}
valid = true; // ignore CRC error
}
else {
LogError(LOG_P25, "DataHeader::decodeExtAddr(), failed CRC CCITT-162 check");
}
}
}
if (!valid) {
return false;
}
#if DEBUG_P25_PDU_DATA
Utils::dump(1U, "P25, DataHeader::decodeExtAddr(), PDU Extended Address Data", m_extAddrData, P25_PDU_HEADER_LENGTH_BYTES);
#endif
m_exSap = m_extAddrData[1U] & 0x3FU; // Service Access Point
m_srcLlId = (m_extAddrData[3U] << 16) + (m_extAddrData[4U] << 8) + // Source Logical Link ID
m_extAddrData[5U];
}
return true;
}
/* Encodes P25 PDU extended addressing header. */
void DataHeader::encodeExtAddr(uint8_t* data, bool noTrellis)
{
assert(data != nullptr);
if (m_sap != PDUSAP::EXT_ADDR)
return;
uint8_t header[P25_PDU_HEADER_LENGTH_BYTES];
::memset(header, 0x00U, P25_PDU_HEADER_LENGTH_BYTES);
if (m_fmt == PDUFormatType::CONFIRMED) {
header[3U] = m_exSap & 0x3FU; // Service Access Point
header[3U] |= 0xC0U;
header[0U] = (m_srcLlId >> 16) & 0xFFU; // Source Logical Link ID
header[1U] = (m_srcLlId >> 8) & 0xFFU;
header[2U] = (m_srcLlId >> 0) & 0xFFU;
#if DEBUG_P25_PDU_DATA
Utils::dump(1U, "P25, DataHeader::encodeExtAddr(), PDU Header Data", header, P25_PDU_HEADER_LENGTH_BYTES);
#endif
::memcpy(data, header, 4U); // only copy the 4 bytes of header data for confirmed
} else if (m_fmt == PDUFormatType::UNCONFIRMED) {
header[0U] = m_fmt & 0x1FU; // Packet Format
header[1U] = m_exSap & 0x3FU; // Service Access Point
header[1U] |= 0xC0U;
header[2U] = m_mfId; // Mfg Id.
header[3U] = (m_srcLlId >> 16) & 0xFFU; // Source Logical Link ID
header[4U] = (m_srcLlId >> 8) & 0xFFU;
header[5U] = (m_srcLlId >> 0) & 0xFFU;
// compute CRC-CCITT 16
edac::CRC::addCCITT162(header, P25_PDU_HEADER_LENGTH_BYTES);
#if DEBUG_P25_PDU_DATA
Utils::dump(1U, "P25, DataHeader::encodeExtAddr(), PDU Header Data", header, P25_PDU_HEADER_LENGTH_BYTES);
#endif
if (!noTrellis) {
// encode 1/2 rate Trellis
m_trellis.encode12(header, data);
} else {
::memcpy(data, header, P25_PDU_HEADER_LENGTH_BYTES);
}
}
}
/* Helper to reset data values to defaults. */
void DataHeader::reset()
@ -318,6 +438,17 @@ uint32_t DataHeader::getData(uint8_t* buffer) const
return P25_PDU_HEADER_LENGTH_BYTES;
}
/* Gets the raw extended address header data. */
uint32_t DataHeader::getExtAddrData(uint8_t* buffer) const
{
assert(buffer != nullptr);
assert(m_extAddrData != nullptr);
::memcpy(buffer, m_extAddrData, P25_PDU_HEADER_LENGTH_BYTES);
return P25_PDU_HEADER_LENGTH_BYTES;
}
/* Helper to calculate the number of blocks to follow and padding length for a PDU. */
void DataHeader::calculateLength(uint32_t packetLength)

29
src/common/p25/data/DataHeader.h
Unescape View File

@ -62,6 +62,20 @@ namespace p25
*/
void encode(uint8_t* data, bool noTrellis = false);
/**
* @brief Decodes P25 PDU extended addressing header.
* @param[in] data Buffer containing a PDU data header to decode.
* @param noTrellis Flag indicating not to perform Trellis encoding.
* @returns bool True, if PDU data header decoded, otherwise false.
*/
bool decodeExtAddr(const uint8_t* data, bool noTrellis = false);
/**
* @brief Encodes P25 PDU extended addressing header.
* @param[out] data Buffer to encode a PDU data header.
* @param noTrellis Flag indicating not to perform Trellis encoding.
*/
void encodeExtAddr(uint8_t* data, bool noTrellis = false);
/**
* @brief Helper to reset data values to defaults.
*/
@ -79,6 +93,12 @@ namespace p25
* @returns uint32_t Length of data copied.
*/
uint32_t getData(uint8_t* buffer) const;
/**
* @brief Gets the raw extended address header data.
* @param[out] buffer Buffer to copy raw header data to.
* @returns uint32_t Length of data copied.
*/
uint32_t getExtAddrData(uint8_t* buffer) const;
/**
* @brief Helper to calculate the number of blocks to follow and padding length for a PDU.
@ -158,11 +178,17 @@ namespace p25
*/
__PROPERTY(uint8_t, headerOffset, HeaderOffset);
// Response Data
// Extended Addressing Data
/**
* @brief Service access point.
*/
__PROPERTY(uint8_t, exSap, EXSAP);
/**
* @brief Source Logical link ID.
*/
__PROPERTY(uint32_t, srcLlId, SrcLLId);
// Response Data
/**
* @brief Response class.
*/
@ -194,6 +220,7 @@ namespace p25
edac::Trellis m_trellis;
uint8_t* m_data;
uint8_t* m_extAddrData;
static bool m_warnCRC;
};

163
src/fne/network/callhandler/packetdata/P25PacketData.cpp
Unescape View File

@ -112,6 +112,18 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
RxStatus* status = m_status[peerId];
// a PDU header only with no blocks to follow is usually a response header
if (status->header.getBlocksToFollow() == 0U) {
dispatch(peerId);
LogMessage(LOG_NET, "P25, Data Call End, peer = %u, srcId = %u, dstId = %u, streamId = %u, external = %u",
peerId, status->header.getSrcLLId(), status->header.getLLId(), streamId, external);
delete status;
m_status.erase(peerId);
return true;
}
::memcpy(status->netPDU + status->dataOffset, data + 24U, blockLength);
status->dataOffset += blockLength;
status->netPDUCount++;
@ -120,6 +132,7 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
if (status->dataBlockCnt >= status->header.getBlocksToFollow()) {
uint32_t blocksToFollow = status->header.getBlocksToFollow();
uint32_t offset = 0U;
uint32_t dataOffset = 0U;
uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
@ -129,7 +142,7 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
::memcpy(buffer, status->netPDU, P25_PDU_FEC_LENGTH_BYTES);
bool ret = status->secondHeader.decode(buffer);
bool ret = status->header.decodeExtAddr(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);
@ -140,38 +153,32 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
return false;
}
LogMessage(LOG_NET, P25_PDU_STR ", peerId = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u",
peerId, status->secondHeader.getFormat(), status->secondHeader.getMFId(), status->secondHeader.getSAP(), status->secondHeader.getFullMessage(),
status->secondHeader.getBlocksToFollow(), status->secondHeader.getPadLength(), status->secondHeader.getNs(), status->secondHeader.getFSN(), status->secondHeader.getLastFragment(),
status->secondHeader.getHeaderOffset(), status->secondHeader.getLLId());
LogMessage(LOG_NET, P25_PDU_STR ", ISP, sap = $%02X, srcLlId = %u",
status->header.getEXSAP(), status->header.getSrcLLId());
status->useSecondHeader = true;
status->extendedAddress = true;
offset += P25_PDU_FEC_LENGTH_BYTES;
blocksToFollow--;
// if we are using a secondary header place it in the PDU user data buffer
status->header.getExtAddrData(status->pduUserData + dataOffset);
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
status->pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES;
}
status->dataBlockCnt = 0U;
// process all blocks in the data stream
uint32_t dataOffset = 0U;
status->pduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U];
::memset(status->pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
// if we are using a secondary header place it in the PDU user data buffer
if (status->useSecondHeader) {
status->secondHeader.getData(status->pduUserData + dataOffset);
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
status->pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES;
}
// decode data blocks
for (uint32_t i = 0U; i < blocksToFollow; i++) {
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
::memcpy(buffer, status->netPDU + offset, P25_PDU_FEC_LENGTH_BYTES);
bool ret = status->blockData[i].decode(buffer, (status->useSecondHeader) ? status->secondHeader : status->header);
bool ret = status->blockData[i].decode(buffer, status->header);
if (ret) {
// if we are getting unconfirmed or confirmed blocks, and if we've reached the total number of blocks
// set this block as the last block for full packet CRC
@ -184,8 +191,8 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
// are we processing extended address data from the first block?
if (status->header.getSAP() == PDUSAP::EXT_ADDR && status->header.getFormat() == PDUFormatType::CONFIRMED &&
status->blockData[i].getSerialNo() == 0U) {
LogMessage(LOG_NET, P25_PDU_STR ", peerId = %u, block %u, fmt = $%02X, lastBlock = %u, sap = $%02X, llId = %u",
peerId, status->blockData[i].getSerialNo(), status->blockData[i].getFormat(), status->blockData[i].getLastBlock(), status->blockData[i].getSAP(), status->blockData[i].getLLId());
LogMessage(LOG_NET, P25_PDU_STR ", ISP, block %u, fmt = $%02X, lastBlock = %u",
status->blockData[i].getSerialNo(), status->blockData[i].getFormat(), status->blockData[i].getLastBlock());
if (m_network->m_dumpDataPacket) {
uint8_t dataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(dataBlock, 0xAAU, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
@ -193,11 +200,15 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
Utils::dump(2U, "Data Block", dataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
status->secondHeader.reset();
status->secondHeader.setAckNeeded(true);
status->secondHeader.setFormat(status->blockData[i].getFormat());
status->secondHeader.setLLId(status->blockData[i].getLLId());
status->secondHeader.setSAP(status->blockData[i].getSAP());
uint8_t secondHeader[P25_PDU_HEADER_LENGTH_BYTES];
::memset(secondHeader, 0x00U, P25_PDU_HEADER_LENGTH_BYTES);
status->blockData[i].getData(secondHeader);
status->header.decodeExtAddr(secondHeader);
LogMessage(LOG_NET, P25_PDU_STR ", ISP, sap = $%02X, srcLlId = %u",
status->header.getEXSAP(), status->header.getSrcLLId());
status->extendedAddress = true;
}
else {
@ -251,8 +262,8 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
}
uint64_t duration = hrc::diff(pktTime, status->callStartTime);
uint32_t srcId = status->header.getLLId();
uint32_t dstId = (status->useSecondHeader || status->extendedAddress) ? status->secondHeader.getLLId() : status->header.getLLId();
uint32_t srcId = (status->extendedAddress) ? status->header.getSrcLLId() : status->header.getLLId();
uint32_t dstId = status->header.getLLId();
LogMessage(LOG_NET, "P25, Data Call End, peer = %u, srcId = %u, dstId = %u, blocks = %u, duration = %u, streamId = %u, external = %u",
peerId, srcId, dstId, status->header.getBlocksToFollow(), duration / 1000, streamId, external);
@ -291,8 +302,8 @@ void P25PacketData::dispatch(uint32_t peerId)
Utils::dump(1U, "PDU Packet", status->pduUserData, status->pduUserDataLength);
}
uint32_t srcId = status->header.getLLId();
uint32_t dstId = (status->useSecondHeader || status->extendedAddress) ? status->secondHeader.getLLId() : status->header.getLLId();
uint32_t srcId = (status->extendedAddress) ? status->header.getSrcLLId() : status->header.getLLId();
uint32_t dstId = status->header.getLLId();
// repeat traffic to the connected peers
if (m_network->m_peers.size() > 0U) {
@ -304,7 +315,7 @@ void P25PacketData::dispatch(uint32_t peerId)
m_network->m_frameQueue->flushQueue();
}
write_PDU_User(peer.first, nullptr, status->header, status->secondHeader, status->useSecondHeader, status->pduUserData, true);
write_PDU_User(peer.first, nullptr, status->header, status->extendedAddress, status->pduUserData, true);
if (m_network->m_debug) {
LogDebug(LOG_NET, "P25, srcPeer = %u, dstPeer = %u, duid = $%02X, srcId = %u, dstId = %u",
peerId, peer.first, DUID::PDU, srcId, dstId);
@ -334,7 +345,7 @@ void P25PacketData::dispatch(uint32_t peerId)
continue;
}
write_PDU_User(dstPeerId, peer.second, status->header, status->secondHeader, status->useSecondHeader, status->pduUserData);
write_PDU_User(dstPeerId, peer.second, status->header, status->extendedAddress, status->pduUserData);
if (m_network->m_debug) {
LogDebug(LOG_NET, "P25, srcPeer = %u, dstPeer = %u, duid = $%02X, srcId = %u, dstId = %u",
peerId, dstPeerId, DUID::PDU, srcId, dstId);
@ -346,8 +357,8 @@ void P25PacketData::dispatch(uint32_t peerId)
/* Helper to write user data as a P25 PDU packet. */
void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNet, data::DataHeader& dataHeader, data::DataHeader& secondHeader,
bool useSecondHeader, uint8_t* pduUserData, bool queueOnly)
void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNet, data::DataHeader& dataHeader,
bool extendedAddress, uint8_t* pduUserData, bool queueOnly)
{
assert(pduUserData != nullptr);
@ -369,55 +380,51 @@ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNe
writeNetwork(peerId, peerNet, dataHeader, 0U, buffer, P25_PDU_FEC_LENGTH_BYTES, pktSeq, streamId, queueOnly);
++pktSeq;
uint32_t dataOffset = 0U;
// generate the second PDU header
if (useSecondHeader) {
secondHeader.encode(pduUserData, true);
uint32_t packetLength = (dataHeader.getPacketLength() + dataHeader.getPadLength() + 4U);
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
secondHeader.encode(buffer);
writeNetwork(peerId, peerNet, dataHeader, 1U, buffer, P25_PDU_FEC_LENGTH_BYTES, pktSeq, streamId, queueOnly);
++pktSeq;
if (blocksToFollow > 0U) {
uint32_t dataOffset = 0U;
uint32_t networkBlock = 1U;
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
blocksToFollow--;
// generate the second PDU header
if ((dataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (dataHeader.getSAP() == PDUSAP::EXT_ADDR) && extendedAddress) {
dataHeader.encodeExtAddr(pduUserData, true);
LogMessage(LOG_NET, P25_PDU_STR ", OSP, peerId = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u",
peerId, secondHeader.getFormat(), secondHeader.getMFId(), secondHeader.getSAP(), secondHeader.getFullMessage(),
secondHeader.getBlocksToFollow(), secondHeader.getPadLength(), secondHeader.getNs(), secondHeader.getFSN(), secondHeader.getLastFragment(),
secondHeader.getHeaderOffset(), secondHeader.getLLId());
}
if (dataHeader.getFormat() != PDUFormatType::AMBT) {
edac::CRC::addCRC32(pduUserData, (dataHeader.getPacketLength() + dataHeader.getPadLength() + 4U));
}
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
dataHeader.encodeExtAddr(buffer);
writeNetwork(peerId, peerNet, dataHeader, 1U, buffer, P25_PDU_FEC_LENGTH_BYTES, pktSeq, streamId, queueOnly);
++pktSeq;
uint32_t networkBlock = 1U;
if (useSecondHeader)
++networkBlock;
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
packetLength += P25_PDU_HEADER_LENGTH_BYTES;
blocksToFollow--;
++networkBlock;
// generate the PDU data
for (uint32_t i = 0U; i < blocksToFollow; i++) {
data::DataBlock dataBlock = data::DataBlock();
dataBlock.setFormat((useSecondHeader) ? secondHeader : dataHeader);
dataBlock.setSerialNo(i);
dataBlock.setData(pduUserData + dataOffset);
LogMessage(LOG_RF, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
dataHeader.getEXSAP(), dataHeader.getSrcLLId());
}
// are we processing extended address data from the first block?
if (dataHeader.getSAP() == PDUSAP::EXT_ADDR && dataHeader.getFormat() == PDUFormatType::CONFIRMED &&
dataBlock.getSerialNo() == 0U) {
LogMessage(LOG_NET, P25_PDU_STR ", OSP, peerId = %u, block %u, fmt = $%02X, lastBlock = %u, sap = $%02X, llId = %u",
peerId, dataBlock.getSerialNo(), dataBlock.getFormat(), dataBlock.getLastBlock(), dataBlock.getSAP(), dataBlock.getLLId());
if ((dataHeader.getFormat() == PDUFormatType::CONFIRMED) && (dataHeader.getSAP() == PDUSAP::EXT_ADDR) && extendedAddress) {
dataHeader.encodeExtAddr(pduUserData);
if (m_network->m_dumpDataPacket) {
uint8_t rawDataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(rawDataBlock, 0xAAU, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
dataBlock.getData(rawDataBlock);
Utils::dump(2U, "Data Block", rawDataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
LogMessage(LOG_RF, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
dataHeader.getEXSAP(), dataHeader.getSrcLLId());
}
if (dataHeader.getFormat() != PDUFormatType::AMBT) {
edac::CRC::addCRC32(pduUserData, packetLength);
}
else {
Utils::dump("OSP PDU User Data", pduUserData, packetLength);
// generate the PDU data
for (uint32_t i = 0U; i < blocksToFollow; i++) {
data::DataBlock dataBlock = data::DataBlock();
dataBlock.setFormat(dataHeader);
dataBlock.setSerialNo(i);
dataBlock.setData(pduUserData + dataOffset);
LogMessage(LOG_NET, P25_PDU_STR ", OSP, peerId = %u, block %u, fmt = $%02X, lastBlock = %u",
peerId, (dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? dataBlock.getSerialNo() : i, dataBlock.getFormat(),
dataBlock.getLastBlock());
@ -428,16 +435,16 @@ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNe
dataBlock.getData(rawDataBlock);
Utils::dump(2U, "Data Block", rawDataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
}
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
dataBlock.encode(buffer);
writeNetwork(peerId, peerNet, dataHeader, networkBlock, buffer, P25_PDU_FEC_LENGTH_BYTES, (dataBlock.getLastBlock()) ? RTP_END_OF_CALL_SEQ : pktSeq, streamId);
++pktSeq;
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
dataBlock.encode(buffer);
writeNetwork(peerId, peerNet, dataHeader, networkBlock, buffer, P25_PDU_FEC_LENGTH_BYTES, (dataBlock.getLastBlock()) ? RTP_END_OF_CALL_SEQ : pktSeq, streamId);
++pktSeq;
networkBlock++;
networkBlock++;
dataOffset += (dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
dataOffset += (dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}
}
}

11
src/fne/network/callhandler/packetdata/P25PacketData.h
Unescape View File

@ -80,8 +80,6 @@ namespace network
p25::data::DataBlock* blockData;
p25::data::DataHeader header;
p25::data::DataHeader secondHeader;
bool useSecondHeader;
bool extendedAddress;
uint32_t dataOffset;
uint8_t dataBlockCnt;
@ -100,8 +98,6 @@ namespace network
peerId(0U),
blockData(nullptr),
header(),
secondHeader(),
useSecondHeader(false),
extendedAddress(false),
dataOffset(0U),
dataBlockCnt(0U),
@ -144,12 +140,11 @@ namespace network
* @param peerId Peer ID.
* @param peerNet Instance of PeerNetwork to use to send traffic.
* @param dataHeader Instance of a PDU data header.
* @param secondHeader Instance of a PDU data header.
* @param useSecondHeader Flag indicating whether or not to use a second data header.
* @param extendedAddress Flag indicating whether or not to extended addressing is in use.
* @param pduUserData Buffer containing user data to transmit.
*/
void write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNet, p25::data::DataHeader& dataHeader, p25::data::DataHeader& secondHeader,
bool useSecondHeader, uint8_t* pduUserData, bool queueOnly = false);
void write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNet, p25::data::DataHeader& dataHeader,
bool extendedAddress, uint8_t* pduUserData, bool queueOnly = false);
/**
* @brief Write data processed to the network.

2
src/host/p25/packet/ControlSignaling.cpp
Unescape View File

@ -1608,7 +1608,7 @@ void ControlSignaling::writeRF_TSDU_AMBT(lc::AMBT* ambt, bool imm)
Utils::dump(1U, "!!! *PDU (AMBT) TSBK Block Data", pduUserData, P25_PDU_UNCONFIRMED_LENGTH_BYTES * header.getBlocksToFollow());
}
m_p25->m_data->writeRF_PDU_User(header, header, false, pduUserData, imm);
m_p25->m_data->writeRF_PDU_User(header, false, pduUserData, imm);
}
/*

456
src/host/p25/packet/Data.cpp
Unescape View File

@ -81,6 +81,7 @@ bool Data::process(uint8_t* data, uint32_t len)
if (duid == DUID::PDU) {
if (m_p25->m_rfState != RS_RF_DATA) {
m_rfDataHeader.reset();
m_rfExtendedAddress = false;
m_rfDataBlockCnt = 0U;
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
@ -111,7 +112,7 @@ bool Data::process(uint8_t* data, uint32_t len)
Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
m_rfDataHeader.reset();
m_rfSecondHeader.reset();
m_rfExtendedAddress = false;
m_rfDataBlockCnt = 0U;
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
@ -131,7 +132,7 @@ bool Data::process(uint8_t* data, uint32_t len)
LogError(LOG_RF, P25_PDU_STR ", ISP, too many PDU blocks to process, %u > %u", m_rfDataHeader.getBlocksToFollow(), P25_MAX_PDU_BLOCKS);
m_rfDataHeader.reset();
m_rfSecondHeader.reset();
m_rfExtendedAddress = false;
m_rfDataBlockCnt = 0U;
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
@ -148,7 +149,7 @@ bool Data::process(uint8_t* data, uint32_t len)
m_p25->m_ccHalted = false;
m_rfDataHeader.reset();
m_rfSecondHeader.reset();
m_rfExtendedAddress = false;
m_rfDataBlockCnt = 0U;
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
@ -159,7 +160,6 @@ bool Data::process(uint8_t* data, uint32_t len)
// only send data blocks across the network, if we're not an AMBT,
// an RSP or a registration service
if ((m_rfDataHeader.getFormat() != PDUFormatType::AMBT) &&
(m_rfDataHeader.getFormat() != PDUFormatType::RSP) &&
(m_rfDataHeader.getSAP() != PDUSAP::CONV_DATA_REG)) {
writeNetwork(0U, buffer, P25_PDU_FEC_LENGTH_BYTES, false);
}
@ -167,19 +167,19 @@ bool Data::process(uint8_t* data, uint32_t len)
if (m_p25->m_rfState == RS_RF_DATA) {
uint32_t blocksToFollow = m_rfDataHeader.getBlocksToFollow();
uint32_t dataOffset = 0U;
// process second header if we're using enhanced addressing
if (m_rfDataHeader.getSAP() == PDUSAP::EXT_ADDR &&
m_rfDataHeader.getFormat() == PDUFormatType::UNCONFIRMED) {
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
Utils::getBitRange(m_rfPDU, buffer, offset, P25_PDU_FEC_LENGTH_BITS);
bool ret = m_rfSecondHeader.decode(buffer);
bool ret = m_rfDataHeader.decodeExtAddr(buffer);
if (!ret) {
LogWarning(LOG_RF, P25_PDU_STR ", unfixable RF 1/2 rate second header data");
Utils::dump(1U, "Unfixable PDU Data", m_rfPDU + offset, P25_PDU_HEADER_LENGTH_BYTES);
m_rfDataHeader.reset();
m_rfSecondHeader.reset();
m_rfUseSecondHeader = false;
m_rfDataBlockCnt = 0U;
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
@ -188,55 +188,43 @@ bool Data::process(uint8_t* data, uint32_t len)
}
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", ISP, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u",
m_rfSecondHeader.getFormat(), m_rfSecondHeader.getMFId(), m_rfSecondHeader.getSAP(), m_rfSecondHeader.getFullMessage(),
m_rfSecondHeader.getBlocksToFollow(), m_rfSecondHeader.getPadLength(), m_rfSecondHeader.getNs(), m_rfSecondHeader.getFSN(), m_rfSecondHeader.getLastFragment(),
m_rfSecondHeader.getHeaderOffset(), m_rfSecondHeader.getLLId());
LogMessage(LOG_RF, P25_PDU_STR ", ISP, sap = $%02X, srcLlId = %u",
m_rfDataHeader.getEXSAP(), m_rfDataHeader.getSrcLLId());
}
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() != PDUFormatType::AMBT) &&
(m_rfDataHeader.getFormat() != PDUFormatType::RSP) &&
(m_rfDataHeader.getSAP() != PDUSAP::CONV_DATA_REG)) {
writeNetwork(1U, buffer, P25_PDU_FEC_LENGTH_BYTES, false);
}
m_rfExtendedAddress = true;
writeNetwork(1U, buffer, P25_PDU_FEC_LENGTH_BYTES, false);
offset += P25_PDU_FEC_LENGTH_BITS;
m_rfPDUCount++;
blocksToFollow--;
m_rfDataBlockCnt++;
// if we are using a secondary header place it in the PDU user data buffer
m_rfDataHeader.getExtAddrData(m_pduUserData + dataOffset);
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
m_pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES;
}
uint32_t srcId = m_rfDataHeader.getLLId();
uint32_t dstId = (m_rfUseSecondHeader || m_rfExtendedAddress) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId();
uint32_t srcId = (m_rfExtendedAddress) ? m_rfDataHeader.getSrcLLId() : m_rfDataHeader.getLLId();
uint32_t dstId = m_rfDataHeader.getLLId();
m_rfPDUCount++;
uint32_t bitLength = ((blocksToFollow + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
if (m_rfPDUBits >= bitLength) {
// process all blocks in the data stream
uint32_t dataOffset = 0U;
// if the primary header has a header offset ensure data if offset by that amount
if (m_rfDataHeader.getHeaderOffset() > 0U) {
offset += m_rfDataHeader.getHeaderOffset() * 8;
m_pduUserDataLength -= m_rfDataHeader.getHeaderOffset();
}
// if we are using a secondary header place it in the PDU user data buffer
if (m_rfUseSecondHeader) {
m_rfSecondHeader.getData(m_pduUserData + dataOffset);
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
m_pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES;
}
// decode data blocks
for (uint32_t i = 0U; i < blocksToFollow; i++) {
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
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_rfDataHeader);
if (ret) {
// if we are getting unconfirmed or confirmed blocks, and if we've reached the total number of blocks
// set this block as the last block for full packet CRC
@ -250,8 +238,8 @@ bool Data::process(uint8_t* data, uint32_t len)
if (m_rfDataHeader.getSAP() == PDUSAP::EXT_ADDR && m_rfDataHeader.getFormat() == PDUFormatType::CONFIRMED &&
m_rfData[i].getSerialNo() == 0U) {
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", ISP, 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());
LogMessage(LOG_RF, P25_PDU_STR ", ISP, block %u, fmt = $%02X, lastBlock = %u",
m_rfData[i].getSerialNo(), m_rfData[i].getFormat(), m_rfData[i].getLastBlock());
if (m_dumpPDUData) {
uint8_t dataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
@ -261,12 +249,20 @@ bool Data::process(uint8_t* data, uint32_t len)
}
}
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());
dstId = m_rfSecondHeader.getLLId();
uint8_t secondHeader[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(secondHeader, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
m_rfData[i].getData(secondHeader);
Utils::dump("p25 second header", secondHeader, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
m_rfDataHeader.decodeExtAddr(secondHeader);
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", ISP, sap = $%02X, srcLlId = %u",
m_rfDataHeader.getEXSAP(), m_rfDataHeader.getSrcLLId());
}
srcId = m_rfDataHeader.getSrcLLId();
m_rfExtendedAddress = true;
}
else {
@ -294,9 +290,6 @@ bool Data::process(uint8_t* data, uint32_t len)
(m_rfDataHeader.getFormat() != PDUFormatType::RSP) &&
(m_rfDataHeader.getSAP() != PDUSAP::CONV_DATA_REG)) {
uint32_t networkBlock = m_rfDataBlockCnt + 1U;
if (m_rfUseSecondHeader)
++networkBlock;
writeNetwork(networkBlock, buffer, P25_PDU_FEC_LENGTH_BYTES, m_rfData[i].getLastBlock());
}
@ -464,7 +457,7 @@ bool Data::process(uint8_t* data, uint32_t len)
if (m_repeatPDU) {
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", repeating PDU, llId = %u", (m_rfUseSecondHeader || m_rfExtendedAddress) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId());
LogMessage(LOG_RF, P25_PDU_STR ", repeating PDU, llId = %u", (m_rfExtendedAddress) ? m_rfDataHeader.getSrcLLId() : m_rfDataHeader.getLLId());
}
writeRF_PDU_Buffered(); // re-generate buffered PDU and send it on
@ -476,8 +469,7 @@ bool Data::process(uint8_t* data, uint32_t len)
}
m_rfDataHeader.reset();
m_rfSecondHeader.reset();
m_rfUseSecondHeader = false;
m_rfExtendedAddress = false;
m_rfDataBlockCnt = 0U;
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
@ -505,7 +497,6 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
if (m_p25->m_netState != RS_NET_DATA) {
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netDataOffset = 0U;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
@ -524,7 +515,6 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
m_p25->m_netState = RS_NET_IDLE;
@ -543,7 +533,6 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
LogError(LOG_NET, P25_PDU_STR ", too many PDU blocks to process, %u > %u", m_netDataHeader.getBlocksToFollow(), P25_MAX_PDU_BLOCKS);
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netDataOffset = 0U;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
@ -558,7 +547,6 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
}
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netDataOffset = 0U;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
@ -567,6 +555,63 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
}
m_netPDUCount++;
// did we receive a response header?
if (m_netDataHeader.getFormat() == PDUFormatType::RSP) {
m_p25->m_netState = RS_NET_IDLE;
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", ISP, response, fmt = $%02X, rspClass = $%02X, rspType = $%02X, rspStatus = $%02X, llId = %u, srcLlId = %u",
m_netDataHeader.getFormat(), m_netDataHeader.getResponseClass(), m_netDataHeader.getResponseType(), m_netDataHeader.getResponseStatus(),
m_netDataHeader.getLLId(), m_netDataHeader.getSrcLLId());
if (m_netDataHeader.getResponseClass() == PDUAckClass::ACK && m_netDataHeader.getResponseType() == PDUAckType::ACK) {
LogMessage(LOG_NET, P25_PDU_STR ", ISP, response, OSP ACK, llId = %u",
m_netDataHeader.getLLId());
} else {
if (m_netDataHeader.getResponseClass() == PDUAckClass::NACK) {
switch (m_netDataHeader.getResponseType()) {
case PDUAckType::NACK_ILLEGAL:
LogMessage(LOG_NET, P25_PDU_STR ", ISP, response, OSP NACK, illegal format, llId = %u",
m_netDataHeader.getLLId());
break;
case PDUAckType::NACK_PACKET_CRC:
LogMessage(LOG_NET, P25_PDU_STR ", ISP, response, OSP NACK, packet CRC error, llId = %u",
m_netDataHeader.getLLId());
break;
case PDUAckType::NACK_SEQ:
case PDUAckType::NACK_OUT_OF_SEQ:
LogMessage(LOG_NET, P25_PDU_STR ", ISP, response, OSP NACK, packet out of sequence, llId = %u",
m_netDataHeader.getLLId());
break;
case PDUAckType::NACK_UNDELIVERABLE:
LogMessage(LOG_NET, P25_PDU_STR ", ISP, response, OSP NACK, packet undeliverable, llId = %u",
m_netDataHeader.getLLId());
break;
default:
break;
}
}
}
}
if (m_repeatPDU) {
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", repeating PDU, llId = %u, srcLlId = %u", m_netDataHeader.getLLId(), m_netDataHeader.getSrcLLId());
}
writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on
}
m_netDataHeader.reset();
m_netExtendedAddress = false;
m_netDataOffset = 0U;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
m_pduUserDataLength = 0U;
}
return true;
}
@ -579,6 +624,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
if (m_netDataBlockCnt >= m_netDataHeader.getBlocksToFollow()) {
uint32_t blocksToFollow = m_netDataHeader.getBlocksToFollow();
uint32_t offset = 0U;
uint32_t dataOffset = 0U;
uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
@ -588,14 +634,12 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
::memcpy(buffer, m_netPDU, P25_PDU_FEC_LENGTH_BYTES);
bool ret = m_netSecondHeader.decode(buffer);
bool ret = m_netDataHeader.decodeExtAddr(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_netUseSecondHeader = false;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
m_p25->m_netState = RS_NET_IDLE;
@ -603,36 +647,30 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
}
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %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.getPadLength(), m_netSecondHeader.getNs(), m_netSecondHeader.getFSN(), m_netSecondHeader.getLastFragment(),
m_netSecondHeader.getHeaderOffset(), m_netSecondHeader.getLLId());
LogMessage(LOG_NET, P25_PDU_STR ", ISP, sap = $%02X, srcLlId = %u",
m_netDataHeader.getEXSAP(), m_netDataHeader.getSrcLLId());
}
m_netUseSecondHeader = true;
m_netExtendedAddress = true;
offset += P25_PDU_FEC_LENGTH_BYTES;
blocksToFollow--;
}
m_netDataBlockCnt = 0U;
m_netDataBlockCnt++;
// process all blocks in the data stream
uint32_t dataOffset = 0U;
// if we are using a secondary header place it in the PDU user data buffer
if (m_netUseSecondHeader) {
m_netSecondHeader.getData(m_pduUserData + dataOffset);
// if we are using a secondary header place it in the PDU user data buffer
m_netDataHeader.getExtAddrData(m_pduUserData + dataOffset);
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
m_pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES;
}
m_netDataBlockCnt = 0U;
// decode data blocks
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);
bool ret = m_netData[i].decode(buffer, m_netDataHeader);
if (ret) {
// if we are getting unconfirmed or confirmed blocks, and if we've reached the total number of blocks
// set this block as the last block for full packet CRC
@ -645,20 +683,29 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
// are we processing extended address data from the first block?
if (m_netDataHeader.getSAP() == PDUSAP::EXT_ADDR && m_netDataHeader.getFormat() == PDUFormatType::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());
if (m_dumpPDUData) {
uint8_t dataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(dataBlock, 0xAAU, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
m_netData[i].getData(dataBlock);
Utils::dump(2U, "Network Data Block", dataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", ISP, block %u, fmt = $%02X, lastBlock = %u",
m_netData[i].getSerialNo(), m_netData[i].getFormat(), m_netData[i].getLastBlock());
if (m_dumpPDUData) {
uint8_t dataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(dataBlock, 0xAAU, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
m_netData[i].getData(dataBlock);
Utils::dump(2U, "Data Block", dataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
}
uint8_t secondHeader[P25_PDU_HEADER_LENGTH_BYTES];
::memset(secondHeader, 0x00U, P25_PDU_HEADER_LENGTH_BYTES);
m_netData[i].getData(secondHeader);
m_netDataHeader.decodeExtAddr(secondHeader);
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", ISP, sap = $%02X, srcLlId = %u",
m_netDataHeader.getEXSAP(), m_netDataHeader.getSrcLLId());
}
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 {
@ -710,13 +757,13 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
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 srcId = (m_netExtendedAddress) ? m_netDataHeader.getSrcLLId() : 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_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", transmitting network PDU, llId = %u", (m_netUseSecondHeader || m_netExtendedAddress) ? m_netSecondHeader.getLLId() : m_netDataHeader.getLLId());
LogMessage(LOG_NET, P25_PDU_STR ", transmitting network PDU, llId = %u", (m_netExtendedAddress) ? m_netDataHeader.getSrcLLId() : m_netDataHeader.getLLId());
}
writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on
@ -724,7 +771,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
::ActivityLog("P25", false, "end of Net data transmission");
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netExtendedAddress = false;
m_netDataOffset = 0U;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
@ -757,7 +804,7 @@ bool Data::hasLLIdFNEReg(uint32_t llId) const
/* Helper to write user data as a P25 PDU packet. */
void Data::writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& secondHeader, bool useSecondHeader, uint8_t* pduUserData, bool imm)
void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, uint8_t* pduUserData, bool imm)
{
assert(pduUserData != nullptr);
@ -785,56 +832,51 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& seco
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
offset += P25_PDU_FEC_LENGTH_BITS;
uint32_t dataOffset = 0U;
if (blocksToFollow > 0U) {
uint32_t dataOffset = 0U;
uint32_t packetLength = (dataHeader.getPacketLength() + dataHeader.getPadLength() + 4U);
// generate the second PDU header
if (useSecondHeader) {
secondHeader.encode(pduUserData, true);
// generate the second PDU header
if ((dataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (dataHeader.getSAP() == PDUSAP::EXT_ADDR) && extendedAddress) {
dataHeader.encodeExtAddr(pduUserData, true);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
secondHeader.encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
dataHeader.encodeExtAddr(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
bitLength += P25_PDU_FEC_LENGTH_BITS;
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
blocksToFollow--;
bitLength += P25_PDU_FEC_LENGTH_BITS;
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
packetLength += P25_PDU_HEADER_LENGTH_BYTES;
blocksToFollow--;
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, bitLength = %u, llId = %u",
secondHeader.getFormat(), secondHeader.getMFId(), secondHeader.getSAP(), secondHeader.getFullMessage(),
secondHeader.getBlocksToFollow(), secondHeader.getPadLength(), secondHeader.getNs(), secondHeader.getFSN(), secondHeader.getLastFragment(),
secondHeader.getHeaderOffset(), bitLength, secondHeader.getLLId());
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
dataHeader.getEXSAP(), dataHeader.getSrcLLId());
}
}
}
if (dataHeader.getFormat() != PDUFormatType::AMBT) {
edac::CRC::addCRC32(pduUserData, (dataHeader.getPacketLength() + dataHeader.getPadLength() + 4U));
}
// generate the PDU data
for (uint32_t i = 0U; i < blocksToFollow; i++) {
DataBlock dataBlock = DataBlock();
dataBlock.setFormat((useSecondHeader) ? secondHeader : dataHeader);
dataBlock.setSerialNo(i);
dataBlock.setData(pduUserData + dataOffset);
// are we processing extended address data from the first block?
if (dataHeader.getSAP() == PDUSAP::EXT_ADDR && dataHeader.getFormat() == PDUFormatType::CONFIRMED &&
dataBlock.getSerialNo() == 0U) {
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u, sap = $%02X, llId = %u",
dataBlock.getSerialNo(), dataBlock.getFormat(), dataBlock.getLastBlock(), dataBlock.getSAP(), dataBlock.getLLId());
if ((dataHeader.getFormat() == PDUFormatType::CONFIRMED) && (dataHeader.getSAP() == PDUSAP::EXT_ADDR) && extendedAddress) {
dataHeader.encodeExtAddr(pduUserData);
if (m_dumpPDUData) {
uint8_t rawDataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(rawDataBlock, 0xAAU, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
dataBlock.getData(rawDataBlock);
Utils::dump(2U, "Data Block", rawDataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
dataHeader.getEXSAP(), dataHeader.getSrcLLId());
}
}
else {
if (dataHeader.getFormat() != PDUFormatType::AMBT) {
edac::CRC::addCRC32(pduUserData, packetLength);
}
// generate the PDU data
for (uint32_t i = 0U; i < blocksToFollow; i++) {
DataBlock dataBlock = DataBlock();
dataBlock.setFormat(dataHeader);
dataBlock.setSerialNo(i);
dataBlock.setData(pduUserData + dataOffset);
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u",
(dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? dataBlock.getSerialNo() : i, dataBlock.getFormat(),
@ -847,14 +889,14 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& seco
Utils::dump(2U, "Data Block", rawDataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
}
}
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
dataBlock.encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
dataBlock.encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += (dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += (dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}
}
writeRF_PDU(data, bitLength, false, imm);
@ -1052,8 +1094,6 @@ Data::Data(Control* p25, bool dumpPDUData, bool repeatPDU, bool debug, bool verb
m_prevRfState(RS_RF_LISTENING),
m_rfData(nullptr),
m_rfDataHeader(),
m_rfSecondHeader(),
m_rfUseSecondHeader(false),
m_rfExtendedAddress(false),
m_rfDataBlockCnt(0U),
m_rfPDU(nullptr),
@ -1061,8 +1101,6 @@ Data::Data(Control* p25, bool dumpPDUData, bool repeatPDU, bool debug, bool verb
m_rfPDUBits(0U),
m_netData(nullptr),
m_netDataHeader(),
m_netSecondHeader(),
m_netUseSecondHeader(false),
m_netExtendedAddress(false),
m_netDataOffset(0U),
m_netDataBlockCnt(0U),
@ -1225,7 +1263,7 @@ bool Data::processSNDCPControl()
osp->encode(pduUserData);
rspHeader.calculateLength(2U);
writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, false, pduUserData);
return true;
}
@ -1240,7 +1278,7 @@ bool Data::processSNDCPControl()
osp->encode(pduUserData);
rspHeader.calculateLength(2U);
writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, false, pduUserData);
sndcpReset(llId, true);
}
@ -1255,7 +1293,7 @@ bool Data::processSNDCPControl()
osp->encode(pduUserData);
rspHeader.calculateLength(2U);
writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, false, pduUserData);
sndcpReset(llId, true);
@ -1291,7 +1329,7 @@ bool Data::processSNDCPControl()
osp->encode(pduUserData);
rspHeader.calculateLength(2U);
writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, false, pduUserData);
sndcpReset(llId, true);
}
@ -1424,57 +1462,50 @@ void Data::writeNet_PDU_Buffered()
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
offset += P25_PDU_FEC_LENGTH_BITS;
uint32_t dataOffset = 0U;
if (blocksToFollow > 0U) {
uint32_t dataOffset = 0U;
// generate the second PDU header
if (m_netUseSecondHeader) {
m_netSecondHeader.encode(m_pduUserData, true);
// generate the second PDU header
if ((m_netDataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (m_netDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_netExtendedAddress) {
m_netDataHeader.encodeExtAddr(m_pduUserData, true);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_netSecondHeader.encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_netDataHeader.encodeExtAddr(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
bitLength += P25_PDU_FEC_LENGTH_BITS;
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
blocksToFollow--;
bitLength += P25_PDU_FEC_LENGTH_BITS;
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
blocksToFollow--;
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", OSP, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %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.getPadLength(), m_netSecondHeader.getNs(), m_netSecondHeader.getFSN(), m_netSecondHeader.getLastFragment(),
m_netSecondHeader.getHeaderOffset(), m_netSecondHeader.getLLId());
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
m_netDataHeader.getEXSAP(), m_netDataHeader.getSrcLLId());
}
}
}
edac::CRC::addCRC32(m_pduUserData, m_pduUserDataLength);
// generate the PDU data
for (uint32_t i = 0U; i < blocksToFollow; i++) {
m_netData[i].setFormat((m_netUseSecondHeader) ? m_netSecondHeader : m_netDataHeader);
m_netData[i].setSerialNo(i);
m_netData[i].setData(m_pduUserData + dataOffset);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_netData[i].encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
// are we processing extended address data from the first block?
if (m_netDataHeader.getSAP() == PDUSAP::EXT_ADDR && m_netDataHeader.getFormat() == PDUFormatType::CONFIRMED &&
m_netData[i].getSerialNo() == 0U) {
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", OSP, 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());
if ((m_netDataHeader.getFormat() == PDUFormatType::CONFIRMED) && (m_netDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_netExtendedAddress) {
m_netDataHeader.encodeExtAddr(m_pduUserData);
if (m_dumpPDUData) {
uint8_t dataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(dataBlock, 0xAAU, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
m_netData[i].getData(dataBlock);
Utils::dump(2U, "Data Block", dataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
m_netDataHeader.getEXSAP(), m_netDataHeader.getSrcLLId());
}
}
else {
edac::CRC::addCRC32(m_pduUserData, m_pduUserDataLength);
// generate the PDU data
for (uint32_t i = 0U; i < blocksToFollow; i++) {
m_netData[i].setFormat(m_netDataHeader);
m_netData[i].setSerialNo(i);
m_netData[i].setData(m_pduUserData + dataOffset);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_netData[i].encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u",
(m_netDataHeader.getFormat() == PDUFormatType::CONFIRMED) ? m_netData[i].getSerialNo() : i, m_netData[i].getFormat(),
@ -1487,10 +1518,10 @@ void Data::writeNet_PDU_Buffered()
Utils::dump(2U, "Data Block", dataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
}
}
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += (m_netDataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += (m_netDataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}
}
writeRF_PDU(data, bitLength);
@ -1526,11 +1557,11 @@ void Data::writeRF_PDU_Buffered()
uint32_t dataOffset = 0U;
// generate the second PDU header
if (m_rfUseSecondHeader && m_rfDataHeader.getFormat() != PDUFormatType::RSP) {
m_rfSecondHeader.encode(m_pduUserData, true);
if ((m_rfDataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (m_rfDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_rfExtendedAddress) {
m_rfDataHeader.encodeExtAddr(m_pduUserData, true);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_rfSecondHeader.encode(block);
m_rfDataHeader.encodeExtAddr(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
bitLength += P25_PDU_FEC_LENGTH_BITS;
@ -1539,10 +1570,18 @@ void Data::writeRF_PDU_Buffered()
blocksToFollow--;
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, bitLength = %u, llId = %u",
m_rfSecondHeader.getFormat(), m_rfSecondHeader.getMFId(), m_rfSecondHeader.getSAP(), m_rfSecondHeader.getFullMessage(),
m_rfSecondHeader.getBlocksToFollow(), m_rfSecondHeader.getPadLength(), m_rfSecondHeader.getNs(), m_rfSecondHeader.getFSN(), m_rfSecondHeader.getLastFragment(),
m_rfSecondHeader.getHeaderOffset(), bitLength, m_rfSecondHeader.getLLId());
LogMessage(LOG_RF, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
m_rfDataHeader.getEXSAP(), m_rfDataHeader.getSrcLLId());
}
}
// are we processing extended address data from the first block?
if ((m_rfDataHeader.getFormat() == PDUFormatType::CONFIRMED) && (m_rfDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_rfExtendedAddress) {
m_rfDataHeader.encodeExtAddr(m_pduUserData);
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
m_rfDataHeader.getEXSAP(), m_rfDataHeader.getSrcLLId());
}
}
@ -1550,37 +1589,20 @@ void Data::writeRF_PDU_Buffered()
// generate the PDU data
for (uint32_t i = 0U; i < blocksToFollow; i++) {
m_rfData[i].setFormat((m_rfUseSecondHeader) ? m_rfSecondHeader : m_rfDataHeader);
m_rfData[i].setFormat(m_rfDataHeader);
m_rfData[i].setSerialNo(i);
m_rfData[i].setData(m_pduUserData + dataOffset);
// are we processing extended address data from the first block?
if (m_rfDataHeader.getSAP() == PDUSAP::EXT_ADDR && m_rfDataHeader.getFormat() == PDUFormatType::CONFIRMED &&
m_rfData[i].getSerialNo() == 0U) {
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, 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());
if (m_dumpPDUData) {
uint8_t dataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(dataBlock, 0xAAU, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
m_rfData[i].getData(dataBlock);
Utils::dump(2U, "Data Block", dataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
}
}
else {
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u",
(m_rfDataHeader.getFormat() == PDUFormatType::CONFIRMED) ? m_rfData[i].getSerialNo() : i, m_rfData[i].getFormat(),
m_rfData[i].getLastBlock());
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u",
(m_rfDataHeader.getFormat() == PDUFormatType::CONFIRMED) ? m_rfData[i].getSerialNo() : i, m_rfData[i].getFormat(),
m_rfData[i].getLastBlock());
if (m_dumpPDUData) {
uint8_t dataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(dataBlock, 0xAAU, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
m_rfData[i].getData(dataBlock);
Utils::dump(2U, "Data Block", dataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
if (m_dumpPDUData) {
uint8_t dataBlock[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(dataBlock, 0xAAU, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
m_rfData[i].getData(dataBlock);
Utils::dump(2U, "Data Block", dataBlock, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
}
@ -1626,7 +1648,7 @@ void Data::writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, uint32_t ipA
}
rspHeader.calculateLength(12U);
writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, false, pduUserData);
}
/* Helper to write a PDU acknowledge response. */

9
src/host/p25/packet/Data.h
Unescape View File

@ -83,12 +83,11 @@ namespace p25
/**
* @brief Helper to write user data as a P25 PDU packet.
* @param dataHeader Instance of a PDU data header.
* @param secondHeader Instance of a PDU data header.
* @param useSecondHeader Flag indicating whether or not to use a second data header.
* @param extendedAddress Flag indicating whether or not to extended addressing is in use.
* @param pduUserData Buffer containing user data to transmit.
* @param imm Flag indicating the PDU should be written to the immediate queue.
*/
void writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& secondHeader, bool useSecondHeader, uint8_t* pduUserData, bool imm = false);
void writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, uint8_t* pduUserData, bool imm = false);
/**
* @brief Updates the processor by the passed number of milliseconds.
@ -123,8 +122,6 @@ namespace p25
data::DataBlock* m_rfData;
data::DataHeader m_rfDataHeader;
data::DataHeader m_rfSecondHeader;
bool m_rfUseSecondHeader;
bool m_rfExtendedAddress;
uint8_t m_rfDataBlockCnt;
uint8_t* m_rfPDU;
@ -133,8 +130,6 @@ namespace p25
data::DataBlock* m_netData;
data::DataHeader m_netDataHeader;
data::DataHeader m_netSecondHeader;
bool m_netUseSecondHeader;
bool m_netExtendedAddress;
uint32_t m_netDataOffset;
uint8_t m_netDataBlockCnt;

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.