From f3c3cc95cef3239fe151eb949fdbdf7c82883250 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Sat, 6 Jul 2024 22:47:05 -0400 Subject: [PATCH] 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; --- src/common/p25/data/DataBlock.cpp | 33 +- src/common/p25/data/DataBlock.h | 11 +- src/common/p25/data/DataHeader.cpp | 133 ++++- src/common/p25/data/DataHeader.h | 31 +- .../callhandler/packetdata/P25PacketData.cpp | 163 ++++--- .../callhandler/packetdata/P25PacketData.h | 11 +- src/host/p25/packet/ControlSignaling.cpp | 2 +- src/host/p25/packet/Data.cpp | 456 +++++++++--------- src/host/p25/packet/Data.h | 9 +- 9 files changed, 496 insertions(+), 353 deletions(-) diff --git a/src/common/p25/data/DataBlock.cpp b/src/common/p25/data/DataBlock.cpp index d37206e5..c77eb311 100644 --- a/src/common/p25/data/DataBlock.cpp +++ b/src/common/p25/data/DataBlock.cpp @@ -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); diff --git a/src/common/p25/data/DataBlock.h b/src/common/p25/data/DataBlock.h index 7e9fcad1..19f6fa1c 100644 --- a/src/common/p25/data/DataBlock.h +++ b/src/common/p25/data/DataBlock.h @@ -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; diff --git a/src/common/p25/data/DataHeader.cpp b/src/common/p25/data/DataHeader.cpp index 76306179..198955f4 100644 --- a/src/common/p25/data/DataHeader.cpp +++ b/src/common/p25/data/DataHeader.cpp @@ -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) diff --git a/src/common/p25/data/DataHeader.h b/src/common/p25/data/DataHeader.h index 7cce148f..aefeebe4 100644 --- a/src/common/p25/data/DataHeader.h +++ b/src/common/p25/data/DataHeader.h @@ -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. */ @@ -175,7 +201,7 @@ namespace p25 * @brief Response status. */ __PROPERTY(uint8_t, rspStatus, ResponseStatus); - + // AMBT Data /** * @brief Alternate Trunking Block Opcode @@ -194,6 +220,7 @@ namespace p25 edac::Trellis m_trellis; uint8_t* m_data; + uint8_t* m_extAddrData; static bool m_warnCRC; }; diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.cpp b/src/fne/network/callhandler/packetdata/P25PacketData.cpp index 94751de2..7149255b 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.cpp +++ b/src/fne/network/callhandler/packetdata/P25PacketData.cpp @@ -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; + } } } diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.h b/src/fne/network/callhandler/packetdata/P25PacketData.h index 00bf2a1f..207dd99b 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.h +++ b/src/fne/network/callhandler/packetdata/P25PacketData.h @@ -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. diff --git a/src/host/p25/packet/ControlSignaling.cpp b/src/host/p25/packet/ControlSignaling.cpp index f6d51a03..af4f01f7 100644 --- a/src/host/p25/packet/ControlSignaling.cpp +++ b/src/host/p25/packet/ControlSignaling.cpp @@ -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); } /* diff --git a/src/host/p25/packet/Data.cpp b/src/host/p25/packet/Data.cpp index 44e76b0d..22a3ad92 100644 --- a/src/host/p25/packet/Data.cpp +++ b/src/host/p25/packet/Data.cpp @@ -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. */ diff --git a/src/host/p25/packet/Data.h b/src/host/p25/packet/Data.h index 61938fb8..c82bbb21 100644 --- a/src/host/p25/packet/Data.h +++ b/src/host/p25/packet/Data.h @@ -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;