From 90ca8f1a6a732203afa83d95f60aa350e6c951be Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Sun, 24 May 2026 21:22:08 -0400 Subject: [PATCH] still not prime-time ready for anyone to use -- but I am over here implementing some fixes because my testcases are blowing up; --- src/fne/network/P25OTARService.cpp | 135 ++++++++++++++---- src/fne/network/P25OTARService.h | 6 +- .../callhandler/packetdata/P25PacketData.cpp | 38 ++++- .../callhandler/packetdata/P25PacketData.h | 6 +- 4 files changed, 151 insertions(+), 34 deletions(-) diff --git a/src/fne/network/P25OTARService.cpp b/src/fne/network/P25OTARService.cpp index 3937f5af..71c91ed3 100644 --- a/src/fne/network/P25OTARService.cpp +++ b/src/fne/network/P25OTARService.cpp @@ -78,39 +78,66 @@ P25OTARService::~P25OTARService() /* Helper used to process KMM frames from PDU data. */ -bool P25OTARService::processDLD(const uint8_t* data, uint32_t len, uint32_t llId, uint8_t n, bool encrypted) +bool P25OTARService::processDLD(const uint8_t* data, uint32_t len, uint32_t llId, uint8_t n, bool encrypted, + uint8_t algoId, uint16_t kid, const uint8_t* mi) { + uint8_t resolvedAlgoId = algoId; + uint16_t resolvedKId = kid; + uint8_t resolvedMI[MI_LENGTH_BYTES]; + ::memset(resolvedMI, 0x00U, MI_LENGTH_BYTES); + m_packetData->write_PDU_Ack_Response(PDUAckClass::ACK, PDUAckType::ACK, n, llId, false); if (m_debug) Utils::dump(1U, "P25OTARService::processDLD(), KMM Network Message", data, len); + UInt8Array kmmPayload = std::make_unique(len); + ::memset(kmmPayload.get(), 0x00U, len); + ::memcpy(kmmPayload.get(), data, len); + + if (encrypted) { + // prefer metadata provided from decoded Auxiliary ES header + if (mi != nullptr) { + ::memcpy(resolvedMI, mi, MI_LENGTH_BYTES); + } + else { + // legacy fallback for payload-embedded KMM encryption parameters + if (len < 11U) { + LogError(LOG_P25, P25_KMM_STR ", encrypted KMM payload too short, len = %u", len); + return false; + } + + for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { + resolvedMI[i] = data[i]; + } + + resolvedAlgoId = data[9U]; + resolvedKId = GET_UINT16(data, 10U); + } + + kmmPayload = cryptKMM(resolvedAlgoId, resolvedKId, resolvedMI, data, len, false); + if (kmmPayload == nullptr) { + LogError(LOG_P25, P25_KMM_STR ", unable to decrypt KMM, algoId = $%02X, kID = $%04X", resolvedAlgoId, resolvedKId); + return false; + } + } + uint32_t payloadSize = 0U; - UInt8Array pduUserData = processKMM(data, len, llId, encrypted, &payloadSize); + UInt8Array pduUserData = processKMM(kmmPayload.get(), len, llId, false, &payloadSize); if (pduUserData == nullptr) return false; // handle DLD encrypted KMM frame if (encrypted) { - // read crypto parameters from KMM header - uint8_t mi[MI_LENGTH_BYTES]; - ::memset(mi, 0x00U, MI_LENGTH_BYTES); - for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { - mi[i] = data[i]; - } - - uint8_t algoId = data[9U]; - uint16_t kid = GET_UINT16(data, 10U); - // re-encrypt the KMM response - pduUserData = cryptKMM(algoId, kid, mi, pduUserData.get(), payloadSize, true); + 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", algoId, kid); + LogError(LOG_P25, P25_KMM_STR ", unable to encrypt KMM response, algoId = $%02X, kID = $%04X", resolvedAlgoId, resolvedKId); return false; } } - m_packetData->write_PDU_KMM(pduUserData.get(), payloadSize, llId, encrypted); + m_packetData->write_PDU_KMM(pduUserData.get(), payloadSize, llId, encrypted, resolvedAlgoId, resolvedKId, resolvedMI); return true; } @@ -234,6 +261,15 @@ void P25OTARService::taskNetworkRx(OTARPacketRequest* req) uint32_t payloadSize = 0U; UInt8Array pduUserData = network->processKMM(buffer.get(), req->length - 13U, 0U, false, &payloadSize); + if (pduUserData == nullptr || payloadSize == 0U) { + if (network->m_debug) + LogDebug(LOG_P25, P25_KMM_STR ", no KMM response generated for network request"); + + if (req->buffer != nullptr) + delete[] req->buffer; + delete req; + return; + } if (encrypted) { // re-encrypt the KMM response @@ -312,7 +348,8 @@ 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) +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) { if (payloadSize != nullptr) *payloadSize = 0U; @@ -323,20 +360,36 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ // handle DLD encrypted KMM frame if (encrypted) { - // read crypto parameters from KMM header - uint8_t mi[MI_LENGTH_BYTES]; - ::memset(mi, 0x00U, MI_LENGTH_BYTES); - for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { - mi[i] = data[i]; + uint8_t resolvedAlgoId = algoId; + uint16_t resolvedKId = kid; + + uint8_t resolvedMI[MI_LENGTH_BYTES]; + ::memset(resolvedMI, 0x00U, MI_LENGTH_BYTES); + + // prefer metadata provided from decoded Auxiliary ES header + if (mi != nullptr) { + ::memcpy(resolvedMI, mi, MI_LENGTH_BYTES); + buffer = cryptKMM(algoId, kid, resolvedMI, data, len, false); } + else { + // legacy fallback for payload-embedded KMM encryption parameters + if (len < 11U) { + LogError(LOG_P25, P25_KMM_STR ", encrypted KMM payload too short, len = %u", len); + return nullptr; + } - uint8_t algoId = data[9U]; - uint16_t kid = GET_UINT16(data, 10U); + for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { + resolvedMI[i] = data[i]; + } + + resolvedAlgoId = data[9U]; + resolvedKId = GET_UINT16(data, 10U); + + buffer = cryptKMM(resolvedAlgoId, resolvedKId, resolvedMI, data + 10U, len - 10U, false); + } - // decrypt frame before processing - buffer = cryptKMM(algoId, kid, mi, data + 10U, len - 10U, false); if (buffer == nullptr) { - LogError(LOG_P25, P25_KMM_STR ", unable to decrypt KMM, algoId = $%02X, kID = $%04X", algoId, kid); + LogError(LOG_P25, P25_KMM_STR ", unable to decrypt KMM, algoId = $%02X, kID = $%04X", resolvedAlgoId, resolvedKId); return nullptr; } @@ -369,6 +422,7 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ case KMM_MessageType::HELLO: { KMMHello* kmm = static_cast(frame.get()); + uint8_t respKind = kmm->getResponseKind(); if (m_verbose) { LogInfoEx(LOG_P25, P25_KMM_STR ", %s, llId = %u, flag = $%02X", kmm->toString().c_str(), llId, kmm->getFlag()); @@ -382,6 +436,20 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ } } + // ignore Response Kind 2 command requests initiated from a SU + if (respKind == KMM_ResponseKind::DELAYED) { + LogWarning(LOG_P25, P25_KMM_STR ", %s, discarding SU initiated Response Kind 2 command, llId = %u", kmm->toString().c_str(), llId); + return nullptr; + } + + // response Kind 1 requests no OTAR response message + if (respKind == KMM_ResponseKind::NONE) { + if (m_verbose) { + LogInfoEx(LOG_P25, P25_KMM_STR ", %s, Response Kind 1 request, no OTAR response sent, llId = %u", kmm->toString().c_str(), llId); + } + return nullptr; + } + // respond with No-Service if KMF services are disabled if (!m_network->m_kmfServicesEnabled) return write_KMM_NoService(llId, kmm->getSrcLLId(), payloadSize); @@ -432,9 +500,24 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ case KMM_MessageType::DEREG_CMD: { KMMDeregistrationCommand* kmm = static_cast(frame.get()); + uint8_t respKind = kmm->getResponseKind(); LogInfoEx(LOG_P25, P25_KMM_STR ", %s, llId = %u", kmm->toString().c_str(), llId); + // ignore Response Kind 2 command requests initiated from a SU + if (respKind == KMM_ResponseKind::DELAYED) { + LogWarning(LOG_P25, P25_KMM_STR ", %s, discarding SU initiated Response Kind 2 command, llId = %u", kmm->toString().c_str(), llId); + return nullptr; + } + + // response Kind 1 requests no OTAR response message + if (respKind == KMM_ResponseKind::NONE) { + if (m_verbose) { + LogInfoEx(LOG_P25, P25_KMM_STR ", %s, Response Kind 1 request, no OTAR response sent, llId = %u", kmm->toString().c_str(), llId); + } + return nullptr; + } + // respond with No-Service if KMF services are disabled if (!m_network->m_kmfServicesEnabled) return write_KMM_NoService(llId, kmm->getSrcLLId(), payloadSize); diff --git a/src/fne/network/P25OTARService.h b/src/fne/network/P25OTARService.h index 6a6bf5fe..224ed77d 100644 --- a/src/fne/network/P25OTARService.h +++ b/src/fne/network/P25OTARService.h @@ -75,7 +75,8 @@ namespace network * @param encrypted Flag indicating whether or not the KMM frame is encrypted. * @returns bool True, if KMM processed, otherwise false. */ - bool processDLD(const uint8_t* data, uint32_t len, uint32_t llId, uint8_t n, bool encrypted); + bool processDLD(const uint8_t* data, uint32_t len, uint32_t llId, uint8_t n, bool encrypted, + uint8_t algoId = P25DEF::ALGO_UNENCRYPT, uint16_t kid = 0U, const uint8_t* mi = nullptr); /** * @brief Updates the timer by the passed number of milliseconds. @@ -139,7 +140,8 @@ namespace network * @param[out] payloadSize Size of the returned KMM payload. * @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); + 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); /** * @brief Helper used to return a Rekey-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 f1a714e5..3f4258d2 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.cpp +++ b/src/fne/network/callhandler/packetdata/P25PacketData.cpp @@ -439,7 +439,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) +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) { // assemble a P25 PDU frame header for transport... data::DataHeader dataHeader = data::DataHeader(); @@ -447,17 +447,31 @@ void P25PacketData::write_PDU_KMM(const uint8_t* data, uint32_t len, uint32_t ll dataHeader.setMFId(MFG_STANDARD); dataHeader.setAckNeeded(true); dataHeader.setOutbound(true); - dataHeader.setSAP((encrypted) ? PDUSAP::ENC_KMM : PDUSAP::UNENC_KMM); + dataHeader.setSAP((encrypted) ? PDUSAP::ENC_USER_DATA : PDUSAP::UNENC_KMM); dataHeader.setLLId(llId); dataHeader.setBlocksToFollow(1U); + bool auxiliaryES = false; + if (encrypted) { + if (mi == nullptr) { + LogError(LOG_P25, P25_PDU_STR ", missing MI for encrypted KMM, llId = %u", llId); + return; + } + + dataHeader.setEXSAP(PDUSAP::UNENC_KMM); + dataHeader.setAlgId(algId); + dataHeader.setKId(kId); + dataHeader.setMI(mi); + auxiliaryES = true; + } + dataHeader.calculateLength(len); uint32_t pduLength = dataHeader.getPDULength(); DECLARE_UINT8_ARRAY(pduUserData, pduLength); ::memcpy(pduUserData, data, len); - dispatchUserFrameToFNE(dataHeader, false, false, pduUserData); + dispatchUserFrameToFNE(dataHeader, false, auxiliaryES, pduUserData); } /* Updates the timer by the passed number of milliseconds. */ @@ -882,9 +896,23 @@ void P25PacketData::dispatch(uint32_t peerId) LogInfoEx(LOG_P25, P25_PDU_STR ", KMM (Key Management Message), peer = %u, blocksToFollow = %u", peerId, status->assembler.dataHeader.getBlocksToFollow()); - bool encrypted = (sap == PDUSAP::ENC_KMM); + bool encrypted = (status->assembler.dataHeader.getSAP() == PDUSAP::ENC_KMM || + status->assembler.dataHeader.getSAP() == PDUSAP::ENC_USER_DATA); + uint8_t algId = P25DEF::ALGO_UNENCRYPT; + uint16_t kId = 0U; + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + + // if this was transported as encrypted user data with auxiliary ES, + // the KMM appears as UNENC_KMM via EXSAP but is still encrypted + if (status->assembler.getAuxiliaryES() && status->assembler.dataHeader.getSAP() == PDUSAP::ENC_USER_DATA) { + algId = status->assembler.dataHeader.getAlgId(); + kId = status->assembler.dataHeader.getKId(); + status->assembler.dataHeader.getMI(mi); + } + m_network->m_p25OTARService->processDLD(status->pduUserData, status->pduUserDataLength, status->llId, - status->assembler.dataHeader.getNs(), encrypted); + status->assembler.dataHeader.getNs(), encrypted, algId, kId, encrypted ? mi : nullptr); } break; default: diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.h b/src/fne/network/callhandler/packetdata/P25PacketData.h index 474dfa71..6aad025f 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.h +++ b/src/fne/network/callhandler/packetdata/P25PacketData.h @@ -96,8 +96,12 @@ namespace network * @param len Length of data. * @param llId Logical Link ID. * @param encrypted Flag indicating whether or not the KMM frame is encrypted. + * @param algId Encryption Algorithm ID. + * @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); + void 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); /** * @brief Updates the timer by the passed number of milliseconds.