diff --git a/src/common/p25/data/Assembler.cpp b/src/common/p25/data/Assembler.cpp index d056a18e..081d463e 100644 --- a/src/common/p25/data/Assembler.cpp +++ b/src/common/p25/data/Assembler.cpp @@ -231,12 +231,6 @@ bool Assembler::disassemble(const uint8_t* pduBlock, uint32_t blockLength, bool dataBlocks[i].getData(m_pduUserData + dataOffset); - // is this the first unconfirmed data block after a auxiliary ES header? - if (i == 0U && dataHeader.getFormat() == PDUFormatType::UNCONFIRMED && m_auxiliaryES) { - uint8_t exSAP = m_pduUserData[0U]; // first byte of the first data block after an aux ES header is the extended SAP - dataHeader.setEXSAP(exSAP); - } - dataOffset += (dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; m_dataBlockCnt++; } @@ -274,6 +268,12 @@ bool Assembler::disassemble(const uint8_t* pduBlock, uint32_t blockLength, bool 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 @@ -346,9 +346,9 @@ UInt8Array Assembler::assemble(data::DataHeader& dataHeader, bool extendedAddres 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; - if (dataHeader.getPadLength() > 0U) - bitLength += (dataHeader.getPadLength() * 8U); uint32_t offset = P25_PREAMBLE_LENGTH_BITS; diff --git a/src/common/p25/data/DataHeader.cpp b/src/common/p25/data/DataHeader.cpp index 2c0e2f2c..48494742 100644 --- a/src/common/p25/data/DataHeader.cpp +++ b/src/common/p25/data/DataHeader.cpp @@ -445,6 +445,8 @@ bool DataHeader::decodeAuxES(const uint8_t* data) m_exSap = m_auxESData[12U] & 0x3FU; // Service Access Point } else if (m_fmt == PDUFormatType::UNCONFIRMED) { + // only the first 12 octets fit in block zero. Assembler resolves the + // thirteenth octet (EXSAP) after the next block is available ::memcpy(m_auxESData, data, P25_PDU_HEADER_LENGTH_BYTES); #if DEBUG_P25_PDU_DATA @@ -650,8 +652,8 @@ uint32_t DataHeader::getAuxiliaryESData(uint8_t* buffer) const ::memcpy(buffer, m_auxESData, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); return P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; } else { - ::memcpy(buffer, m_auxESData, P25_PDU_HEADER_LENGTH_BYTES); - return P25_PDU_HEADER_LENGTH_BYTES; + ::memcpy(buffer, m_auxESData, P25_PDU_HEADER_LENGTH_BYTES + 1U); + return P25_PDU_HEADER_LENGTH_BYTES + 1U; } } @@ -674,13 +676,10 @@ void DataHeader::calculateLength(uint32_t packetLength) uint32_t blockLen = (m_fmt == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; - if (len > blockLen) { - m_padLength = blockLen - (len % blockLen); - m_blocksToFollow = (uint8_t)ceilf((float)len / (float)blockLen); - } else { - m_padLength = 0U; + m_padLength = (blockLen - (len % blockLen)) % blockLen; + m_blocksToFollow = (uint8_t)((len + blockLen - 1U) / blockLen); + if (m_blocksToFollow == 0U) m_blocksToFollow = 1U; - } } /* Helper to determine the pad length for a given packet length. */ @@ -689,10 +688,10 @@ uint32_t DataHeader::calculatePadLength(uint8_t fmt, uint32_t packetLength) { uint32_t len = packetLength + 4U; // packet length + CRC32 if (fmt == PDUFormatType::CONFIRMED) { - return P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - (len % P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); + return (P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - (len % P25_PDU_CONFIRMED_DATA_LENGTH_BYTES)) % P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; } else { - return P25_PDU_UNCONFIRMED_LENGTH_BYTES - (len % P25_PDU_UNCONFIRMED_LENGTH_BYTES); + return (P25_PDU_UNCONFIRMED_LENGTH_BYTES - (len % P25_PDU_UNCONFIRMED_LENGTH_BYTES)) % P25_PDU_UNCONFIRMED_LENGTH_BYTES; } } diff --git a/src/host/modem/Modem.cpp b/src/host/modem/Modem.cpp index bf375b58..76ece249 100644 --- a/src/host/modem/Modem.cpp +++ b/src/host/modem/Modem.cpp @@ -1866,43 +1866,47 @@ bool Modem::writeP25Frame(const uint8_t* data, uint32_t length, bool imm) return false; } - DECLARE_UINT8_ARRAY(buffer, MAX_LENGTH); + // the serialized modem message includes framing bytes in addition to + // the caller's tagged P25 frame + DECLARE_UINT8_ARRAY(buffer, MAX_LENGTH + 3U); - if (length < 252U) { + const uint32_t p25Length = length; + uint32_t serializedLength = 0U; + + if (p25Length < 252U) { + serializedLength = p25Length + 2U; buffer[0U] = DVM_SHORT_FRAME_START; - buffer[1U] = length + 2U; + buffer[1U] = serializedLength; buffer[2U] = CMD_P25_DATA; - ::memcpy(buffer + 3U, data + 1U, length - 1U); + ::memcpy(buffer + 3U, data + 1U, p25Length - 1U); } else { - length += 3U; + serializedLength = p25Length + 3U; buffer[0U] = DVM_LONG_FRAME_START; - buffer[1U] = (length >> 8U) & 0xFFU; - buffer[2U] = (length & 0xFFU); + buffer[1U] = (serializedLength >> 8U) & 0xFFU; + buffer[2U] = serializedLength & 0xFFU; buffer[3U] = CMD_P25_DATA; - ::memcpy(buffer + 4U, data + 1U, length - 1U); + ::memcpy(buffer + 4U, data + 1U, p25Length - 1U); } - uint32_t len = length + 2U; - // write or buffer P25 data to air interface // for V.24/DFSI, the modem status space value may lag call startup; do // not block initial Net->RF frames solely on stale p25Space - if (m_p25Space >= length || isV24Connected()) { + if (m_p25Space >= p25Length || isV24Connected()) { if (m_debug) - LogDebugEx(LOG_MODEM, "Modem::writeP25Frame()", "immediate write (len %u)", length); + LogDebugEx(LOG_MODEM, "Modem::writeP25Frame()", "immediate write (len %u)", p25Length); if (m_trace) - Utils::dump(1U, "Modem::writeP25Frame(), Immediate TX P25 Data", buffer + 3U, length - 3U); + Utils::dump(1U, "Modem::writeP25Frame(), Immediate TX P25 Data", data + 1U, p25Length - 1U); - int ret = write(buffer, len, imm); - if (ret != int(len)) { + int ret = write(buffer, serializedLength, imm); + if (ret != int(serializedLength)) { LogError(LOG_MODEM, "Error writing P25 data"); return false; } - m_p25Space -= length; + m_p25Space -= p25Length; if ((int32_t)m_p25Space < 0) { if (m_debug) - LogDebugEx(LOG_MODEM, "Modem::writeP25Frame()", "p25Space underflow, space = %u, length = %u", m_p25Space, length); + LogDebugEx(LOG_MODEM, "Modem::writeP25Frame()", "p25Space underflow, space = %u, length = %u", m_p25Space, p25Length); m_p25Space = 0U; } } diff --git a/src/host/p25/packet/Data.cpp b/src/host/p25/packet/Data.cpp index c168e3bc..83d8f745 100644 --- a/src/host/p25/packet/Data.cpp +++ b/src/host/p25/packet/Data.cpp @@ -946,6 +946,23 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ac assert(pdu != nullptr); assert(bitLength > 0U); + // status insertion expands the no-status bitstream -- encode into a + // dynamically sized staging buffer first, then enforce the native modem's + // single-frame CAI limit before queuing or recording retry state + const uint32_t stagingBytes = ((bitLength * 2U) + 7U) / 8U; + UInt8Array dataArray = std::make_unique(stagingBytes + 2U); + uint8_t* data = dataArray.get(); + ::memset(data, 0x00U, stagingBytes + 2U); + + uint32_t newBitLength = P25Utils::encodeByLength(pdu, data + 2U, bitLength); + uint32_t newByteLength = (newBitLength + 7U) / 8U; + if (newByteLength > P25_PDU_FRAME_LENGTH_BYTES) { + LogError(LOG_RF, P25_PDU_STR ", OSP frame exceeds native modem limit, blocks/bitLength/airBytes = %u/%u/%u", + (bitLength > P25_PREAMBLE_LENGTH_BITS) ? ((bitLength - P25_PREAMBLE_LENGTH_BITS) / P25_PDU_FEC_LENGTH_BITS) - 1U : 0U, + bitLength, newByteLength); + return; + } + m_p25->writeRF_TDU(true, imm); for (uint8_t i = 0U; i < 5U; i++) @@ -965,15 +982,6 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ac LogInfoEx(LOG_RF, P25_PDU_STR ", OSP, ack retry, bitLength = %u", bitLength); } - uint8_t data[P25_PDU_FRAME_LENGTH_BYTES + 2U]; - ::memset(data, 0x00U, P25_PDU_FRAME_LENGTH_BYTES + 2U); - - // add the data - uint32_t newBitLength = P25Utils::encodeByLength(pdu, data + 2U, bitLength); - uint32_t newByteLength = newBitLength / 8U; - if ((newBitLength % 8U) > 0U) - newByteLength++; - // generate Sync Sync::addP25Sync(data + 2U); diff --git a/tests/fne/FNETestHooks.cpp b/tests/fne/FNETestHooks.cpp index ddf7311b..8106eb0c 100644 --- a/tests/fne/FNETestHooks.cpp +++ b/tests/fne/FNETestHooks.cpp @@ -158,7 +158,6 @@ bool FNETestHooks::processOTARDLDPDU(TrafficNetwork& network, const std::vector< if (assembled == nullptr || bitLength <= P25_PREAMBLE_LENGTH_BITS) return false; - bitLength -= header.getPadLength() * 8U; const uint32_t blockCount = (bitLength - P25_PREAMBLE_LENGTH_BITS) / P25_PDU_FEC_LENGTH_BITS; auto* packetData = network.m_tagP25->packetData(); for (uint32_t block = 0U; block < blockCount; ++block) { diff --git a/tests/p25/PDU_Confirmed_AuxES_Test.cpp b/tests/p25/PDU_Confirmed_AuxES_Test.cpp index e2e09172..a1cb3f25 100644 --- a/tests/p25/PDU_Confirmed_AuxES_Test.cpp +++ b/tests/p25/PDU_Confirmed_AuxES_Test.cpp @@ -85,9 +85,6 @@ TEST_CASE("P25 PDU Confirmed AuxES Test", "[p25][pdu_confirmed_auxes]") { uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES); - // for the purposes of our test we strip the pad bit length from the bit length - bitLength -= dataHeader.getPadLength() * 8U; - uint32_t blockCnt = 0U; for (uint32_t i = P25_PREAMBLE_LENGTH_BITS; i < bitLength; i += P25_PDU_FEC_LENGTH_BITS) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); diff --git a/tests/p25/PDU_Confirmed_ExtAddr_Test.cpp b/tests/p25/PDU_Confirmed_ExtAddr_Test.cpp index db2bddfb..35e40398 100644 --- a/tests/p25/PDU_Confirmed_ExtAddr_Test.cpp +++ b/tests/p25/PDU_Confirmed_ExtAddr_Test.cpp @@ -77,9 +77,6 @@ TEST_CASE("P25 PDU Confirmed ExtAddr Test", "[p25][pdu_confirmed_extaddr]") { uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES); - // for the purposes of our test we strip the pad bit length from the bit length - bitLength -= dataHeader.getPadLength() * 8U; - uint32_t blockCnt = 0U; for (uint32_t i = P25_PREAMBLE_LENGTH_BITS; i < bitLength; i += P25_PDU_FEC_LENGTH_BITS) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); diff --git a/tests/p25/PDU_Confirmed_Large_Test.cpp b/tests/p25/PDU_Confirmed_Large_Test.cpp index f6f3a6cd..a55fd741 100644 --- a/tests/p25/PDU_Confirmed_Large_Test.cpp +++ b/tests/p25/PDU_Confirmed_Large_Test.cpp @@ -80,9 +80,6 @@ TEST_CASE("P25 PDU Confirmed Large Test", "[p25][pdu_confirmed_large]") { uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES); - // for the purposes of our test we strip the pad bit length from the bit length - bitLength -= dataHeader.getPadLength() * 8U; - uint32_t blockCnt = 0U; for (uint32_t i = P25_PREAMBLE_LENGTH_BITS; i < bitLength; i += P25_PDU_FEC_LENGTH_BITS) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); diff --git a/tests/p25/PDU_Confirmed_Small_Test.cpp b/tests/p25/PDU_Confirmed_Small_Test.cpp index 90aa47ed..46d44039 100644 --- a/tests/p25/PDU_Confirmed_Small_Test.cpp +++ b/tests/p25/PDU_Confirmed_Small_Test.cpp @@ -74,9 +74,6 @@ TEST_CASE("P25 PDU Confirmed Small Test", "[p25][pdu_confirmed_small]") { uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES); - // for the purposes of our test we strip the pad bit length from the bit length - bitLength -= dataHeader.getPadLength() * 8U; - uint32_t blockCnt = 0U; for (uint32_t i = P25_PREAMBLE_LENGTH_BITS; i < bitLength; i += P25_PDU_FEC_LENGTH_BITS) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); @@ -114,3 +111,24 @@ TEST_CASE("P25 PDU Confirmed Small Test", "[p25][pdu_confirmed_small]") { REQUIRE(failed==false); } + +TEST_CASE("P25 PDU exact block boundaries have no padding", "[p25][pdu][length]") +{ + DataHeader confirmed; + confirmed.setFormat(PDUFormatType::CONFIRMED); + confirmed.setSAP(PDUSAP::USER_DATA); + confirmed.calculateLength(28U); // payload + CRC32 == two 16-octet blocks + CHECK(confirmed.getBlocksToFollow() == 2U); + CHECK(confirmed.getPadLength() == 0U); + CHECK(confirmed.getPacketLength() == 28U); + CHECK(DataHeader::calculatePadLength(PDUFormatType::CONFIRMED, 28U) == 0U); + + DataHeader unconfirmed; + unconfirmed.setFormat(PDUFormatType::UNCONFIRMED); + unconfirmed.setSAP(PDUSAP::USER_DATA); + unconfirmed.calculateLength(20U); // payload + CRC32 == two 12-octet blocks + CHECK(unconfirmed.getBlocksToFollow() == 2U); + CHECK(unconfirmed.getPadLength() == 0U); + CHECK(unconfirmed.getPacketLength() == 20U); + CHECK(DataHeader::calculatePadLength(PDUFormatType::UNCONFIRMED, 20U) == 0U); +} diff --git a/tests/p25/PDU_Unconfirmed_AuxES_Test.cpp b/tests/p25/PDU_Unconfirmed_AuxES_Test.cpp index f5aeb396..c76c1510 100644 --- a/tests/p25/PDU_Unconfirmed_AuxES_Test.cpp +++ b/tests/p25/PDU_Unconfirmed_AuxES_Test.cpp @@ -85,9 +85,6 @@ TEST_CASE("P25 PDU Unconfirmed AuxES Test", "[p25][pdu_unconfirmed_auxes]") { uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES); - // for the purposes of our test we strip the pad bit length from the bit length - bitLength -= dataHeader.getPadLength() * 8U; - uint32_t blockCnt = 0U; for (uint32_t i = P25_PREAMBLE_LENGTH_BITS; i < bitLength; i += P25_PDU_FEC_LENGTH_BITS) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); @@ -110,6 +107,8 @@ TEST_CASE("P25 PDU Unconfirmed AuxES Test", "[p25][pdu_unconfirmed_auxes]") { } if (assembler.getComplete()) { + CHECK(assembler.getAuxiliaryES()); + CHECK(assembler.dataHeader.getEXSAP() == PDUSAP::USER_DATA); uint8_t pduUserData2[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U]; uint32_t pduUserDataLength = assembler.getUserDataLength(); assembler.getUserData(pduUserData2); diff --git a/tests/p25/PDU_Unconfirmed_ExtAddr_Test.cpp b/tests/p25/PDU_Unconfirmed_ExtAddr_Test.cpp index cf62b34d..9fdea7ff 100644 --- a/tests/p25/PDU_Unconfirmed_ExtAddr_Test.cpp +++ b/tests/p25/PDU_Unconfirmed_ExtAddr_Test.cpp @@ -77,9 +77,6 @@ TEST_CASE("P25 PDU Unconfirmed ExtAddr Test", "[p25][pdu_unconfirmed_extaddr]") uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES); - // for the purposes of our test we strip the pad bit length from the bit length - bitLength -= dataHeader.getPadLength() * 8U; - uint32_t blockCnt = 0U; for (uint32_t i = P25_PREAMBLE_LENGTH_BITS; i < bitLength; i += P25_PDU_FEC_LENGTH_BITS) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); diff --git a/tests/p25/PDU_Unconfirmed_Test.cpp b/tests/p25/PDU_Unconfirmed_Test.cpp index 6fd1c753..7ea91e55 100644 --- a/tests/p25/PDU_Unconfirmed_Test.cpp +++ b/tests/p25/PDU_Unconfirmed_Test.cpp @@ -80,9 +80,6 @@ TEST_CASE("P25 PDU Unconfirmed Test", "[p25][pdu_unconfirmed]") { uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES); - // for the purposes of our test we strip the pad bit length from the bit length - bitLength -= dataHeader.getPadLength() * 8U; - uint32_t blockCnt = 0U; for (uint32_t i = P25_PREAMBLE_LENGTH_BITS; i < bitLength; i += P25_PDU_FEC_LENGTH_BITS) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);