From 1cb128183976e585ab5be8e4def17132a06a9e25 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Thu, 8 Oct 2026 14:28:58 -0400 Subject: [PATCH] harden PDU delivery and batch OTAR rekey commands; --- src/common/p25/data/Assembler.cpp | 27 ++- src/fne/network/P25OTARService.cpp | 222 +++++++++++++----- src/fne/network/P25OTARService.h | 10 +- .../callhandler/packetdata/P25PacketData.cpp | 108 +++++++-- .../callhandler/packetdata/P25PacketData.h | 23 +- src/host/p25/Control.cpp | 25 +- src/host/p25/Control.h | 2 +- src/host/p25/packet/Data.cpp | 54 ++++- src/host/p25/packet/Data.h | 10 +- tests/fne/FNETestHooks.cpp | 31 +++ tests/fne/FNETestHooks.h | 9 + tests/p25/P25_KMM_Tests.cpp | 52 ++++ tests/p25/PDU_Confirmed_Large_Test.cpp | 43 ++++ 13 files changed, 486 insertions(+), 130 deletions(-) diff --git a/src/common/p25/data/Assembler.cpp b/src/common/p25/data/Assembler.cpp index 081d463e..899433e2 100644 --- a/src/common/p25/data/Assembler.cpp +++ b/src/common/p25/data/Assembler.cpp @@ -55,8 +55,8 @@ Assembler::Assembler() : 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_PDU_FRAME_LENGTH_BYTES + 2U]; - ::memset(m_rawPDU, 0x00U, P25_PDU_FRAME_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. */ @@ -67,6 +67,8 @@ Assembler::~Assembler() 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. */ @@ -75,6 +77,13 @@ bool Assembler::disassemble(const uint8_t* pduBlock, uint32_t blockLength, bool { 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(); } @@ -125,7 +134,17 @@ bool Assembler::disassemble(const uint8_t* pduBlock, uint32_t blockLength, bool return true; } else { - ::memcpy(m_rawPDU + ((m_blockCount - 1U) * blockLength), pduBlock, blockLength); + 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++; @@ -545,7 +564,7 @@ void Assembler::resetDisassemblyState() m_dataOffset = 0U; ::memset(m_pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U); - ::memset(m_rawPDU, 0x00U, P25_PDU_FRAME_LENGTH_BYTES + 2U); + ::memset(m_rawPDU, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_FEC_LENGTH_BYTES); m_packetCRCFailed = false; m_complete = false; diff --git a/src/fne/network/P25OTARService.cpp b/src/fne/network/P25OTARService.cpp index 1b89c82c..11672aaa 100644 --- a/src/fne/network/P25OTARService.cpp +++ b/src/fne/network/P25OTARService.cpp @@ -229,9 +229,10 @@ bool P25OTARService::processDLD(const uint8_t* data, uint32_t len, uint32_t llId // The payload is already decrypted. Preserve the outer encryption context so // processKMM can construct the encrypted/authenticated NACKs required by // AACA-D 6.22 and 7.4 without decrypting the request a second time. + std::vector> additionalResponses; UInt8Array pduUserData = processKMM(kmmPayload.get(), len, llId, false, &payloadSize, encrypted ? resolvedAlgoId : ALGO_UNENCRYPT, encrypted ? resolvedKId : 0U, - encrypted ? resolvedMI : nullptr); + encrypted ? resolvedMI : nullptr, false, &additionalResponses); if (pduUserData == nullptr || payloadSize == 0U) { // no OTAR response is required for this message; acknowledge successful processing m_packetData->write_PDU_Ack_Response(PDUAckClass::ACK, PDUAckType::ACK, n, llId, false); @@ -253,18 +254,54 @@ bool P25OTARService::processDLD(const uint8_t* data, uint32_t len, uint32_t llId } } - if (responseEncrypted) { - // re-encrypt the KMM response - pduUserData = cryptKMM(resolvedAlgoId, resolvedKId, resolvedMI, pduUserData.get(), payloadSize, true); - if (pduUserData == nullptr) { - LogError(LOG_P25, P25_KMM_STR ", unable to encrypt KMM response, algoId = $%02X, kID = $%04X", resolvedAlgoId, resolvedKId); + // lambda function to handle dispatching of KMM responses, including encryption if required + auto dispatchResponse = [&](const uint8_t* response, uint32_t responseLength) -> bool { + uint8_t responseMI[MI_LENGTH_BYTES]; + ::memset(responseMI, 0x00U, sizeof(responseMI)); + const uint8_t* outgoing = response; + + UInt8Array encryptedResponse; + if (responseEncrypted) { + P25Crypto responseCrypto; + responseCrypto.generateMI(); + responseCrypto.getMI(responseMI); + encryptedResponse = cryptKMM(resolvedAlgoId, resolvedKId, responseMI, response, responseLength, true); + if (encryptedResponse == nullptr) + return false; + + outgoing = encryptedResponse.get(); + } + + return m_packetData->write_PDU_KMM(outgoing, responseLength, llId, responseEncrypted, + resolvedAlgoId, resolvedKId, responseEncrypted ? responseMI : nullptr); + }; + + // dispatch the initial KMM response + if (!dispatchResponse(pduUserData.get(), payloadSize)) { + if (responseEncrypted) { + LogError(LOG_P25, P25_KMM_STR ", unable to encrypt or dispatch KMM response, algoId = $%02X, kID = $%04X", resolvedAlgoId, resolvedKId); + } + else { + LogError(LOG_P25, P25_KMM_STR ", unable to dispatch KMM response, llId = %u", llId); + } + + sendNack(PDUAckType::NACK_UNDELIVERABLE); + return false; + } + + // dispatch any additional Rekey Command responses + for (const std::vector& response : additionalResponses) { + if (!dispatchResponse(response.data(), (uint32_t)response.size())) { + LogError(LOG_P25, P25_KMM_STR ", unable to dispatch additional Rekey Command, llId = %u", llId); sendNack(PDUAckType::NACK_UNDELIVERABLE); return false; } } - m_packetData->write_PDU_KMM(pduUserData.get(), payloadSize, llId, responseEncrypted, - resolvedAlgoId, resolvedKId, resolvedMI); + /** + * The CAI data-link ACK covers receipt of the triggering request. Each RK3 + * Rekey Command is acknowledged separately by its Rekey-Acknowledgment. + */ m_packetData->write_PDU_Ack_Response(PDUAckClass::ACK, PDUAckType::ACK, n, llId, false); return true; } @@ -396,8 +433,9 @@ void P25OTARService::taskNetworkRx(OTARPacketRequest* req) } uint32_t payloadSize = 0U; + std::vector> additionalResponses; UInt8Array pduUserData = network->processKMM(buffer.get(), kmmLength, 0U, false, &payloadSize, - encrypted ? algoId : ALGO_UNENCRYPT, encrypted ? kid : 0U, encrypted ? mi : nullptr, true); + encrypted ? algoId : ALGO_UNENCRYPT, encrypted ? kid : 0U, encrypted ? mi : nullptr, true, &additionalResponses); if (pduUserData == nullptr || payloadSize == 0U) { if (network->m_debug) LogDebug(LOG_P25, P25_KMM_STR ", no KMM response generated for network request"); @@ -424,31 +462,47 @@ void P25OTARService::taskNetworkRx(OTARPacketRequest* req) } } - if (responseEncrypted) { - // re-encrypt the KMM response - pduUserData = network->cryptKMM(algoId, kid, mi, pduUserData.get(), payloadSize, true); - if (pduUserData == nullptr) { - LogError(LOG_P25, P25_KMM_STR ", unable to encrypt KMM response, algoId = $%02X, kID = $%04X", algoId, kid); - if (req->buffer != nullptr) - delete[] req->buffer; - delete req; - return; + // lambda function to send KMM responses, handling encryption if necessary + auto sendResponse = [&](const uint8_t* response, uint32_t responseLength) -> bool { + uint8_t responseMI[MI_LENGTH_BYTES] = { 0U }; + const uint8_t* outgoing = response; + UInt8Array encryptedResponse; + if (responseEncrypted) { + encryptedResponse = network->cryptKMM(algoId, kid, responseMI, response, responseLength, true); + if (encryptedResponse == nullptr) + return false; + outgoing = encryptedResponse.get(); } - } - UInt8Array datagram = std::make_unique(payloadSize + 14U); - ::memset(datagram.get(), 0x00U, payloadSize + 14U); + UInt8Array datagram = std::make_unique(responseLength + 14U); + ::memset(datagram.get(), 0x00U, responseLength + 14U); + + datagram[0U] = 0U; + datagram[1U] = MFG_STANDARD; + datagram[2U] = responseEncrypted ? algoId : ALGO_UNENCRYPT; + SET_UINT16(responseEncrypted ? kid : 0U, datagram.get(), 3U); + + if (responseEncrypted) + ::memcpy(datagram.get() + 5U, responseMI, MI_LENGTH_BYTES); - datagram[0U] = 0U; // Version-0 preamble format - datagram[1U] = MFG_STANDARD; - datagram[2U] = responseEncrypted ? algoId : ALGO_UNENCRYPT; - SET_UINT16(responseEncrypted ? kid : 0U, datagram.get(), 3U); + ::memcpy(datagram.get() + 14U, outgoing, responseLength); - if (responseEncrypted) - ::memcpy(datagram.get() + 5U, mi, MI_LENGTH_BYTES); - ::memcpy(datagram.get() + 14U, pduUserData.get(), payloadSize); + return network->m_frameQueue->write(datagram.get(), responseLength + 14U, + req->address, req->addrLen); + }; - network->m_frameQueue->write(datagram.get(), payloadSize + 14U, req->address, req->addrLen); + // send the initial DLI KMM response + if (!sendResponse(pduUserData.get(), payloadSize)) { + LogError(LOG_P25, P25_KMM_STR ", unable to send DLI KMM response"); + } + else { + for (const std::vector& response : additionalResponses) { + if (!sendResponse(response.data(), (uint32_t)response.size())) { + LogError(LOG_P25, P25_KMM_STR ", unable to send additional DLI Rekey Command"); + break; + } + } + } } if (req->buffer != nullptr) @@ -522,7 +576,7 @@ UInt8Array P25OTARService::cryptKMM(uint8_t algoId, uint16_t kid, uint8_t* mi, c /* Helper used to process KMM frames. */ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_t llId, bool encrypted, uint32_t* payloadSize, - uint8_t algoId, uint16_t kid, const uint8_t* mi, bool dataLinkIndependent) + uint8_t algoId, uint16_t kid, const uint8_t* mi, bool dataLinkIndependent, std::vector>* additionalResponses) { if (payloadSize != nullptr) *payloadSize = 0U; @@ -640,7 +694,7 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ return nullptr; } - // bryanb: yay CPP lambdas! + // lambda function to create a negative acknowledgment (NACK) for the KMM frame auto makeNack = [&](uint8_t status) -> UInt8Array { // table 55 requires a NACK to be encrypted and authenticated - prefer // the request MAC TEK; when it is unavailable, the outer AES TEK is the @@ -664,12 +718,13 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ status, payloadSize, auth); }; - if (frame->getMACType() == KMM_MAC::NO_MAC && - (frame->getHasMessageNumber() || !allowsUnauthenticatedKMM(frame->getMessageId()))) { + // check if a MAC is required and present for the KMM frame + if (frame->getMACType() == KMM_MAC::NO_MAC && (frame->getHasMessageNumber() || !allowsUnauthenticatedKMM(frame->getMessageId()))) { LogWarning(LOG_P25, P25_KMM_STR ", MAC required for message type/MN, RSI = %u", frame->getSrcLLId()); return makeNack(KMM_Status::INVALID_MSG_NUMBER); } + // verify the MAC if it is present if (frame->getMACType() != KMM_MAC::NO_MAC) { if (frame->getMACType() != KMM_MAC::ENH_MAC || frame->getMACAlgId() != ALGO_AES_256 || (frame->getMACFormat() != KMM_MAC_FORMAT_CBC && frame->getMACFormat() != KMM_MAC_FORMAT_CMAC)) { @@ -695,6 +750,7 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ } } + // validate the inbound message number (MN) and maintain a fingerprint for replay protection if (frame->getHasMessageNumber()) { const uint32_t rsi = frame->getSrcLLId(); const uint16_t received = frame->getMessageNumber(); @@ -733,6 +789,7 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ if (m_rsiMessageNumber.find(llId) == m_rsiMessageNumber.end()) m_rsiMessageNumber[llId] = 0U; + // handle the KMM message based on its type switch (frame->getMessageId()) { case KMM_MessageType::HELLO: { @@ -793,7 +850,7 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ LogInfoEx(LOG_P25, P25_KMM_STR ", %s, no UKEK found for rekey request, llId = %u", kmm->toString().c_str(), llId); return write_KMM_NoService(llId, kmm->getSrcLLId(), payloadSize, auth); } else { - return write_KMM_Rekey_Command(llId, kmm->getSrcLLId(), kmm->getFlag(), payloadSize, auth); + return write_KMM_Rekey_Command(llId, kmm->getSrcLLId(), kmm->getFlag(), payloadSize, auth, additionalResponses); } } else { LogInfoEx(LOG_P25, P25_KMM_STR ", %s, rekey request denied, llId = %u", kmm->toString().c_str(), llId); @@ -905,7 +962,7 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ /* Helper used to return a Rekey-Command KMM to the calling SU. */ UInt8Array P25OTARService::write_KMM_Rekey_Command(uint32_t llId, uint32_t kmmRSI, uint8_t flags, uint32_t* payloadSize, - const KMMAuthContext& auth) + const KMMAuthContext& auth, std::vector>* additionalResponses) { uint8_t mi[MI_LENGTH_BYTES]; ::memset(mi, 0x00U, MI_LENGTH_BYTES); @@ -932,6 +989,7 @@ UInt8Array P25OTARService::write_KMM_Rekey_Command(uint32_t llId, uint32_t kmmRS uint8_t kekAlgId = P25DEF::ALGO_UNENCRYPT; uint16_t kekKId = 0U; + // Attempt to find the UKEK (User Key Encryption Key) for the given RSI (Rekey Sequence Identifier) ::EKCKeyItem keyItem = m_network->m_cryptoLookup->findUKEK(kmmRSI); if (!keyItem.isInvalid()) { uint8_t keyLength = keyItem.getKey(kekKey); @@ -996,6 +1054,7 @@ UInt8Array P25OTARService::write_KMM_Rekey_Command(uint32_t llId, uint32_t kmmRS } } + // ensure that a valid MAC TEK (Traffic Encryption Key) has been selected for the KMM response if (macTekItem.isInvalid()) { LogError(LOG_P25, P25_KMM_STR ", %s, no common AES TEK available for MAC, RSI = %u", outKmm.toString().c_str(), kmmRSI); @@ -1016,6 +1075,7 @@ UInt8Array P25OTARService::write_KMM_Rekey_Command(uint32_t llId, uint32_t kmmRS else ks.keyLength(P25DEF::MAX_ENC_KEY_LENGTH_BYTES); + // iterate through all available AES-256 keys and prepare them for inclusion in the KMM response for (EKCKeyItem keyItem : m_network->m_cryptoLookup->keys()) { if (keyItem.algId() != ALGO_AES_256) { LogWarning(LOG_P25, P25_KMM_STR", %s, ignoring kId = %u, is not an AES-256 key, llId = %u, RSI = %u", outKmm.toString().c_str(), @@ -1061,32 +1121,82 @@ UInt8Array P25OTARService::write_KMM_Rekey_Command(uint32_t llId, uint32_t kmmRS return nullptr; } - std::vector keysets; - keysets.push_back(ks); + // AACA-D permits a multi-KMM response and defines the D bit as + // "more to follow". Emit independently authenticated RK3 Rekey Commands, + // each with its own monotonically increasing MN and no more than four + // wrapped keys. Each command is answered by its own Rekey-Acknowledgment. + static constexpr size_t MAX_KEYS_PER_REKEY = 4U; + static constexpr uint32_t MAX_NATIVE_ENCRYPTED_KMM_LENGTH = (19U * P25_PDU_CONFIRMED_DATA_LENGTH_BYTES) - 13U - 4U; + const std::vector keys = ks.keys(); + const size_t batchCount = (keys.size() + MAX_KEYS_PER_REKEY - 1U) / MAX_KEYS_PER_REKEY; + UInt8Array firstFrame; + + // prepare to batch keys into multiple RK3 Rekey Commands if necessary + if (additionalResponses != nullptr) + additionalResponses->clear(); + + // iterate over each batch of keys and construct the corresponding RK3 Rekey Command + for (size_t batchIndex = 0U; batchIndex < batchCount; batchIndex++) { + KeysetItem batchKeyset; + batchKeyset.keysetId(ks.keysetId()); + batchKeyset.algId(ks.algId()); + batchKeyset.keyLength(ks.keyLength()); + + const size_t begin = batchIndex * MAX_KEYS_PER_REKEY; + const size_t end = std::min(begin + MAX_KEYS_PER_REKEY, keys.size()); + for (size_t i = begin; i < end; i++) + batchKeyset.push_back(keys[i]); + + // KMMRekeyCommand owns its MI buffer and is intentionally built per + // packet here -- do not copy outKmm: the KMM classes are not + // safely copyable, and every RK3 is an independent authenticated KMM + KMMRekeyCommand batch; + batch.setDecryptInfoFmt(KMM_DECRYPT_INSTRUCT_NONE); + batch.setSrcLLId(WUID_FNE); + batch.setDstLLId(kmmRSI); + batch.setMACType(KMM_MAC::ENH_MAC); + batch.setMACFormat(auth.authenticated ? auth.format : KMM_MAC_FORMAT_CBC); + batch.setHasMessageNumber(true); + batch.setAlgId(kekAlgId); + batch.setKId(kekKId); + batch.setMACAlgId(macTekItem.algId()); + batch.setMACKId((uint16_t)macTekItem.kId()); + batch.setKeysets(std::vector{batchKeyset}); + batch.setMessageNumber((uint16_t)(mn + batchIndex)); + batch.setComplete((batchIndex + 1U) == batchCount); + + // check if the frame length exceeds the maximum allowed native encrypted KMM length + const uint32_t frameLength = batch.fullLength(); + if (frameLength > MAX_NATIVE_ENCRYPTED_KMM_LENGTH) { + LogError(LOG_P25, P25_KMM_STR ", %s, four-key rekey batch exceeds native PDU transport, len/max = %u/%u, llId = %u, RSI = %u", + batch.toString().c_str(), frameLength, MAX_NATIVE_ENCRYPTED_KMM_LENGTH, + batch.getSrcLLId(), batch.getDstLLId()); + return write_KMM_NoService(llId, kmmRSI, payloadSize, auth); + } - outKmm.setKeysets(keysets); + std::vector encoded(frameLength, 0x00U); + batch.encode(encoded.data()); + batch.generateMAC(macTek, encoded.data()); - if (outKmm.fullLength() > 512U) { - LogError(LOG_P25, P25_KMM_STR ", %s, rekey exceeds AACA-D DLD maximum, len = %u, llId = %u, RSI = %u", - outKmm.toString().c_str(), outKmm.fullLength(), outKmm.getSrcLLId(), outKmm.getDstLLId()); - return nullptr; - } + if (m_verbose) { + LogInfoEx(LOG_P25, P25_KMM_STR ", %s, llId = %u, RSI = %u, batch = %u/%u, keyCount = %u, moreToFollow = %u", + batch.toString().c_str(), batch.getSrcLLId(), batch.getDstLLId(), uint32_t(batchIndex + 1U), + uint32_t(batchCount), uint32_t(end - begin), batch.getComplete() ? 0U : 1U); + } - if (m_verbose) { - LogInfoEx(LOG_P25, P25_KMM_STR ", %s, llId = %u, RSI = %u, keyCount = %u", outKmm.toString().c_str(), - outKmm.getSrcLLId(), outKmm.getDstLLId(), ks.keys().size()); + if (batchIndex == 0U) { + if (payloadSize != nullptr) + *payloadSize = frameLength; + firstFrame = std::make_unique(frameLength); + ::memcpy(firstFrame.get(), encoded.data(), frameLength); + } + else if (additionalResponses != nullptr) { + additionalResponses->push_back(std::move(encoded)); + } } - const uint32_t frameLength = outKmm.fullLength(); - if (payloadSize != nullptr) - *payloadSize = frameLength; - - UInt8Array kmmFrame = std::make_unique(frameLength); - outKmm.encode(kmmFrame.get()); - outKmm.generateMAC(macTek, kmmFrame.get()); - - m_rsiMessageNumber[llId] = (uint16_t)(mn + 1U); - return kmmFrame; + m_rsiMessageNumber[llId] = (uint16_t)(mn + batchCount); + return firstFrame; } /* Helper used to return a Registration-Command KMM to the calling SU. */ diff --git a/src/fne/network/P25OTARService.h b/src/fne/network/P25OTARService.h index 338d9fb3..32ce3891 100644 --- a/src/fne/network/P25OTARService.h +++ b/src/fne/network/P25OTARService.h @@ -161,11 +161,16 @@ namespace network * @param encrypted Flag indicating whether or not the KMM frame is encrypted. * @param llId Logical Link ID. * @param[out] payloadSize Size of the returned KMM payload. + * @param algoId Algorithm ID. + * @param kid Key ID. + * @param mi Message Indicator. + * @param dataLinkIndependent Flag indicating whether the KMM frame is data-link independent. + * @param additionalResponses Optional container for additional KMM responses. * @returns UInt8Array Buffer containing the processed KMM frame (if any). */ UInt8Array processKMM(const uint8_t* data, uint32_t len, uint32_t llId, bool encrypted, uint32_t* payloadSize, uint8_t algoId = P25DEF::ALGO_UNENCRYPT, uint16_t kid = 0U, const uint8_t* mi = nullptr, - bool dataLinkIndependent = false); + bool dataLinkIndependent = false, std::vector>* additionalResponses = nullptr); /** * @brief Helper used to return a Rekey-Command KMM to the calling SU. @@ -174,10 +179,11 @@ namespace network * @param flags Hello KMM flags. * @param[out] payloadSize Size of the returned KMM payload. * @param auth Authentication context containing MAC/MN fields to mirror. + * @param additionalResponses Optional container for additional KMM responses. * @returns UInt8Array Buffer containing the processed KMM frame (if any). */ UInt8Array write_KMM_Rekey_Command(uint32_t llId, uint32_t kmmRSI, uint8_t flags, uint32_t* payloadSize, - const KMMAuthContext& auth); + const KMMAuthContext& auth, std::vector>* additionalResponses = nullptr); /** * @brief Helper used to return a Registration-Command KMM to the calling SU. diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.cpp b/src/fne/network/callhandler/packetdata/P25PacketData.cpp index 91f2d96b..0ff8f842 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.cpp +++ b/src/fne/network/callhandler/packetdata/P25PacketData.cpp @@ -83,7 +83,7 @@ P25PacketData::P25PacketData(TrafficNetwork* network, TagP25Data* tag, bool debu m_assembler = new data::Assembler(); m_assembler->setBlockWriter([](const void* userContext, const uint8_t currentBlock, const uint8_t *data, uint32_t len, bool lastBlock) { - const UserContext* context = static_cast(userContext); + UserContext* context = const_cast(static_cast(userContext)); if (context == nullptr) { return; } @@ -93,8 +93,9 @@ P25PacketData::P25PacketData(TrafficNetwork* network, TagP25Data* tag, bool debu return; } - packetData->writeNetwork(context->peerId, context->srcPeerId, context->peerNet, *(context->header), - currentBlock, data, len, context->pktSeq, context->streamId); + if (!packetData->writeNetwork(context->peerId, context->srcPeerId, context->peerNet, *(context->header), + currentBlock, data, len, context->pktSeq, context->streamId)) + context->success = false; }); } @@ -110,6 +111,12 @@ P25PacketData::~P25PacketData() bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t peerId, uint16_t pktSeq, uint32_t streamId, bool fromUpstream) { + // validate the incoming network data frame + if (data == nullptr || len < 24U) { + LogError(LOG_P25, P25_PDU_STR ", truncated network PDU header, len = %u", len); + return false; + } + hrc::hrc_t pktTime = hrc::now(); uint8_t totalBlocks = data[20U] + 1U; @@ -170,20 +177,40 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee status->lastPacket = hrc::now(); m_status.unlock(); - // make sure we don't get a PDU with more blocks then we support - if (currentBlock >= P25_MAX_PDU_BLOCKS || status->totalBlocks > P25_MAX_PDU_BLOCKS) { + // network PDU messages always carry one 25-byte encoded CAI block after + // their 24-byte metadata header -- reject inconsistent framing before copy + if (blockLength != P25_PDU_FEC_LENGTH_BYTES || len < (24U + blockLength) || + totalBlocks == 0U || currentBlock >= totalBlocks || + currentBlock >= P25_MAX_PDU_BLOCKS || status->totalBlocks > P25_MAX_PDU_BLOCKS) { LogError(LOG_P25, P25_PDU_STR ", too many PDU blocks to process, %u > %u", currentBlock, P25_MAX_PDU_BLOCKS); return false; } + // check for inconsistent total block count + if (status->dataBlockCnt > 0U && status->totalBlocks != totalBlocks) { + LogError(LOG_P25, P25_PDU_STR ", inconsistent total block count, received/expected = %u/%u", + totalBlocks, status->totalBlocks); + return false; + } + LogInfoEx(LOG_NET, P25_PDU_STR ", received block %u, peerId = %u, len = %u", currentBlock, peerId, blockLength); // store the received block + auto existing = status->receivedBlocks.find(currentBlock); + if (existing != status->receivedBlocks.end()) { + if (::memcmp(existing->second, data + 24U, blockLength) != 0) { + LogError(LOG_P25, P25_PDU_STR ", conflicting duplicate block %u", currentBlock); + return false; + } + LogInfoEx(LOG_P25, P25_PDU_STR ", ignoring identical duplicate block %u", currentBlock); + return true; + } + uint8_t* blockData = new uint8_t[blockLength]; ::memcpy(blockData, data + 24U, blockLength); - status->receivedBlocks[currentBlock] = blockData; - status->dataBlockCnt++; + status->receivedBlocks.emplace(currentBlock, blockData); + status->dataBlockCnt = (uint16_t)status->receivedBlocks.size(); totalBlocks = status->totalBlocks; if (status->dataBlockCnt == totalBlocks) { @@ -413,7 +440,7 @@ void P25PacketData::write_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, ui /* Helper used to return a KMM to the calling SU. */ -void P25PacketData::write_PDU_KMM(const uint8_t* data, uint32_t len, uint32_t llId, bool encrypted, uint8_t algId, uint16_t kId, const uint8_t* mi) +bool P25PacketData::write_PDU_KMM(const uint8_t* data, uint32_t len, uint32_t llId, bool encrypted, uint8_t algId, uint16_t kId, const uint8_t* mi) { // assemble a P25 PDU frame header for transport... data::DataHeader dataHeader = data::DataHeader(); @@ -429,7 +456,7 @@ void P25PacketData::write_PDU_KMM(const uint8_t* data, uint32_t len, uint32_t ll if (encrypted) { if (mi == nullptr) { LogError(LOG_P25, P25_PDU_STR ", missing MI for encrypted KMM, llId = %u", llId); - return; + return false; } dataHeader.setEXSAP(PDUSAP::UNENC_KMM); @@ -445,7 +472,7 @@ void P25PacketData::write_PDU_KMM(const uint8_t* data, uint32_t len, uint32_t ll DECLARE_UINT8_ARRAY(pduUserData, pduLength); ::memcpy(pduUserData, data, len); - dispatchUserFrameToFNE(dataHeader, false, auxiliaryES, pduUserData); + return dispatchUserFrameToFNE(dataHeader, false, auxiliaryES, pduUserData); } /* Updates the timer by the passed number of milliseconds. */ @@ -511,9 +538,25 @@ void P25PacketData::clock(uint32_t ms) LogInfoEx(LOG_P25, "VTUN -> PDU IP Data (queued), dstIp = %s (%u), userDataLen = %u, proto = %02X%s, retries = %u", targetIP.c_str(), packet->llId, uint32_t(packet->userData.size()), protocol, (protocol == 0x01U) ? " (ICMP)" : "", packet->retryCount); - m_dataLinkManager.setReady(packet->llId, false); - dispatchUserFrameToFNE(packet->header, false, false, packet->userData.data()); - m_packetScheduler.pop(); + + const bool confirmed = packet->header.getFormat() == PDUFormatType::CONFIRMED; + if (confirmed) + m_dataLinkManager.setReady(packet->llId, false); + + if (dispatchUserFrameToFNE(packet->header, false, false, packet->userData.data())) { + m_packetScheduler.pop(); + } + else { + if (confirmed) + m_dataLinkManager.setReady(packet->llId, true); + + packet->dueAt = now + SUBSCRIBER_READY_RETRY_MS; + packet->retryCount++; + m_packetScheduler.rotate(); + + LogWarning(LOG_P25, P25_PDU_STR ", downlink dispatch rejected, dstIp = %s (%u), retry = %u", + targetIP.c_str(), packet->llId, packet->retryCount); + } #endif // !defined(_WIN32) } @@ -569,8 +612,9 @@ void P25PacketData::dispatch(uint32_t peerId) peerId, status->assembler.dataHeader.getFormat(), status->assembler.dataHeader.getResponseClass(), status->assembler.dataHeader.getResponseType(), status->assembler.dataHeader.getResponseStatus(), status->assembler.dataHeader.getLLId(), status->assembler.dataHeader.getSrcLLId()); - // bryanb: this is naive and possibly error prone - m_dataLinkManager.setReady(status->assembler.dataHeader.getSrcLLId(), true); + const uint32_t responseLlId = status->assembler.getExtendedAddress() ? status->assembler.dataHeader.getSrcLLId() : status->assembler.dataHeader.getLLId(); + if (responseLlId != 0U) + m_dataLinkManager.setReady(responseLlId, true); if (status->assembler.dataHeader.getResponseClass() == PDUAckClass::ACK && status->assembler.dataHeader.getResponseType() == PDUAckType::ACK) { LogInfoEx(LOG_P25, P25_PDU_STR ", ISP, response, OSP ACK, peer = %u, llId = %u, all blocks received OK, n = %u", @@ -605,8 +649,13 @@ void P25PacketData::dispatch(uint32_t peerId) return; } + // handle unconfirmed PDU format and set the data link manager ready state if necessary if (status->assembler.dataHeader.getFormat() == PDUFormatType::UNCONFIRMED) { - m_dataLinkManager.setReady(status->assembler.dataHeader.getSrcLLId(), true); + const uint32_t responseLlId = status->assembler.getExtendedAddress() ? + status->assembler.dataHeader.getSrcLLId() : status->assembler.dataHeader.getLLId(); + + if (responseLlId != 0U) + m_dataLinkManager.setReady(responseLlId, true); } // do not route or inject incomplete/invalid confirmed payloads -- the empty @@ -629,9 +678,9 @@ void P25PacketData::dispatch(uint32_t peerId) if (status->assembler.getAuxiliaryES()) sap = status->assembler.dataHeader.getEXSAP(); - // The peer is the only channel identity carried at this boundary. Preserve - // unknown channel fields as zero until peer metadata supplies them. A - // registration establishes location only after the binding is accepted. + // the peer is the only channel identity carried at this boundary -- preserve + // unknown channel fields as zero until peer metadata supplies them, a + // registration establishes location only after the binding is accepted if (sap != PDUSAP::CONV_DATA_REG && m_locationRegistry.learnFromInbound()) { uint32_t inboundLlId = status->assembler.getExtendedAddress() ? status->assembler.dataHeader.getSrcLLId() : status->assembler.dataHeader.getLLId(); @@ -952,7 +1001,7 @@ void P25PacketData::dispatchToFNE(uint32_t peerId) /* Helper to dispatch PDU user data back to the local FNE network. (Will not transmit to neighbor FNE peers.) */ -void P25PacketData::dispatchUserFrameToFNE(DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData) +bool P25PacketData::dispatchUserFrameToFNE(DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData) { uint32_t srcId = (extendedAddress) ? dataHeader.getSrcLLId() : dataHeader.getLLId(); uint32_t dstId = dataHeader.getLLId(); @@ -972,9 +1021,10 @@ void P25PacketData::dispatchUserFrameToFNE(DataHeader& dataHeader, bool extended routes.push_back(route); } + // if no routes are available, log a warning and return false if (routes.empty()) { LogWarning(LOG_P25, P25_PDU_STR ", no RF route for downlink, llId = %u", dstId); - return; + return false; } // update V(S) only after a usable RF route has been selected @@ -985,17 +1035,18 @@ void P25PacketData::dispatchUserFrameToFNE(DataHeader& dataHeader, bool extended dataHeader.setNs(sendSequence); // dispatch the user frame to each resolved RF route + bool dispatched = false; for (const DataRoute& route : routes) { auto localPeer = m_network->m_peers.find(route.peerId); if (localPeer != m_network->m_peers.end()) { - write_PDU_User(route.peerId, m_network->m_peerId, nullptr, dataHeader, - extendedAddress, auxiliaryES, pduUserData); + dispatched = write_PDU_User(route.peerId, m_network->m_peerId, nullptr, dataHeader, + extendedAddress, auxiliaryES, pduUserData) || dispatched; } else { for (const auto& peer : m_network->m_host->m_peerNetworks) { if (peer.second->getPeerId() == route.peerId && peer.second->isEnabled()) { - write_PDU_User(route.peerId, m_network->m_peerId, peer.second, - dataHeader, extendedAddress, auxiliaryES, pduUserData); + dispatched = write_PDU_User(route.peerId, m_network->m_peerId, peer.second, + dataHeader, extendedAddress, auxiliaryES, pduUserData) || dispatched; break; } } @@ -1006,6 +1057,7 @@ void P25PacketData::dispatchUserFrameToFNE(DataHeader& dataHeader, bool extended route.peerId, DUID::PDU, srcId, dstId); } } + return dispatched; } /* Helper used to process conventional data registration from PDU data. */ @@ -1422,7 +1474,7 @@ void P25PacketData::write_PDU_ARP_Reply(uint32_t targetAddr, uint32_t requestorL /* Helper to write user data as a P25 PDU packet. */ -void P25PacketData::write_PDU_User(uint32_t peerId, uint32_t srcPeerId, network::PeerNetwork* peerNet, data::DataHeader& dataHeader, +bool P25PacketData::write_PDU_User(uint32_t peerId, uint32_t srcPeerId, network::PeerNetwork* peerNet, data::DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData) { uint32_t streamId = m_network->createStreamId(); @@ -1439,9 +1491,13 @@ void P25PacketData::write_PDU_User(uint32_t peerId, uint32_t srcPeerId, network: context->header = new data::DataHeader(dataHeader); context->pktSeq = pktSeq; context->streamId = streamId; + context->success = true; m_assembler->assemble(dataHeader, extendedAddress, auxiliaryES, pduUserData, nullptr, context); + const bool success = context->success; + delete context->header; delete context; + return success; } /* Write data processed to the network. */ diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.h b/src/fne/network/callhandler/packetdata/P25PacketData.h index ffd3f115..fc56e3b7 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.h +++ b/src/fne/network/callhandler/packetdata/P25PacketData.h @@ -102,7 +102,7 @@ namespace network * @param kId Encryption Key ID. * @param mi 9-byte Encryption Message Indicator. */ - void write_PDU_KMM(const uint8_t* data, uint32_t len, uint32_t llId, bool encrypted, + bool write_PDU_KMM(const uint8_t* data, uint32_t len, uint32_t llId, bool encrypted, uint8_t algId = P25DEF::ALGO_UNENCRYPT, uint16_t kId = 0U, const uint8_t* mi = nullptr); /** @@ -127,13 +127,14 @@ namespace network * @ingroup fne_network */ struct UserContext { - void* obj; //!< Instance of the P25PacketData class. - uint32_t peerId; //!< Peer ID for this request. - uint32_t srcPeerId; //!< Source Peer ID for this request. - network::PeerNetwork* peerNet; //!< Instance of the peer network for an upstream peer. - ::p25::data::DataHeader* header; //!< PDU data header. - uint16_t pktSeq; //!< Packet sequence. - uint32_t streamId; //!< Stream ID. + void* obj; //!< Instance of the P25PacketData class. + uint32_t peerId; //!< Peer ID for this request. + uint32_t srcPeerId; //!< Source Peer ID for this request. + network::PeerNetwork* peerNet; //!< Instance of the peer network for an upstream peer. + ::p25::data::DataHeader* header; //!< PDU data header. + uint16_t pktSeq; //!< Packet sequence. + uint32_t streamId; //!< Stream ID. + bool success; //!< Flag indicating every network block was accepted. }; P25PacketScheduler m_packetScheduler; @@ -150,7 +151,7 @@ namespace network uint32_t peerId; //!< Peer ID std::unordered_map receivedBlocks; - ::p25::data::Assembler assembler; //!< PDU Assembler Instance + ::p25::data::Assembler assembler; //!< PDU Assembler Instance bool hasRxHeader; //!< Flag indicating whether or not a valid Rx header has been received uint16_t dataBlockCnt; //!< Number of data blocks received uint16_t totalBlocks; //!< Total number of blocks expected @@ -237,7 +238,7 @@ namespace network * @param auxiliaryES Flag indicating whether or not an auxiliary ES is included. * @param pduUserData Buffer containing user data to transmit. */ - void dispatchUserFrameToFNE(::p25::data::DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData); + bool dispatchUserFrameToFNE(::p25::data::DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData); /** * @brief Helper used to process conventional data registration from PDU data. @@ -276,7 +277,7 @@ namespace network * @param auxiliaryES Flag indicating whether or not an auxiliary ES is included. * @param pduUserData Buffer containing user data to transmit. */ - void write_PDU_User(uint32_t peerId, uint32_t srcPeerId, network::PeerNetwork* peerNet, ::p25::data::DataHeader& dataHeader, + bool write_PDU_User(uint32_t peerId, uint32_t srcPeerId, network::PeerNetwork* peerNet, ::p25::data::DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData); /** diff --git a/src/host/p25/Control.cpp b/src/host/p25/Control.cpp index f3de3393..28b51ecf 100644 --- a/src/host/p25/Control.cpp +++ b/src/host/p25/Control.cpp @@ -1380,7 +1380,7 @@ uint32_t Control::getLastSrcId() const /* Add data frame to the data ring buffer. */ -void Control::addFrame(const uint8_t* data, uint32_t length, bool net, bool imm) +bool Control::addFrame(const uint8_t* data, uint32_t length, bool net, bool imm) { assert(data != nullptr); @@ -1388,10 +1388,10 @@ void Control::addFrame(const uint8_t* data, uint32_t length, bool net, bool imm) if (!net) { if (m_rfTimeoutTimer.isRunning() && m_rfTimeoutTimer.hasExpired()) - return; + return false; } else { if (m_netTimeoutTimer.isRunning() && m_netTimeoutTimer.hasExpired()) - return; + return false; } if (m_debug) { @@ -1406,16 +1406,16 @@ void Control::addFrame(const uint8_t* data, uint32_t length, bool net, bool imm) if (imm) { // resize immediate queue if necessary (this shouldn't really ever happen) uint32_t space = m_txImmQueue.freeSpace(); - if (space < (length + 1U)) { + if (space < (length + 2U)) { if (!net) { uint32_t queueLen = m_txImmQueue.length(); m_txImmQueue.resize(queueLen + P25_LDU_FRAME_LENGTH_BYTES); LogError(LOG_P25, "overflow in the P25 queue while writing imm data; queue free is %u, needed %u; resized was %u is %u, fifoSpace = %u", space, length, queueLen, m_txImmQueue.length(), fifoSpace); - return; + return false; } else { LogError(LOG_P25, "overflow in the P25 queue while writing imm network data; queue free is %u, needed %u, fifoSpace = %u", space, length, fifoSpace); - return; + return false; } } @@ -1425,24 +1425,22 @@ void Control::addFrame(const uint8_t* data, uint32_t length, bool net, bool imm) else lenBuffer[0U] = 0x00U; lenBuffer[1U] = length & 0xFFU; - m_txImmQueue.addData(lenBuffer, 2U); - m_txImmQueue.addData(data, length); - return; + return m_txImmQueue.addData(lenBuffer, 2U) && m_txImmQueue.addData(data, length); } // resize queue if necessary (this shouldn't really ever happen) uint32_t space = m_txQueue.freeSpace(); - if (space < (length + 1U)) { + if (space < (length + 2U)) { if (!net) { uint32_t queueLen = m_txQueue.length(); m_txQueue.resize(queueLen + P25_LDU_FRAME_LENGTH_BYTES); LogError(LOG_P25, "overflow in the P25 queue while writing data; queue free is %u, needed %u; resized was %u is %u, fifoSpace = %u", space, length, queueLen, m_txQueue.length(), fifoSpace); - return; + return false; } else { LogError(LOG_P25, "overflow in the P25 queue while writing network data; queue free is %u, needed %u, fifoSpace = %u", space, length, fifoSpace); - return; + return false; } } @@ -1452,9 +1450,8 @@ void Control::addFrame(const uint8_t* data, uint32_t length, bool net, bool imm) else lenBuffer[0U] = 0x00U; lenBuffer[1U] = length & 0xFFU; - m_txQueue.addData(lenBuffer, 2U); - m_txQueue.addData(data, length); + return m_txQueue.addData(lenBuffer, 2U) && m_txQueue.addData(data, length); } /* Process a data frames from the network. */ diff --git a/src/host/p25/Control.h b/src/host/p25/Control.h index 3f0fa07e..78de7c0d 100644 --- a/src/host/p25/Control.h +++ b/src/host/p25/Control.h @@ -434,7 +434,7 @@ namespace p25 * @param net Flag indicating whether the data came from the network or not * @param imm Flag indicating whether or not the data is priority and is added to the immediate queue. */ - void addFrame(const uint8_t* data, uint32_t length, bool net = false, bool imm = false); + bool addFrame(const uint8_t* data, uint32_t length, bool net = false, bool imm = false); /** * @brief Process a data frames from the network. diff --git a/src/host/p25/packet/Data.cpp b/src/host/p25/packet/Data.cpp index 83d8f745..1ad1f372 100644 --- a/src/host/p25/packet/Data.cpp +++ b/src/host/p25/packet/Data.cpp @@ -416,6 +416,16 @@ bool Data::process(uint8_t* data, uint32_t len) bool Data::processNetwork(uint8_t* data, uint32_t len, uint8_t currentBlock, uint32_t blockLength, uint16_t totalBlocks) { + // validate network block framing + bool rfAccepted = true; + if (data == nullptr || blockLength != P25_PDU_FEC_LENGTH_BYTES || + len < (24U + blockLength) || totalBlocks == 0U || + totalBlocks > P25_MAX_PDU_BLOCKS || currentBlock >= totalBlocks) { + LogError(LOG_NET, P25_PDU_STR ", invalid network block framing, block/length/total/input = %u/%u/%u/%u", + currentBlock, blockLength, totalBlocks, len); + return false; + } + if (m_p25->m_netState != RS_NET_DATA) { m_p25->m_netState = RS_NET_DATA; m_inbound = false; @@ -426,11 +436,29 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint8_t currentBlock, uin m_p25->m_networkWatchdog.start(); + // check for inconsistent total block count + if (m_netDataBlockCnt > 0U && m_netTotalBlocks != totalBlocks) { + LogError(LOG_NET, P25_PDU_STR ", inconsistent total block count, received/expected = %u/%u", + totalBlocks, m_netTotalBlocks); + return false; + } + + // check for duplicate received blocks + auto existing = m_netReceivedBlocks.find(currentBlock); + if (existing != m_netReceivedBlocks.end()) { + if (::memcmp(existing->second, data + 24U, blockLength) != 0) { + LogError(LOG_NET, P25_PDU_STR ", conflicting duplicate block %u", currentBlock); + return false; + } + LogInfoEx(LOG_NET, P25_PDU_STR ", ignoring identical duplicate block %u", currentBlock); + return true; + } + // store the received block uint8_t* blockData = new uint8_t[blockLength]; ::memcpy(blockData, data + 24U, blockLength); - m_netReceivedBlocks[currentBlock] = blockData; - m_netDataBlockCnt++; + m_netReceivedBlocks.emplace(currentBlock, blockData); + m_netDataBlockCnt = (uint16_t)m_netReceivedBlocks.size(); m_netTotalBlocks = totalBlocks; if (m_p25->m_netState == RS_NET_DATA) { @@ -558,7 +586,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint8_t currentBlock, uin } } - writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on + rfAccepted = writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on } break; default: @@ -569,7 +597,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint8_t currentBlock, uin LogInfoEx(LOG_NET, P25_PDU_STR ", transmitting network PDU, llId = %u", (m_netAssembler->getExtendedAddress()) ? m_netAssembler->dataHeader.getSrcLLId() : m_netAssembler->dataHeader.getLLId()); } - writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on + rfAccepted = writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on ::ActivityLog("P25", false, "end of Net data transmission"); break; @@ -588,7 +616,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint8_t currentBlock, uin } } - return true; + return rfAccepted; } /* Helper to reset received network blocks. */ @@ -609,7 +637,7 @@ void Data::resetReceivedBlocks() /* Helper to write user data as a P25 PDU packet. */ -void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData, bool imm) +bool Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData, bool imm) { assert(pduUserData != nullptr); @@ -619,7 +647,7 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, UInt8Array data = m_rfAssembler->assemble(dataHeader, extendedAddress, auxiliaryES, pduUserData, &bitLength); bool trackRetry = dataHeader.getFormat() == PDUFormatType::CONFIRMED && dataHeader.getAckNeeded(); - writeRF_PDU(data.get(), bitLength, imm, false, dataHeader.getLLId(), trackRetry); + return writeRF_PDU(data.get(), bitLength, imm, false, dataHeader.getLLId(), trackRetry); } /* Helper to write user data as a P25 PDU packet. */ @@ -940,7 +968,7 @@ void Data::writeNetwork(const uint8_t currentBlock, const uint8_t* data, uint32_ /* Helper to write a P25 PDU packet. */ -void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ackRetry, +bool Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ackRetry, uint32_t retryLlId, bool trackRetry) { assert(pdu != nullptr); @@ -960,7 +988,7 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ac 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; + return false; } m_p25->writeRF_TDU(true, imm); @@ -994,25 +1022,27 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ac //Utils::dump("P25, Data::writeRF_PDU(), Raw PDU OSP", data, newByteLength + 2U); + bool queued = false; if (m_p25->m_duplex) { data[0U] = modem::TAG_DATA; data[1U] = 0x00U; - m_p25->addFrame(data, newByteLength + 2U, false, imm); + queued = m_p25->addFrame(data, newByteLength + 2U, false, imm); } m_p25->writeRF_TDU(true, imm); + return queued; } /* Helper to write a network P25 PDU packet. */ -void Data::writeNet_PDU_Buffered() +bool Data::writeNet_PDU_Buffered() { uint32_t bitLength = 0U; UInt8Array data = m_rfAssembler->assemble(m_netAssembler->dataHeader, m_netAssembler->getExtendedAddress(), m_netAssembler->getAuxiliaryES(), m_netPduUserData, &bitLength); bool trackRetry = m_netAssembler->dataHeader.getFormat() == PDUFormatType::CONFIRMED && m_netAssembler->dataHeader.getAckNeeded(); - writeRF_PDU(data.get(), bitLength, false, false, m_netAssembler->dataHeader.getLLId(), trackRetry); + return writeRF_PDU(data.get(), bitLength, false, false, m_netAssembler->dataHeader.getLLId(), trackRetry); } /* Helper to re-write a received P25 PDU packet. */ diff --git a/src/host/p25/packet/Data.h b/src/host/p25/packet/Data.h index 2b4ad976..cbc45851 100644 --- a/src/host/p25/packet/Data.h +++ b/src/host/p25/packet/Data.h @@ -86,7 +86,7 @@ namespace p25 * @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, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData, bool imm = false); + bool writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData, bool imm = false); /** * @brief Helper to write user data as a P25 PDU packet. * @param dataHeader Instance of a PDU data header. @@ -219,14 +219,16 @@ namespace p25 * @param bitlength Length of PDU in bits. * @param imm Flag indicating the PDU should be written to the immediate queue. * @param ackRetry Flag indicating the PDU is being sent as an acknowledged retry. + * @param retryLlId Logical Link ID for retrying the PDU. + * @param trackRetry Flag indicating whether or not to track the retry. */ - void writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm = false, - bool ackRetry = false, uint32_t retryLlId = 0U, bool trackRetry = false); + bool writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm = false, bool ackRetry = false, + uint32_t retryLlId = 0U, bool trackRetry = false); /** * @brief Helper to write a network P25 PDU packet. * This will take buffered network PDU data and repeat it over the air. */ - void writeNet_PDU_Buffered(); + bool writeNet_PDU_Buffered(); /** * @brief Helper to re-write a received P25 PDU packet. * This will take buffered received PDU data and repeat it over the air. diff --git a/tests/fne/FNETestHooks.cpp b/tests/fne/FNETestHooks.cpp index 8106eb0c..6d486868 100644 --- a/tests/fne/FNETestHooks.cpp +++ b/tests/fne/FNETestHooks.cpp @@ -94,6 +94,37 @@ std::unique_ptr FNETestHooks::processOTARKMM(TrafficNetwork& network, false, &payloadSize, outerAlgoId, outerKId, nullptr, dataLinkIndependent); } +/** + * @brief Builds an OTAR rekey command for the specified logical link ID and KMM RSI. + * @param network The TrafficNetwork instance. + * @param llId The logical link ID. + * @param kmmRSI The KMM RSI value. + * @return A vector of vectors containing the OTAR rekey frames. + */ +std::vector> FNETestHooks::buildOTARRekey(TrafficNetwork& network, + uint32_t llId, uint32_t kmmRSI) +{ + std::vector> frames; + if (network.m_p25OTARService == nullptr) + return frames; + + P25OTARService::KMMAuthContext auth; + uint32_t firstLength = 0U; + std::vector> additional; + const bool previousAllowClear = network.m_p25OTARService->m_allowNoUKEKRekey; + network.m_p25OTARService->m_allowNoUKEKRekey = true; + UInt8Array first = network.m_p25OTARService->write_KMM_Rekey_Command(llId, kmmRSI, + KMM_HelloFlag::REKEY_REQUEST_NO_UKEK, &firstLength, auth, &additional); + network.m_p25OTARService->m_allowNoUKEKRekey = previousAllowClear; + if (first == nullptr || firstLength == 0U) + return frames; + + frames.emplace_back(first.get(), first.get() + firstLength); + for (std::vector& frame : additional) + frames.push_back(std::move(frame)); + return frames; +} + /** * @brief Passes a DLD KMM through the public P25 OTAR bearer entry point. * @param network The TrafficNetwork instance. diff --git a/tests/fne/FNETestHooks.h b/tests/fne/FNETestHooks.h index d87e39ea..7e6df3ce 100644 --- a/tests/fne/FNETestHooks.h +++ b/tests/fne/FNETestHooks.h @@ -98,6 +98,15 @@ public: const std::vector& packet, uint32_t llId, uint32_t& payloadSize, uint8_t outerAlgoId = P25DEF::ALGO_UNENCRYPT, uint16_t outerKId = 0U, bool dataLinkIndependent = false); + /** + * @brief Builds an OTAR rekey command for the specified logical link ID and KMM RSI. + * @param network The TrafficNetwork instance. + * @param llId The logical link ID. + * @param kmmRSI The KMM RSI value. + * @return A vector of vectors containing the OTAR rekey frames. + */ + static std::vector> buildOTARRekey(network::TrafficNetwork& network, + uint32_t llId, uint32_t kmmRSI); /** * @brief Passes a DLD KMM through the public P25 OTAR bearer entry point. * @param network The TrafficNetwork that owns the OTAR service. diff --git a/tests/p25/P25_KMM_Tests.cpp b/tests/p25/P25_KMM_Tests.cpp index d064e90a..9dcb954b 100644 --- a/tests/p25/P25_KMM_Tests.cpp +++ b/tests/p25/P25_KMM_Tests.cpp @@ -17,6 +17,7 @@ #include "common/p25/kmm/KMMNoService.h" #include "common/p25/kmm/KMMNegativeAck.h" #include "common/p25/kmm/KMMRekeyAck.h" +#include "common/p25/kmm/KMMRekeyCommand.h" #include "fne/FNETestHooks.h" #include "fne/HostFNE.h" #include "common/lookups/RadioIdLookup.h" @@ -523,6 +524,57 @@ TEST_CASE("P25 OTAR DLI validates its Version-0 preamble and dispatches KMM", "[ } } +TEST_CASE("P25 OTAR Rekey Commands batch four keys per KMM", "[p25][kmm][otar][rekey]") +{ + KMMFNEHarness harness; + constexpr uint32_t SU_RSI = 0x654321U; + std::vector allowed; + + for (uint16_t i = 0U; i < 9U; i++) { + EKCKeyItem key; + key.id(i + 1U); + key.algId(ALGO_AES_256); + key.kId((uint16_t)(0x2000U + i)); + key.sln((uint16_t)(0x0100U + i)); + key.keyMaterial("000102030405060708090A0B0C0D0E0F101112131415161718191A1B1C1D1E1F"); + FNETestHooks::addCryptoKey(harness.traffic, key); + allowed.push_back((uint16_t)key.kId()); + } + + EKCKeyItem ukek; + ukek.id(100U); + ukek.rsiId(SU_RSI); + ukek.algId(ALGO_AES_256); + ukek.kId(0x1001U); + ukek.sln(0x0002U); + ukek.keyMaterial("101112131415161718191A1B1C1D1E1F202122232425262728292A2B2C2D2E2F"); + FNETestHooks::addCryptoKey(harness.traffic, ukek); + harness.rid.addEntry(SU_RSI, true, "batch-test", "", true, true, allowed); + + std::vector> frames = + FNETestHooks::buildOTARRekey(harness.traffic, SU_RSI, SU_RSI); + REQUIRE(frames.size() == 3U); + + const uint32_t expectedCounts[] = { 4U, 4U, 1U }; + for (size_t i = 0U; i < frames.size(); i++) { + REQUIRE(frames[i].size() >= 3U); + const uint32_t declaredLength = (((uint32_t)frames[i][1U] << 8U) | frames[i][2U]) + 3U; + REQUIRE(declaredLength == frames[i].size()); + // The bounded air-facing factory deliberately rejects nested-count + // KMMs until it has a dedicated structural validator. This is a + // trusted, locally encoded RK3, so use the normal typed decoder here. + std::unique_ptr decoded = KMMFactory::create(frames[i].data()); + REQUIRE(decoded != nullptr); + KMMRekeyCommand* rekey = dynamic_cast(decoded.get()); + REQUIRE(rekey != nullptr); + REQUIRE(rekey->getKeysets().size() == 1U); + CHECK(rekey->getKeysets()[0U].keys().size() == expectedCounts[i]); + CHECK(rekey->getMessageNumber() == i); + CHECK(rekey->getComplete() == (i + 1U == frames.size())); + CHECK(frames[i].size() <= 287U); + } +} + #if !defined(_WIN32) TEST_CASE("P25 OTAR DLI traverses the real UDP endpoint", "[p25][kmm][otar][dli][integration]") { diff --git a/tests/p25/PDU_Confirmed_Large_Test.cpp b/tests/p25/PDU_Confirmed_Large_Test.cpp index a55fd741..8970f76b 100644 --- a/tests/p25/PDU_Confirmed_Large_Test.cpp +++ b/tests/p25/PDU_Confirmed_Large_Test.cpp @@ -20,6 +20,7 @@ using namespace p25::data; #include #include #include +#include TEST_CASE("P25 PDU Confirmed Large Test", "[p25][pdu_confirmed_large]") { bool failed = false; @@ -117,3 +118,45 @@ TEST_CASE("P25 PDU Confirmed Large Test", "[p25][pdu_confirmed_large]") { REQUIRE(failed==false); } + +TEST_CASE("P25 PDU assembler safely handles more than twenty encoded blocks", + "[p25][pdu][assembler][capacity]") +{ + std::vector source(500U); + for (size_t i = 0U; i < source.size(); i++) + source[i] = (uint8_t)i; + + DataHeader header; + header.setFormat(PDUFormatType::CONFIRMED); + header.setMFId(MFG_STANDARD); + header.setAckNeeded(true); + header.setOutbound(true); + header.setSAP(PDUSAP::USER_DATA); + header.setLLId(0x12345U); + header.setFullMessage(true); + header.calculateLength((uint32_t)source.size()); + REQUIRE(header.getBlocksToFollow() > 20U); + + Assembler encoder; + uint32_t bitLength = 0U; + UInt8Array encoded = encoder.assemble(header, false, false, source.data(), &bitLength); + REQUIRE(encoded != nullptr); + + Assembler decoder; + uint8_t block[P25_PDU_FEC_LENGTH_BYTES] = { 0U }; + uint32_t blockCount = 0U; + for (uint32_t offset = P25_PREAMBLE_LENGTH_BITS; offset < bitLength; + offset += P25_PDU_FEC_LENGTH_BITS, blockCount++) { + Utils::getBitRange(encoded.get(), block, offset, P25_PDU_FEC_LENGTH_BITS); + REQUIRE(decoder.disassemble(block, sizeof(block), blockCount == 0U)); + } + + REQUIRE(decoder.getComplete()); + REQUIRE(decoder.getUserDataLength() == source.size()); + std::vector decoded(source.size()); + decoder.getUserData(decoded.data()); + CHECK(decoded == source); + + Assembler invalidLength; + CHECK_FALSE(invalidLength.disassemble(block, sizeof(block) - 1U, true)); +}