You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
569 lines
26 KiB
569 lines
26 KiB
// SPDX-License-Identifier: GPL-2.0-only
|
|
/*
|
|
* Digital Voice Modem - Common Library
|
|
* GPLv2 Open Source. Use is subject to license terms.
|
|
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
|
|
*
|
|
* Copyright (C) 2025-2026 Bryan Biedenkapp, N2PLL
|
|
*
|
|
*/
|
|
#include "Defines.h"
|
|
#include "p25/P25Defines.h"
|
|
#include "p25/data/Assembler.h"
|
|
#include "edac/CRC.h"
|
|
#include "Log.h"
|
|
#include "Utils.h"
|
|
|
|
using namespace p25;
|
|
using namespace p25::defines;
|
|
using namespace p25::data;
|
|
|
|
#include <cassert>
|
|
#include <cstring>
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Static Class Members
|
|
// ---------------------------------------------------------------------------
|
|
|
|
bool Assembler::s_dumpPDUData = false;
|
|
bool Assembler::s_verbose = false;
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Public Class Members
|
|
// ---------------------------------------------------------------------------
|
|
|
|
/* Initializes a new instance of the Assembler class. */
|
|
|
|
Assembler::Assembler() :
|
|
dataBlocks(nullptr),
|
|
dataHeader(),
|
|
m_extendedAddress(false),
|
|
m_auxiliaryES(false),
|
|
m_dataBlockCnt(0U),
|
|
m_undecodableBlockCnt(0U),
|
|
m_packetCRCFailed(false),
|
|
m_complete(false),
|
|
m_pduUserData(nullptr),
|
|
m_pduUserDataLength(0U),
|
|
m_blockCount(0U),
|
|
m_dataOffset(0U),
|
|
m_usingCustomWriter(false),
|
|
m_blockWriter(nullptr)
|
|
{
|
|
dataBlocks = new data::DataBlock[P25_MAX_PDU_BLOCKS];
|
|
|
|
m_pduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U];
|
|
::memset(m_pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
|
|
|
|
m_rawPDU = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_FEC_LENGTH_BYTES];
|
|
::memset(m_rawPDU, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_FEC_LENGTH_BYTES);
|
|
}
|
|
|
|
/* Finalizes a instance of the Assembler class. */
|
|
|
|
Assembler::~Assembler()
|
|
{
|
|
if (dataBlocks != nullptr)
|
|
delete[] dataBlocks;
|
|
if (m_pduUserData != nullptr)
|
|
delete[] m_pduUserData;
|
|
if (m_rawPDU != nullptr)
|
|
delete[] m_rawPDU;
|
|
}
|
|
|
|
/* Helper to disassemble a P25 PDU frame into user data. */
|
|
|
|
bool Assembler::disassemble(const uint8_t* pduBlock, uint32_t blockLength, bool resetState)
|
|
{
|
|
assert(pduBlock != nullptr);
|
|
|
|
if (blockLength != P25_PDU_FEC_LENGTH_BYTES) {
|
|
LogError(LOG_P25, P25_PDU_STR ", invalid encoded block length, %u != %u",
|
|
blockLength, P25_PDU_FEC_LENGTH_BYTES);
|
|
resetDisassemblyState();
|
|
return false;
|
|
}
|
|
|
|
if (resetState) {
|
|
resetDisassemblyState();
|
|
}
|
|
|
|
#if DEBUG_P25_PDU_DATA
|
|
Utils::dump(1U, "P25, PDU Disassembler Block", pduBlock, blockLength);
|
|
#endif
|
|
|
|
UInt8Array dataArray = std::make_unique<uint8_t[]>(P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
|
|
uint8_t* data = dataArray.get();
|
|
::memset(data, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
|
|
|
|
if (m_blockCount == 0U) {
|
|
bool ret = dataHeader.decode(pduBlock);
|
|
if (!ret) {
|
|
LogWarning(LOG_P25, P25_PDU_STR ", unfixable RF 1/2 rate header data");
|
|
Utils::dump(1U, "P25, Unfixable PDU Data", pduBlock, P25_PDU_FEC_LENGTH_BYTES);
|
|
|
|
resetDisassemblyState();
|
|
return false;
|
|
}
|
|
|
|
if (s_verbose) {
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", ISP, ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, S = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u",
|
|
dataHeader.getAckNeeded(), dataHeader.getOutbound(), dataHeader.getFormat(), dataHeader.getMFId(), dataHeader.getSAP(), dataHeader.getFullMessage(),
|
|
dataHeader.getBlocksToFollow(), dataHeader.getPadLength(), dataHeader.getPacketLength(), dataHeader.getSynchronize(), dataHeader.getNs(), dataHeader.getFSN(), dataHeader.getLastFragment(),
|
|
dataHeader.getHeaderOffset(), dataHeader.getLLId());
|
|
}
|
|
|
|
if (dataHeader.getPacketLength() > P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U) {
|
|
LogError(LOG_P25, P25_PDU_STR ", ISP, packet length %u exceeds maximum supported size %u",
|
|
dataHeader.getPacketLength(), P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES);
|
|
|
|
resetDisassemblyState();
|
|
return false;
|
|
}
|
|
|
|
// make sure we don't get a PDU with more blocks then we support
|
|
if (dataHeader.getBlocksToFollow() >= P25_MAX_PDU_BLOCKS) {
|
|
LogError(LOG_P25, P25_PDU_STR ", ISP, too many PDU blocks to process, %u > %u", dataHeader.getBlocksToFollow(), P25_MAX_PDU_BLOCKS);
|
|
|
|
resetDisassemblyState();
|
|
return false;
|
|
}
|
|
|
|
m_blockCount++;
|
|
m_complete = false;
|
|
return true;
|
|
}
|
|
else {
|
|
const uint32_t rawOffset = (m_blockCount - 1U) * blockLength;
|
|
const uint32_t rawCapacity = P25_MAX_PDU_BLOCKS * P25_PDU_FEC_LENGTH_BYTES;
|
|
if (rawOffset > rawCapacity || blockLength > rawCapacity - rawOffset) {
|
|
LogError(LOG_P25, P25_PDU_STR ", encoded PDU exceeds assembler capacity, offset/length/capacity = %u/%u/%u",
|
|
rawOffset, blockLength, rawCapacity);
|
|
resetDisassemblyState();
|
|
return false;
|
|
}
|
|
|
|
::memcpy(m_rawPDU + rawOffset, pduBlock, blockLength);
|
|
|
|
m_dataOffset += blockLength;
|
|
m_blockCount++;
|
|
|
|
if (m_blockCount - 1U >= dataHeader.getBlocksToFollow()) {
|
|
#if DEBUG_P25_PDU_DATA
|
|
LogDebugEx(LOG_P25, "Assembler::disassemble()", "complete PDU, blocksToFollow = %u, blockCount = %u", dataHeader.getBlocksToFollow(), m_blockCount);
|
|
Utils::dump(1U, "Assembler::disassemble() rawPDU", m_rawPDU, m_dataOffset);
|
|
#endif
|
|
uint32_t blocksToFollow = dataHeader.getBlocksToFollow();
|
|
uint32_t offset = 0U;
|
|
uint32_t dataOffset = 0U;
|
|
|
|
uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
|
|
|
|
m_dataBlockCnt = 0U;
|
|
|
|
// process all blocks in the data stream
|
|
// the header offset counts octets of reassembled user data, it does not shift the coded blocks
|
|
|
|
uint32_t packetLength = dataHeader.getPacketLength();
|
|
uint32_t padLength = dataHeader.getPadLength();
|
|
uint32_t secondHeaderOffset = 0U;
|
|
|
|
// decode data blocks
|
|
for (uint32_t i = 0U; i < blocksToFollow; i++) {
|
|
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
|
|
::memcpy(buffer, m_rawPDU + offset, P25_PDU_FEC_LENGTH_BYTES);
|
|
|
|
bool ret = dataBlocks[i].decode(buffer, dataHeader);
|
|
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
|
|
if ((dataHeader.getFormat() == PDUFormatType::CONFIRMED) || (dataHeader.getFormat() == PDUFormatType::UNCONFIRMED)) {
|
|
if ((m_dataBlockCnt + 1U) == blocksToFollow) {
|
|
dataBlocks[i].setLastBlock(true);
|
|
}
|
|
}
|
|
|
|
// fake data block serial number for unconfirmed mode
|
|
if (dataHeader.getFormat() == PDUFormatType::UNCONFIRMED && dataBlocks[i].getSerialNo() == 0U)
|
|
dataBlocks[i].setSerialNo(i);
|
|
|
|
// are we processing extended address data from the first block?
|
|
if (dataHeader.getSAP() == PDUSAP::EXT_ADDR && dataBlocks[i].getSerialNo() == 0U) {
|
|
uint8_t secondHeader[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
|
|
::memset(secondHeader, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
|
|
dataBlocks[i].getData(secondHeader);
|
|
|
|
dataHeader.decodeExtAddr(secondHeader);
|
|
if (s_verbose) {
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", ISP, block %u, fmt = $%02X, lastBlock = %u, sap = $%02X, srcLlId = %u",
|
|
dataBlocks[i].getSerialNo(), dataBlocks[i].getFormat(), dataBlocks[i].getLastBlock(), dataHeader.getEXSAP(), dataHeader.getSrcLLId());
|
|
}
|
|
|
|
m_extendedAddress = true;
|
|
if (dataHeader.getFormat() == PDUFormatType::CONFIRMED)
|
|
secondHeaderOffset += 4U;
|
|
else
|
|
secondHeaderOffset += P25_PDU_HEADER_LENGTH_BYTES;
|
|
}
|
|
else {
|
|
// are we processing auxiliary ES data from the first block?
|
|
if ((dataHeader.getSAP() == PDUSAP::ENC_USER_DATA || dataHeader.getSAP() == PDUSAP::ENC_KMM) &&
|
|
dataBlocks[i].getSerialNo() == 0U) {
|
|
uint8_t secondHeader[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
|
|
::memset(secondHeader, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
|
|
dataBlocks[i].getData(secondHeader);
|
|
|
|
dataHeader.decodeAuxES(secondHeader);
|
|
if (s_verbose) {
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", ISP, block %u, fmt = $%02X, lastBlock = %u, sap = $%02X, exSAP = $%02X, algoId = $%02X, kId = $%04X",
|
|
dataBlocks[i].getSerialNo(), dataBlocks[i].getFormat(), dataBlocks[i].getLastBlock(), dataHeader.getSAP(),
|
|
dataHeader.getEXSAP(), dataHeader.getAlgId(), dataHeader.getKId());
|
|
|
|
if (dataHeader.getAlgId() != ALGO_UNENCRYPT) {
|
|
uint8_t mi[MI_LENGTH_BYTES];
|
|
::memset(mi, 0x00U, MI_LENGTH_BYTES);
|
|
|
|
dataHeader.getMI(mi);
|
|
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", ISP, Enc Sync, block %u, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X",
|
|
dataBlocks[i].getSerialNo(), mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]);
|
|
}
|
|
}
|
|
|
|
m_auxiliaryES = true;
|
|
if (dataHeader.getFormat() == PDUFormatType::CONFIRMED)
|
|
secondHeaderOffset += 13U;
|
|
else
|
|
secondHeaderOffset += P25_PDU_HEADER_LENGTH_BYTES + 1U;
|
|
}
|
|
else {
|
|
if (s_verbose) {
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", ISP, block %u, fmt = $%02X, lastBlock = %u",
|
|
(dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? dataBlocks[i].getSerialNo() : m_dataBlockCnt, dataBlocks[i].getFormat(),
|
|
dataBlocks[i].getLastBlock());
|
|
}
|
|
}
|
|
}
|
|
|
|
dataBlocks[i].getData(m_pduUserData + dataOffset);
|
|
|
|
dataOffset += (dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
|
|
m_dataBlockCnt++;
|
|
}
|
|
else {
|
|
if (dataBlocks[i].getFormat() == PDUFormatType::CONFIRMED) {
|
|
LogWarning(LOG_P25, P25_PDU_STR ", unfixable PDU data (3/4 rate or CRC), block %u", i);
|
|
|
|
// to prevent data block offset errors fill the bad block with 0's
|
|
uint8_t blankBuf[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
|
|
::memset(blankBuf, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
|
|
::memcpy(m_pduUserData + dataOffset, blankBuf, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
|
|
|
|
dataOffset += P25_PDU_CONFIRMED_DATA_LENGTH_BYTES;
|
|
m_dataBlockCnt++;
|
|
m_undecodableBlockCnt++;
|
|
}
|
|
else {
|
|
LogWarning(LOG_P25, P25_PDU_STR ", unfixable PDU data (1/2 rate or CRC), block %u", i);
|
|
|
|
// to prevent data block offset errors fill the bad block with 0's
|
|
uint8_t blankBuf[P25_PDU_UNCONFIRMED_LENGTH_BYTES];
|
|
::memset(blankBuf, 0x00U, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
|
|
::memcpy(m_pduUserData + dataOffset, blankBuf, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
|
|
|
|
dataOffset += P25_PDU_UNCONFIRMED_LENGTH_BYTES;
|
|
m_dataBlockCnt++;
|
|
m_undecodableBlockCnt++;
|
|
}
|
|
|
|
if (s_dumpPDUData) {
|
|
Utils::dump(1U, "P25, Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
|
|
}
|
|
}
|
|
|
|
offset += P25_PDU_FEC_LENGTH_BYTES;
|
|
}
|
|
|
|
// an unconfirmed Auxiliary ES header is 13 octets and crosses the
|
|
// first 12-octet block boundary -- resolve EXSAP after all blocks
|
|
// have been copied into the contiguous PDU buffer
|
|
if (dataHeader.getFormat() == PDUFormatType::UNCONFIRMED && m_auxiliaryES && blocksToFollow > 1U)
|
|
dataHeader.setEXSAP(m_pduUserData[P25_PDU_HEADER_LENGTH_BYTES] & 0x3FU);
|
|
|
|
#if DEBUG_P25_PDU_DATA
|
|
LogDebugEx(LOG_P25, "Assembler::disassemble()", "packetLength = %u, secondHeaderOffset = %u, padLength = %u, pduLength = %u", packetLength, secondHeaderOffset, padLength, dataHeader.getPDULength());
|
|
#endif
|
|
if (dataHeader.getBlocksToFollow() > 0U) {
|
|
// BUGFIX (verified against a real captured RF frame via brute-force range
|
|
// search against the actual received CRC bytes -- see tools/verify_crc/):
|
|
// CRC-32 covers the ENTIRE PDU content stream -- second header (if any) +
|
|
// payload + padding -- everything except its own trailing 4 bytes. It is
|
|
// NOT scoped to payload-only (my first attempted fix was wrong), and padding
|
|
// is NOT excluded from it either (the original stock code was also wrong, in
|
|
// the other direction). Because the CRC bytes already sit immediately after
|
|
// the padding in m_pduUserData, no buffer rearrangement is needed at all --
|
|
// the old padded/non-padded branching collapses into one direct check.
|
|
uint32_t crcCoveredLength = packetLength + padLength;
|
|
|
|
bool crcRet = edac::CRC::checkCRC32(m_pduUserData, crcCoveredLength + 4U);
|
|
if (!crcRet) {
|
|
// DIAGNOSTIC: log the received CRC bytes next to what we'd compute over
|
|
// this range, to distinguish a real algorithm/scope mismatch from RF
|
|
// corruption on any future failure.
|
|
uint8_t recompute[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U];
|
|
::memcpy(recompute, m_pduUserData, crcCoveredLength + 4U);
|
|
edac::CRC::addCRC32(recompute, crcCoveredLength + 4U);
|
|
|
|
LogWarning(LOG_P25, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u, "
|
|
"received = $%02X%02X%02X%02X, computed = $%02X%02X%02X%02X",
|
|
dataHeader.getBlocksToFollow(), crcCoveredLength,
|
|
m_pduUserData[crcCoveredLength], m_pduUserData[crcCoveredLength + 1U], m_pduUserData[crcCoveredLength + 2U], m_pduUserData[crcCoveredLength + 3U],
|
|
recompute[crcCoveredLength], recompute[crcCoveredLength + 1U], recompute[crcCoveredLength + 2U], recompute[crcCoveredLength + 3U]);
|
|
m_packetCRCFailed = true;
|
|
}
|
|
}
|
|
|
|
// reorganize PDU buffer for second header offsetting
|
|
if (secondHeaderOffset > 0U) {
|
|
uint8_t tempBuf[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U];
|
|
::memcpy(tempBuf, m_pduUserData + secondHeaderOffset, packetLength - 4U);
|
|
::memset(m_pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
|
|
::memcpy(m_pduUserData, tempBuf, packetLength - secondHeaderOffset);
|
|
}
|
|
|
|
if (s_dumpPDUData && m_dataBlockCnt > 0U) {
|
|
Utils::dump(1U, "P25, PDU Packet", m_pduUserData, packetLength - secondHeaderOffset);
|
|
}
|
|
|
|
if (m_dataBlockCnt < blocksToFollow) {
|
|
LogWarning(LOG_P25, P25_PDU_STR ", incomplete PDU (%d / %d blocks)", m_dataBlockCnt, blocksToFollow);
|
|
}
|
|
|
|
m_pduUserDataLength = packetLength - secondHeaderOffset;
|
|
|
|
m_blockCount = 0U;
|
|
m_complete = true;
|
|
return true;
|
|
} else {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
/* Helper to assemble user data as a P25 PDU packet. */
|
|
|
|
UInt8Array Assembler::assemble(data::DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES,
|
|
const uint8_t* pduUserData, uint32_t* assembledBitLength, void* userContext)
|
|
{
|
|
assert(pduUserData != nullptr);
|
|
|
|
if (assembledBitLength != nullptr)
|
|
*assembledBitLength = 0U;
|
|
|
|
// padding is already contained in the payload capacity of the blocks
|
|
// declared by BTF; it does not extend the CAI frame
|
|
uint32_t bitLength = ((dataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
|
|
|
|
uint32_t offset = P25_PREAMBLE_LENGTH_BITS;
|
|
|
|
UInt8Array dataArray = std::make_unique<uint8_t[]>((bitLength / 8U) + 1U);
|
|
uint8_t* data = dataArray.get();
|
|
::memset(data, 0x00U, (bitLength / 8U) + 1U);
|
|
|
|
uint8_t block[P25_PDU_FEC_LENGTH_BYTES];
|
|
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
|
|
|
|
uint32_t blocksToFollow = dataHeader.getBlocksToFollow();
|
|
|
|
if (s_verbose) {
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", OSP, ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, S = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, bitLength = %u, llId = %u",
|
|
dataHeader.getAckNeeded(), dataHeader.getOutbound(), dataHeader.getFormat(), dataHeader.getMFId(), dataHeader.getSAP(), dataHeader.getFullMessage(),
|
|
dataHeader.getBlocksToFollow(), dataHeader.getPadLength(), dataHeader.getPacketLength(), dataHeader.getSynchronize(), dataHeader.getNs(), dataHeader.getFSN(), dataHeader.getLastFragment(),
|
|
dataHeader.getHeaderOffset(), bitLength, dataHeader.getLLId());
|
|
}
|
|
|
|
// generate the PDU header and 1/2 rate Trellis
|
|
dataHeader.encode(block);
|
|
|
|
#if DEBUG_P25_PDU_DATA
|
|
Utils::dump(1U, "P25, PDU Assembler Block", block, P25_PDU_FEC_LENGTH_BYTES);
|
|
#endif
|
|
|
|
if (m_usingCustomWriter && m_blockWriter != nullptr)
|
|
m_blockWriter(userContext, 0U, block, P25_PDU_FEC_LENGTH_BYTES, false);
|
|
else
|
|
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
|
|
offset += P25_PDU_FEC_LENGTH_BITS;
|
|
|
|
if (pduUserData != nullptr && blocksToFollow > 0U) {
|
|
uint32_t dataOffset = 0U;
|
|
uint32_t pduLength = dataHeader.getPDULength() + dataHeader.getPadLength();
|
|
uint32_t dataBlockCnt = 1U;
|
|
uint32_t secondHeaderOffset = 0U;
|
|
|
|
// we pad 20 bytes of extra space -- confirmed data will use various extra space in the PDU
|
|
DECLARE_UINT8_ARRAY(packetData, pduLength + 20U);
|
|
|
|
// are we processing extended address data from the first block?
|
|
if (dataHeader.getSAP() == PDUSAP::EXT_ADDR && extendedAddress) {
|
|
if (dataHeader.getFormat() == PDUFormatType::CONFIRMED)
|
|
secondHeaderOffset += 4U;
|
|
else
|
|
secondHeaderOffset += P25_PDU_HEADER_LENGTH_BYTES;
|
|
dataHeader.encodeExtAddr(packetData);
|
|
|
|
if (s_verbose) {
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", OSP, extended address, sap = $%02X, srcLlId = %u",
|
|
dataHeader.getEXSAP(), dataHeader.getSrcLLId());
|
|
}
|
|
}
|
|
|
|
// are we processing auxiliary ES data from the first block?
|
|
if ((dataHeader.getSAP() == PDUSAP::ENC_USER_DATA || dataHeader.getSAP() == PDUSAP::ENC_KMM) && auxiliaryES) {
|
|
if (dataHeader.getFormat() == PDUFormatType::CONFIRMED)
|
|
secondHeaderOffset += 13U;
|
|
else
|
|
secondHeaderOffset += P25_PDU_HEADER_LENGTH_BYTES + 1U;
|
|
dataHeader.encodeAuxES(packetData);
|
|
|
|
if (s_verbose) {
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", OSP, auxiliary ES, exSAP = $%02X, algId = $%02X, kId = $%04X",
|
|
dataHeader.getEXSAP(), dataHeader.getAlgId(), dataHeader.getKId());
|
|
|
|
if (dataHeader.getAlgId() != ALGO_UNENCRYPT) {
|
|
uint8_t mi[MI_LENGTH_BYTES];
|
|
::memset(mi, 0x00U, MI_LENGTH_BYTES);
|
|
|
|
dataHeader.getMI(mi);
|
|
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", OSP, Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X",
|
|
mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]);
|
|
}
|
|
}
|
|
}
|
|
|
|
uint32_t packetLength = dataHeader.getPacketLength();
|
|
uint32_t padLength = dataHeader.getPadLength();
|
|
#if DEBUG_P25_PDU_DATA
|
|
LogDebugEx(LOG_P25, "Assembler::assemble()", "packetLength = %u, secondHeaderOffset = %u, padLength = %u, pduLength = %u", packetLength, secondHeaderOffset, padLength, pduLength);
|
|
#endif
|
|
if (dataHeader.getFormat() != PDUFormatType::AMBT) {
|
|
// BUGFIX (verified against a real captured RF frame -- see disassemble() above
|
|
// for the brute-force verification detail): pduUserData is netPayloadLength bytes
|
|
// (packetLength - secondHeaderOffset), not the gross packetLength, so copy only
|
|
// that much into its correct position after the second header. CRC-32 then covers
|
|
// the ENTIRE stream -- second header + payload + padding (already zero from
|
|
// DECLARE_UINT8_ARRAY's zero-init) -- everything except its own 4 bytes.
|
|
// addCRC32 writes those bytes directly to their final position, so the old
|
|
// separate "move the CRC after padding" step is no longer needed at all.
|
|
uint32_t netPayloadLength = packetLength - secondHeaderOffset;
|
|
::memcpy(packetData + secondHeaderOffset, pduUserData, netPayloadLength);
|
|
edac::CRC::addCRC32(packetData, packetLength + padLength + 4U);
|
|
} else {
|
|
// our AMBTs have a pre-calculated CRC-32 -- we don't need to do it ourselves
|
|
::memcpy(packetData + secondHeaderOffset, pduUserData, pduLength);
|
|
}
|
|
|
|
#if DEBUG_P25_PDU_DATA
|
|
Utils::dump(1U, "P25, Assembled PDU User Data", packetData, packetLength + padLength + 4U);
|
|
#endif
|
|
|
|
// generate the PDU data
|
|
for (uint32_t i = 0U; i < blocksToFollow; i++) {
|
|
DataBlock dataBlock = DataBlock();
|
|
dataBlock.setFormat(dataHeader);
|
|
dataBlock.setSerialNo(i);
|
|
dataBlock.setData(packetData + dataOffset);
|
|
dataBlock.setLastBlock((i + 1U) == blocksToFollow);
|
|
|
|
if (s_verbose) {
|
|
LogInfoEx(LOG_P25, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u",
|
|
(dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? dataBlock.getSerialNo() : i, dataBlock.getFormat(),
|
|
dataBlock.getLastBlock());
|
|
}
|
|
|
|
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
|
|
dataBlock.encode(block);
|
|
|
|
#if DEBUG_P25_PDU_DATA
|
|
Utils::dump(1U, "P25, PDU Assembler Block", block, P25_PDU_FEC_LENGTH_BYTES);
|
|
#endif
|
|
|
|
if (m_usingCustomWriter && m_blockWriter != nullptr)
|
|
m_blockWriter(userContext, dataBlockCnt, block, P25_PDU_FEC_LENGTH_BYTES, dataBlock.getLastBlock());
|
|
else
|
|
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;
|
|
dataBlockCnt++;
|
|
}
|
|
}
|
|
|
|
if (assembledBitLength != nullptr)
|
|
*assembledBitLength = bitLength;
|
|
|
|
if (m_usingCustomWriter) {
|
|
return nullptr;
|
|
}
|
|
return dataArray;
|
|
}
|
|
|
|
/* Gets the raw user user data stored. */
|
|
|
|
uint32_t Assembler::getUserData(uint8_t* buffer) const
|
|
{
|
|
assert(buffer != nullptr);
|
|
assert(m_pduUserData != nullptr);
|
|
|
|
if (m_pduUserDataLength == 0U) {
|
|
LogError(LOG_P25, P25_PDU_STR ", no user data available to retrieve! BUGBUG!");
|
|
return 0U;
|
|
}
|
|
|
|
if (m_pduUserDataLength > (P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U)) {
|
|
LogError(LOG_P25, P25_PDU_STR ", user data length %u exceeds maximum allowable size! BUGBUG!", m_pduUserDataLength);
|
|
return 0U;
|
|
}
|
|
|
|
if (m_pduUserData == nullptr) {
|
|
LogError(LOG_P25, P25_PDU_STR ", no user data available to retrieve! BUGBUG!");
|
|
return 0U;
|
|
}
|
|
|
|
if (m_complete) {
|
|
::memcpy(buffer, m_pduUserData, m_pduUserDataLength);
|
|
return m_pduUserDataLength;
|
|
} else {
|
|
return 0U;
|
|
}
|
|
}
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Private Class Members
|
|
// ---------------------------------------------------------------------------
|
|
|
|
/* Internal helper to reset the disassembly state. */
|
|
|
|
void Assembler::resetDisassemblyState()
|
|
{
|
|
dataHeader.reset();
|
|
|
|
m_extendedAddress = false;
|
|
m_auxiliaryES = false;
|
|
|
|
m_dataBlockCnt = 0U;
|
|
m_undecodableBlockCnt = 0U;
|
|
|
|
m_blockCount = 0U;
|
|
m_dataOffset = 0U;
|
|
|
|
::memset(m_pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
|
|
::memset(m_rawPDU, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_FEC_LENGTH_BYTES);
|
|
|
|
m_packetCRCFailed = false;
|
|
m_complete = false;
|
|
}
|