diff --git a/src/common/p25/Crypto.cpp b/src/common/p25/Crypto.cpp index 69284450..de5ed2da 100644 --- a/src/common/p25/Crypto.cpp +++ b/src/common/p25/Crypto.cpp @@ -434,25 +434,36 @@ UInt8Array P25Crypto::cryptAES_KMM_CBC(const uint8_t* macKey, const uint8_t* msg 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U }; + if (macKey == nullptr || msg == nullptr || + msgLen < (KMM_AES_MAC_LENGTH + 5U)) + return nullptr; + AES aes = AES(AESKeyLength::AES_256); - // pad the message as necessary - size_t paddedLen = msgLen + (AES::BLOCK_BYTES_LEN - (msgLen % AES::BLOCK_BYTES_LEN)); + // AACA-A 13.5.2 authenticates the complete KMM except for the MAC field + // itself. The MAC length, algorithm ID, key ID, and format that follow the + // MAC field remain part of the authenticated data. + const size_t macOffset = msgLen - (KMM_AES_MAC_LENGTH + 5U); + const size_t authLen = msgLen - KMM_AES_MAC_LENGTH; + const size_t paddedLen = ((authLen + AES::BLOCK_BYTES_LEN - 1U) / AES::BLOCK_BYTES_LEN) * AES::BLOCK_BYTES_LEN; + if (paddedLen == 0U || paddedLen > TEMP_BUFFER_LEN) + return nullptr; + uint8_t paddedMessage[TEMP_BUFFER_LEN]; ::memset(paddedMessage, 0x00U, TEMP_BUFFER_LEN); - ::memcpy(paddedMessage, msg, msgLen - KMM_AES_MAC_LENGTH - 5U); - ::memcpy(paddedMessage + msgLen - KMM_AES_MAC_LENGTH - 5U, msg + msgLen - 5U, 5U); + ::memcpy(paddedMessage, msg, macOffset); + ::memcpy(paddedMessage + macOffset, msg + msgLen - 5U, 5U); // perform AES-CBC encryption uint8_t* tempBuf = aes.encryptCBC(paddedMessage, paddedLen, macKey, iv); - UInt8Array wrappedKey = std::unique_ptr(new uint8_t[8U]); - ::memset(wrappedKey.get(), 0x00U, 8U); - ::memcpy(wrappedKey.get(), tempBuf + (msgLen - AES::BLOCK_BYTES_LEN), 8U); + UInt8Array mac = std::unique_ptr(new uint8_t[KMM_AES_MAC_LENGTH]); + ::memset(mac.get(), 0x00U, KMM_AES_MAC_LENGTH); + ::memcpy(mac.get(), tempBuf + paddedLen - AES::BLOCK_BYTES_LEN, KMM_AES_MAC_LENGTH); delete[] tempBuf; - return wrappedKey; + return mac; } /* Helper to generate a P25 KMM CMAC MAC key with the given AES-256 KEK. */ diff --git a/src/common/p25/kmm/KMMChangeover.cpp b/src/common/p25/kmm/KMMChangeover.cpp new file mode 100644 index 00000000..7e7748b7 --- /dev/null +++ b/src/common/p25/kmm/KMMChangeover.cpp @@ -0,0 +1,100 @@ +// 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) 2026 Bryan Biedenkapp, N2PLL + * + */ +#include "Defines.h" +#include "p25/P25Defines.h" +#include "p25/kmm/KMMChangeover.h" +#include "Log.h" + +using namespace p25; +using namespace p25::defines; +using namespace p25::kmm; + +#include + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/* Initializes a new instance of the KMMChangeover class. */ + +KMMChangeover::KMMChangeover() : KMMFrame(), + m_supersededKeysetId(0U), + m_activeKeysetId(0U) +{ + m_messageId = KMM_MessageType::CHANGEOVER_CMD; + m_respKind = KMM_ResponseKind::IMMEDIATE; +} + +/* Finalizes a instance of the KMMChangeover class. */ + +KMMChangeover::~KMMChangeover() = default; + +/* Gets the byte length of this KMMChangeover. */ + +uint32_t KMMChangeover::length() const +{ + uint32_t len = KMMFrame::length() + 3U; + return len; +} + +/* Decode a KMM changeover command. */ + +bool KMMChangeover::decode(const uint8_t* data) +{ + assert(data != nullptr); + + KMMFrame::decodeHeader(data); + + uint8_t noChangeovers = data[10U + m_bodyOffset]; + + if (noChangeovers >= 1U) { + m_supersededKeysetId = data[11U + m_bodyOffset]; + m_activeKeysetId = data[12U + m_bodyOffset]; + } + + // we don't support extra instructions -- only the first one + + return true; +} + +/* Encode a KMM changeover command. */ + +void KMMChangeover::encode(uint8_t* data) +{ + assert(data != nullptr); + m_messageLength = length(); + + KMMFrame::encodeHeader(data); + + data[10U + m_bodyOffset] = 1U; // number of changeovers + data[11U + m_bodyOffset] = m_supersededKeysetId; + data[12U + m_bodyOffset] = m_activeKeysetId; +} + +/* Returns a string that represents the current KMM frame. */ + +std::string KMMChangeover::toString() +{ + return std::string("KMM, CHANGEOVER_CMD (Changeover Command)"); +} + +// --------------------------------------------------------------------------- +// Protected Class Members +// --------------------------------------------------------------------------- + +/* Internal helper to copy the the class. */ + +void KMMChangeover::copy(const KMMChangeover& data) +{ + KMMFrame::copy(data); + + m_supersededKeysetId = data.m_supersededKeysetId; + m_activeKeysetId = data.m_activeKeysetId; +} diff --git a/src/common/p25/kmm/KMMChangeover.h b/src/common/p25/kmm/KMMChangeover.h new file mode 100644 index 00000000..7acdbec0 --- /dev/null +++ b/src/common/p25/kmm/KMMChangeover.h @@ -0,0 +1,89 @@ +// 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) 2026 Bryan Biedenkapp, N2PLL + * + */ +/** + * @file KMMChangeover.h + * @ingroup p25_kmm + * @file KMMChangeover.cpp + * @ingroup p25_kmm + */ +#if !defined(__P25_KMM__KMM_CHANGEOVER_H__) +#define __P25_KMM__KMM_CHANGEOVER_H__ + +#include "common/Defines.h" +#include "common/p25/kmm/KMMFrame.h" +#include "common/p25/kmm/KeysetItem.h" +#include "common/Utils.h" + +#include +#include + +namespace p25 +{ + namespace kmm + { + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Represents a KMM changeover command/response. + * @ingroup p25_kmm + */ + class DVM_COMMON_API KMMChangeover : public KMMFrame { + public: + /** + * @brief Initializes a new instance of the KMMChangeover class. + */ + KMMChangeover(); + /** + * @brief Finalizes a instance of the KMMChangeover class. + */ + ~KMMChangeover(); + + /** + * @brief Gets the byte length of this KMMFrame. + * @return uint32_t Length of KMMFrame. + */ + uint32_t length() const override; + + /** + * @brief Decode a KMM changeover command. + * @param[in] data Buffer containing KMM frame data to decode. + * @returns bool True, if decoded, otherwise false. + */ + bool decode(const uint8_t* data) override; + /** + * @brief Encode a KMM changeover command. + * @param[out] data Buffer to encode KMM frame data to. + */ + void encode(uint8_t* data) override; + + /** + * @brief Returns a string that represents the current KMM frame. + * @returns std::string String representation of the KMM frame. + */ + std::string toString() override; + + public: + /** + * @brief Superseded keyset ID. + */ + DECLARE_PROPERTY(uint8_t, supersededKeysetId, SupersededKeysetId); + /** + * @brief Active keyset ID. + */ + DECLARE_PROPERTY(uint8_t, activeKeysetId, ActiveKeysetId); + + DECLARE_COPY(KMMChangeover); + }; + } // namespace kmm +} // namespace p25 + +#endif // __P25_KMM__KMM_CHANGEOVER_H__ diff --git a/src/common/p25/kmm/KMMDeregistrationCommand.h b/src/common/p25/kmm/KMMDeregistrationCommand.h index d8a76bb9..fd2592fd 100644 --- a/src/common/p25/kmm/KMMDeregistrationCommand.h +++ b/src/common/p25/kmm/KMMDeregistrationCommand.h @@ -44,6 +44,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM deregistration command. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMDeregistrationCommand : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMDeregistrationResponse.h b/src/common/p25/kmm/KMMDeregistrationResponse.h index 2553d39d..0141cb6d 100644 --- a/src/common/p25/kmm/KMMDeregistrationResponse.h +++ b/src/common/p25/kmm/KMMDeregistrationResponse.h @@ -44,6 +44,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM deregistration response. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMDeregistrationResponse : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMFactory.cpp b/src/common/p25/kmm/KMMFactory.cpp index 5e7b4574..18cc90da 100644 --- a/src/common/p25/kmm/KMMFactory.cpp +++ b/src/common/p25/kmm/KMMFactory.cpp @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2025 Bryan Biedenkapp, N2PLL + * Copyright (C) 2025-2026 Bryan Biedenkapp, N2PLL * */ #include "Defines.h" @@ -40,6 +40,9 @@ std::unique_ptr KMMFactory::create(const uint8_t* data) uint8_t messageId = data[0U]; // Message ID switch (messageId) { + case KMM_MessageType::CHANGEOVER_CMD: + case KMM_MessageType::CHANGEOVER_RSP: + return decode(new KMMChangeover(), data); case KMM_MessageType::HELLO: return decode(new KMMHello(), data); case KMM_MessageType::INVENTORY_CMD: diff --git a/src/common/p25/kmm/KMMFactory.h b/src/common/p25/kmm/KMMFactory.h index a55cd87a..a6f47c6a 100644 --- a/src/common/p25/kmm/KMMFactory.h +++ b/src/common/p25/kmm/KMMFactory.h @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2025 Bryan Biedenkapp, N2PLL + * Copyright (C) 2025-2026 Bryan Biedenkapp, N2PLL * */ /** @@ -21,6 +21,7 @@ #include "common/p25/kmm/KeysetItem.h" #include "common/p25/kmm/KMMFrame.h" +#include "common/p25/kmm/KMMChangeover.h" #include "common/p25/kmm/KMMDeregistrationCommand.h" #include "common/p25/kmm/KMMDeregistrationResponse.h" #include "common/p25/kmm/KMMHello.h" diff --git a/src/common/p25/kmm/KMMHello.h b/src/common/p25/kmm/KMMHello.h index 31788a02..0607d677 100644 --- a/src/common/p25/kmm/KMMHello.h +++ b/src/common/p25/kmm/KMMHello.h @@ -44,6 +44,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM hello message. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMHello : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMInventoryCommand.h b/src/common/p25/kmm/KMMInventoryCommand.h index bcddb02a..23f5519f 100644 --- a/src/common/p25/kmm/KMMInventoryCommand.h +++ b/src/common/p25/kmm/KMMInventoryCommand.h @@ -44,6 +44,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM inventory command. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMInventoryCommand : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMInventoryResponseHeader.h b/src/common/p25/kmm/KMMInventoryResponseHeader.h index 58abe599..537f415f 100644 --- a/src/common/p25/kmm/KMMInventoryResponseHeader.h +++ b/src/common/p25/kmm/KMMInventoryResponseHeader.h @@ -44,6 +44,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM inventory response header. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMInventoryResponseHeader : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMInventoryResponseListKeyIDs.h b/src/common/p25/kmm/KMMInventoryResponseListKeyIDs.h index ded615a1..a68f7311 100644 --- a/src/common/p25/kmm/KMMInventoryResponseListKeyIDs.h +++ b/src/common/p25/kmm/KMMInventoryResponseListKeyIDs.h @@ -45,6 +45,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM inventory response listing key IDs. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMInventoryResponseListKeyIDs : public KMMInventoryResponseHeader { public: /** diff --git a/src/common/p25/kmm/KMMInventoryResponseListKeysets.h b/src/common/p25/kmm/KMMInventoryResponseListKeysets.h index 7b2b50e9..810d2223 100644 --- a/src/common/p25/kmm/KMMInventoryResponseListKeysets.h +++ b/src/common/p25/kmm/KMMInventoryResponseListKeysets.h @@ -32,6 +32,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM inventory response listing keysets. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMInventoryResponseListKeysets : public KMMInventoryResponseHeader { public: /** diff --git a/src/common/p25/kmm/KMMModifyKey.h b/src/common/p25/kmm/KMMModifyKey.h index 9a78f9e0..b61550f7 100644 --- a/src/common/p25/kmm/KMMModifyKey.h +++ b/src/common/p25/kmm/KMMModifyKey.h @@ -45,6 +45,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM modify key command. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMModifyKey : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMNegativeAck.h b/src/common/p25/kmm/KMMNegativeAck.h index f678c9da..028c99ad 100644 --- a/src/common/p25/kmm/KMMNegativeAck.h +++ b/src/common/p25/kmm/KMMNegativeAck.h @@ -44,6 +44,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM negative acknowledgment (NAK). + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMNegativeAck : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMNoService.h b/src/common/p25/kmm/KMMNoService.h index c04d6ced..3b8d3646 100644 --- a/src/common/p25/kmm/KMMNoService.h +++ b/src/common/p25/kmm/KMMNoService.h @@ -31,6 +31,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM no service indication. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMNoService : public KMMFrame { public: /** @@ -43,13 +47,13 @@ namespace p25 ~KMMNoService(); /** - * @brief Decode a KMM hello. + * @brief Decode a KMM no service indication. * @param[in] data Buffer containing KMM frame data to decode. * @returns bool True, if decoded, otherwise false. */ bool decode(const uint8_t* data) override; /** - * @brief Encode a KMM hello. + * @brief Encode a KMM no service indication. * @param[out] data Buffer to encode KMM frame data to. */ void encode(uint8_t* data) override; diff --git a/src/common/p25/kmm/KMMRegistrationCommand.h b/src/common/p25/kmm/KMMRegistrationCommand.h index b5c862d8..ec6fef2b 100644 --- a/src/common/p25/kmm/KMMRegistrationCommand.h +++ b/src/common/p25/kmm/KMMRegistrationCommand.h @@ -44,6 +44,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM registration command. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMRegistrationCommand : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMRegistrationResponse.h b/src/common/p25/kmm/KMMRegistrationResponse.h index f881a356..eb9e767a 100644 --- a/src/common/p25/kmm/KMMRegistrationResponse.h +++ b/src/common/p25/kmm/KMMRegistrationResponse.h @@ -44,6 +44,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM registration response. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMRegistrationResponse : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMRekeyAck.h b/src/common/p25/kmm/KMMRekeyAck.h index 580dff55..4e225677 100644 --- a/src/common/p25/kmm/KMMRekeyAck.h +++ b/src/common/p25/kmm/KMMRekeyAck.h @@ -45,6 +45,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM rekey acknowledgment. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMRekeyAck : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMRekeyCommand.h b/src/common/p25/kmm/KMMRekeyCommand.h index 56bb5b3c..c3ed449a 100644 --- a/src/common/p25/kmm/KMMRekeyCommand.h +++ b/src/common/p25/kmm/KMMRekeyCommand.h @@ -45,6 +45,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM rekey command. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMRekeyCommand : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMUnableToDecrypt.h b/src/common/p25/kmm/KMMUnableToDecrypt.h index f81fd305..34433397 100644 --- a/src/common/p25/kmm/KMMUnableToDecrypt.h +++ b/src/common/p25/kmm/KMMUnableToDecrypt.h @@ -45,6 +45,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM Unable-To-Decrypt command. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMUnableToDecrypt : public KMMFrame { public: /** diff --git a/src/common/p25/kmm/KMMZeroize.h b/src/common/p25/kmm/KMMZeroize.h index fc18cde0..e68ab119 100644 --- a/src/common/p25/kmm/KMMZeroize.h +++ b/src/common/p25/kmm/KMMZeroize.h @@ -31,6 +31,10 @@ namespace p25 // Class Declaration // --------------------------------------------------------------------------- + /** + * @brief Represents a KMM zeroize command. + * @ingroup p25_kmm + */ class DVM_COMMON_API KMMZeroize : public KMMFrame { public: /** @@ -43,13 +47,13 @@ namespace p25 ~KMMZeroize(); /** - * @brief Decode a KMM hello. + * @brief Decode a KMM zeroize command. * @param[in] data Buffer containing KMM frame data to decode. * @returns bool True, if decoded, otherwise false. */ bool decode(const uint8_t* data) override; /** - * @brief Encode a KMM hello. + * @brief Encode a KMM zeroize command. * @param[out] data Buffer to encode KMM frame data to. */ void encode(uint8_t* data) override; diff --git a/src/fne/network/P25OTARService.cpp b/src/fne/network/P25OTARService.cpp index 168d218f..ffaae43f 100644 --- a/src/fne/network/P25OTARService.cpp +++ b/src/fne/network/P25OTARService.cpp @@ -273,6 +273,9 @@ bool P25OTARService::processDLD(const uint8_t* data, uint32_t len, uint32_t llId sendNack(PDUAckType::NACK_UNDELIVERABLE); return false; } + + if (m_debug) + Utils::dump(1U, "P25OTARService::processDLD(), Decrypted KMM", kmmPayload.get(), len); } if (len < 10U || len > 512U || ((((uint32_t)kmmPayload[1U] << 8U) | kmmPayload[2U]) + 3U) != len) { @@ -786,6 +789,14 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ macTek.getKey(tek); if (!frame->verifyMAC(tek, buffer.get(), len)) { + if (frame->getMessageId() == KMM_MessageType::NAK) { + KMMNegativeAck* nack = static_cast(frame.get()); + LogWarning(LOG_P25, P25_KMM_STR ", received unauthenticated KMM NACK, RSI = %u, rejectedMessageId = $%02X, rejectedMN = %u, status = $%02X, headerMN = %u, macKId = $%04X, macFormat = $%02X", + frame->getSrcLLId(), nack->getNakMessageId(), nack->getMessageNumber(), + nack->getStatus(), nack->KMMFrame::getMessageNumber(), frame->getMACKId(), + frame->getMACFormat()); + logResponseStatus(llId, nack->toString(), nack->getStatus()); + } LogWarning(LOG_P25, P25_KMM_STR ", invalid MAC, RSI = %u", frame->getSrcLLId()); return makeNack(KMM_Status::INVALID_MAC); } @@ -827,11 +838,22 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ } // maintain outbound state independently from the validated inbound MN + // AACA-A 13.6.4 initializes the SU's last valid MN (MNL) to zero and + // applies a strict advance for a new RK3 message, so the first KMF MN must + // be one -- sending zero is rejected as a replay before the KMM is processed if (m_rsiMessageNumber.find(llId) == m_rsiMessageNumber.end()) - m_rsiMessageNumber[llId] = 0U; + m_rsiMessageNumber[llId] = 1U; // handle the KMM message based on its type switch (frame->getMessageId()) { + case KMM_MessageType::CHANGEOVER_RSP: + { + KMMChangeover* kmm = static_cast(frame.get()); + LogInfoEx(LOG_P25, P25_KMM_STR ", %s, llId = %u, supersededKeysetId = $%02X, activeKeysetId = $%02X", kmm->toString().c_str(), + llId, kmm->getSupersededKeysetId(), kmm->getActiveKeysetId()); + } + break; + case KMM_MessageType::HELLO: { KMMHello* kmm = static_cast(frame.get()); @@ -904,8 +926,9 @@ UInt8Array P25OTARService::processKMM(const uint8_t* data, uint32_t len, uint32_ case KMM_MessageType::NAK: { KMMNegativeAck* kmm = static_cast(frame.get()); - LogInfoEx(LOG_P25, P25_KMM_STR ", %s, llId = %u, nackMessageId = $%02X, messageNo = %u, status = $%02X", kmm->toString().c_str(), - llId, kmm->getNakMessageId(), kmm->getMessageNumber(), kmm->getStatus()); + LogWarning(LOG_P25, P25_KMM_STR ", authenticated %s, llId = %u, rejectedMessageId = $%02X, rejectedMN = %u, status = $%02X, headerMN = %u", + kmm->toString().c_str(), llId, kmm->getNakMessageId(), + kmm->getMessageNumber(), kmm->getStatus(), kmm->KMMFrame::getMessageNumber()); logResponseStatus(llId, kmm->toString(), kmm->getStatus()); } break; @@ -1008,7 +1031,8 @@ UInt8Array P25OTARService::write_KMM_Rekey_Command(uint32_t llId, uint32_t kmmRS uint8_t mi[MI_LENGTH_BYTES]; ::memset(mi, 0x00U, MI_LENGTH_BYTES); - uint16_t mn = 0U; + // Zero is the SU's initial MNL, not a valid first non-retry RK3 MNR. + uint16_t mn = 1U; if (m_rsiMessageNumber.find(llId) != m_rsiMessageNumber.end()) { mn = m_rsiMessageNumber[llId]; } diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.cpp b/src/fne/network/callhandler/packetdata/P25PacketData.cpp index 23b75dc7..47824069 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.cpp +++ b/src/fne/network/callhandler/packetdata/P25PacketData.cpp @@ -193,11 +193,11 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee 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); + LogWarning(LOG_P25, P25_PDU_STR ", conflicting duplicate block %u", currentBlock); return false; + } else { + LogWarning(LOG_P25, P25_PDU_STR ", ignoring identical duplicate block %u", currentBlock); } - LogInfoEx(LOG_P25, P25_PDU_STR ", ignoring identical duplicate block %u", currentBlock); - return true; } uint8_t* blockData = new uint8_t[blockLength]; @@ -239,8 +239,14 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee // is this a response header? if (status->assembler.dataHeader.getFormat() == PDUFormatType::RSP) { dispatch(peerId); - status->streamId = 0U; - status->clearReceivedBlocks(); + + // A response PDU is a complete one-block transaction. + // Do not retain an RxStatus with totalBlocks reset to + // zero: the next PDU from this peer would reuse it and + // could never satisfy dataBlockCnt == totalBlocks. + m_status.erase(peerId); + delete status; + status = nullptr; return true; } diff --git a/src/host/p25/packet/Data.cpp b/src/host/p25/packet/Data.cpp index b419c58f..c7681248 100644 --- a/src/host/p25/packet/Data.cpp +++ b/src/host/p25/packet/Data.cpp @@ -36,10 +36,14 @@ using namespace p25::packet; // --------------------------------------------------------------------------- const uint8_t MAX_PDU_RETRY_CNT = 2U; -const uint32_t CONV_REG_WAIT_TIMEOUT = 750U; // ms const uint32_t SNDCP_READY_TIMEOUT = 10U; const uint32_t SNDCP_STANDBY_TIMEOUT = 60U; +// Native air-interface modems do not report a PDU transaction-complete event. +// Leave enough time for the modem TX tail to expire before starting another +// complete PDU transaction. DFSI/V.24 supplies its own transaction framing. +#define P25_PDU_NATIVE_MODEM_GUARD_MS 750U + // --------------------------------------------------------------------------- // Public Class Members // --------------------------------------------------------------------------- @@ -174,7 +178,7 @@ bool Data::process(uint8_t* data, uint32_t len) if (retry != m_retryPDUState.end() && retry->second.bitLength > 0U && !retry->second.data.empty()) { if (retry->second.retryCount < MAX_PDU_RETRY_CNT) { m_p25->writeRF_Preamble(); - writeRF_PDU(retry->second.data.data(), retry->second.bitLength, false, true, retryLlId, false); + queueRF_PDU(retry->second.data.data(), retry->second.bitLength, false, true, retryLlId, false); retry->second.retryCount++; } else { @@ -641,13 +645,11 @@ bool Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, { assert(pduUserData != nullptr); - m_p25->writeRF_TDU(true, imm); - uint32_t bitLength = 0U; UInt8Array data = m_rfAssembler->assemble(dataHeader, extendedAddress, auxiliaryES, pduUserData, &bitLength); bool trackRetry = dataHeader.getFormat() == PDUFormatType::CONFIRMED && dataHeader.getAckNeeded(); - return writeRF_PDU(data.get(), bitLength, imm, false, dataHeader.getLLId(), trackRetry); + return queueRF_PDU(data.get(), bitLength, imm, false, dataHeader.getLLId(), trackRetry); } /* Helper to write user data as a P25 PDU packet. */ @@ -664,6 +666,44 @@ void Data::writeNet_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, void Data::clock(uint32_t ms) { + PendingPDU pending; + bool dispatchPending = false; + + // scope is intentional + { + std::lock_guard lock(m_pduPacingMutex); + if (m_pduPacingRemainingMs > ms) + m_pduPacingRemainingMs -= ms; + else + m_pduPacingRemainingMs = 0U; + + if (m_pduPacingRemainingMs == 0U && !m_pendingPDUs.empty()) { + pending = std::move(m_pendingPDUs.front()); + m_pendingPDUs.pop_front(); + dispatchPending = true; + } + } + + if (dispatchPending) { + bool queued = writeRF_PDU(pending.data.data(), pending.bitLength, pending.imm, pending.ackRetry, + pending.retryLlId, pending.trackRetry); + + // scope is intentional + { + std::lock_guard lock(m_pduPacingMutex); + if (queued) + m_pduPacingRemainingMs = P25_PDU_NATIVE_MODEM_GUARD_MS; + } + + if (!queued) { + LogError(LOG_RF, P25_PDU_STR ", unable to dispatch paced PDU, llId = %u, bitLength = %u", + pending.retryLlId, pending.bitLength); + } + else if (m_debug) { + LogDebugEx(LOG_RF, "Data::clock()", "dispatched paced native-modem PDU"); + } + } + if (m_p25->m_sndcpSupport) { // clock all the SNDCP ready timers std::vector sndcpReadyExpired = std::vector(); @@ -813,6 +853,9 @@ Data::Data(Control* p25, bool dumpPDUData, bool repeatPDU, bool debug, bool verb m_netDataBlockCnt(0U), m_netTotalBlocks(0U), m_retryPDUState(), + m_pendingPDUs(), + m_pduPacingRemainingMs(0U), + m_pduPacingMutex(), m_rfPduUserData(nullptr), m_rfPduUserDataLength(0U), m_netPduUserData(nullptr), @@ -966,7 +1009,50 @@ void Data::writeNetwork(const uint8_t currentBlock, const uint8_t* data, uint32_ m_p25->m_network->writeP25PDU(m_rfAssembler->dataHeader, currentBlock, data, len, lastBlock); } -/* Helper to write a P25 PDU packet. */ +/* Helper to queue a complete P25 PDU packet. */ + +bool Data::queueRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ackRetry, + uint32_t retryLlId, bool trackRetry) +{ + assert(pdu != nullptr); + assert(bitLength > 0U); + + // Pacing is required only for native dedicated/hotspot modem transports. + // Keep the entire PDU transaction together; spacing individual modem + // chunks would corrupt the CAI transmission. + if (!m_p25->m_isModemDFSI) { + std::lock_guard lock(m_pduPacingMutex); + if (m_pduPacingRemainingMs > 0U || !m_pendingPDUs.empty()) { + PendingPDU pending; + + uint32_t byteLength = (bitLength + 7U) / 8U; + pending.data.assign(pdu, pdu + byteLength); + pending.bitLength = bitLength; + pending.imm = imm; + pending.ackRetry = ackRetry; + pending.retryLlId = retryLlId; + pending.trackRetry = trackRetry; + + m_pendingPDUs.push_back(std::move(pending)); + + if (m_debug) { + LogDebugEx(LOG_RF, "Data::writeRF_PDU()", "deferred native-modem PDU by up to %u ms, pending = %u, llId = %u", + m_pduPacingRemainingMs, uint32_t(m_pendingPDUs.size()), retryLlId); + } + + return true; + } + } + + bool queued = writeRF_PDU(pdu, bitLength, imm, ackRetry, retryLlId, trackRetry); + if (queued && !m_p25->m_isModemDFSI) { + std::lock_guard lock(m_pduPacingMutex); + m_pduPacingRemainingMs = P25_PDU_NATIVE_MODEM_GUARD_MS; + } + return queued; +} + +/* Helper to write a complete P25 PDU transaction without applying the native-modem guard. */ bool Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ackRetry, uint32_t retryLlId, bool trackRetry) @@ -1042,7 +1128,7 @@ bool Data::writeNet_PDU_Buffered() 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(); - return writeRF_PDU(data.get(), bitLength, false, false, m_netAssembler->dataHeader.getLLId(), trackRetry); + return queueRF_PDU(data.get(), bitLength, false, false, m_netAssembler->dataHeader.getLLId(), trackRetry); } /* Helper to re-write a received P25 PDU packet. */ @@ -1051,7 +1137,7 @@ void Data::writeRF_PDU_Buffered() { uint32_t bitLength = 0U; UInt8Array data = m_rfAssembler->assemble(m_rfAssembler->dataHeader, m_rfAssembler->getExtendedAddress(), m_rfAssembler->getAuxiliaryES(), m_rfPduUserData, &bitLength); - writeRF_PDU(data.get(), bitLength); + queueRF_PDU(data.get(), bitLength); } /* Helper to write a PDU acknowledge response. */ @@ -1097,5 +1183,5 @@ void Data::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint8_t a rspHeader.getResponseClass(), rspHeader.getResponseType(), rspHeader.getResponseStatus(), rspHeader.getLLId(), rspHeader.getSrcLLId()); } - writeRF_PDU(data, bitLength); + queueRF_PDU(data, bitLength); } diff --git a/src/host/p25/packet/Data.h b/src/host/p25/packet/Data.h index cbc45851..1d19f280 100644 --- a/src/host/p25/packet/Data.h +++ b/src/host/p25/packet/Data.h @@ -27,6 +27,8 @@ #include "p25/Control.h" #include +#include +#include #include #include #include @@ -155,6 +157,18 @@ namespace p25 }; std::unordered_map m_retryPDUState; + struct PendingPDU { + std::vector data; + uint32_t bitLength = 0U; + bool imm = false; + bool ackRetry = false; + uint32_t retryLlId = 0U; + bool trackRetry = false; + }; + std::deque m_pendingPDUs; + uint32_t m_pduPacingRemainingMs; + std::mutex m_pduPacingMutex; + uint8_t* m_rfPduUserData; uint32_t m_rfPduUserDataLength; uint8_t* m_netPduUserData; @@ -214,7 +228,7 @@ namespace p25 void resetReceivedBlocks(); /** - * @brief Helper to write a P25 PDU packet. + * @brief Helper to queue a complete P25 PDU packet. * @param[in] pdu Constructed PDU to transmit. * @param bitlength Length of PDU in bits. * @param imm Flag indicating the PDU should be written to the immediate queue. @@ -222,8 +236,20 @@ namespace p25 * @param retryLlId Logical Link ID for retrying the PDU. * @param trackRetry Flag indicating whether or not to track the retry. */ - bool writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm = false, bool ackRetry = false, + bool queueRF_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 complete P25 PDU transaction without applying the native-modem guard. + * @param pdu Constructed PDU to transmit. + * @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. + * @returns bool True, if the PDU was successfully written, otherwise false. + */ + bool writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ackRetry, + uint32_t retryLlId, bool trackRetry); /** * @brief Helper to write a network P25 PDU packet. * This will take buffered network PDU data and repeat it over the air. diff --git a/tests/fne/FNETestHooks.cpp b/tests/fne/FNETestHooks.cpp index 8bcff95a..b3af837b 100644 --- a/tests/fne/FNETestHooks.cpp +++ b/tests/fne/FNETestHooks.cpp @@ -223,6 +223,35 @@ bool FNETestHooks::processOTARDLDPDU(TrafficNetwork& network, const std::vector< return true; } +/** Injects a one-block PDU response through the FNE packet-data receiver. */ + +bool FNETestHooks::processP25PDUResponse(TrafficNetwork& network, uint32_t llId, + uint8_t responseStatus) +{ + if (network.m_tagP25 == nullptr) + return false; + + p25::data::DataHeader header; + header.setFormat(PDUFormatType::RSP); + header.setMFId(MFG_STANDARD); + header.setOutbound(false); + header.setResponseClass(PDUAckClass::ACK); + header.setResponseType(PDUAckType::ACK); + header.setResponseStatus(responseStatus); + header.setLLId(llId); + header.setFullMessage(true); + header.setBlocksToFollow(0U); + + uint8_t envelope[24U + P25_PDU_FEC_LENGTH_BYTES] = { 0U }; + SET_UINT24(P25_PDU_FEC_LENGTH_BYTES, envelope, 8U); + envelope[20U] = 0U; + envelope[21U] = 0U; + header.encode(envelope + 24U); + + return network.m_tagP25->packetData()->processFrame(envelope, sizeof(envelope), + 1U, 0U, 1U); +} + /** * @brief Passes a complete Version-0 DLI datagram through the network receive task. * @param network The TrafficNetwork instance. diff --git a/tests/fne/FNETestHooks.h b/tests/fne/FNETestHooks.h index abaeb706..1a21f26a 100644 --- a/tests/fne/FNETestHooks.h +++ b/tests/fne/FNETestHooks.h @@ -147,6 +147,15 @@ public: static bool processOTARDLDPDU(network::TrafficNetwork& network, const std::vector& packet, uint32_t llId, bool encrypted = false, uint8_t algoId = P25DEF::ALGO_UNENCRYPT, uint16_t kid = 0U, const uint8_t* mi = nullptr); + /** + * @brief Injects a one-block PDU response through the FNE packet-data receiver. + * @param network The TrafficNetwork that owns the packet-data receiver. + * @param llId Logical Link ID associated with the response. + * @param responseStatus Response status/N(R) value. + * @return True if the response was processed successfully. + */ + static bool processP25PDUResponse(network::TrafficNetwork& network, uint32_t llId, + uint8_t responseStatus); /** * @brief Passes a complete Version-0 DLI datagram through the network receive task. * @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 ba7a09fe..db1ab0a4 100644 --- a/tests/p25/P25_KMM_Tests.cpp +++ b/tests/p25/P25_KMM_Tests.cpp @@ -452,6 +452,40 @@ TEST_CASE("P25 OTAR DLD processes KMM through the packet-data service entry poin REQUIRE_FALSE(FNETestHooks::hasOTARInboundMessageNumber(harness.traffic, SU_RSI + 1U, MN + 1U)); } +TEST_CASE("P25 FNE accepts a KMM immediately after a PDU response", "[p25][kmm][otar][pdu-state]") +{ + KMMFNEHarness harness; + constexpr uint32_t SU_RSI = 0x654321U; + constexpr uint16_t MN = 1U; + uint8_t tek[32U]; + for (uint32_t i = 0U; i < sizeof(tek); ++i) + tek[i] = (uint8_t)i; + + EKCKeyItem item; + item.id(1U); item.algId(ALGO_AES_256); item.kId(0x1234U); item.sln(1U); + item.keyMaterial("000102030405060708090A0B0C0D0E0F101112131415161718191A1B1C1D1E1F"); + FNETestHooks::addCryptoKey(harness.traffic, item); + + REQUIRE(FNETestHooks::processP25PDUResponse(harness.traffic, SU_RSI, 5U)); + + KMMHello hello; + hello.setDstLLId(WUID_FNE); + hello.setSrcLLId(SU_RSI); + hello.setResponseKind(KMM_ResponseKind::NONE); + hello.setHasMessageNumber(true); + hello.setMessageNumber(MN); + hello.setFlag(KMM_HelloFlag::IDENT_ONLY); + hello.setMACType(KMM_MAC::ENH_MAC); + hello.setMACAlgId(ALGO_AES_256); + hello.setMACKId(0x1234U); + hello.setMACFormat(KMM_MAC_FORMAT_CBC); + + std::vector encoded = encodeKMM(hello); + hello.generateMAC(tek, encoded.data()); + REQUIRE(FNETestHooks::processOTARDLDPDU(harness.traffic, encoded, SU_RSI)); + REQUIRE(FNETestHooks::hasOTARInboundMessageNumber(harness.traffic, SU_RSI, MN)); +} + TEST_CASE("P25 OTAR DLI validates its Version-0 preamble and dispatches KMM", "[p25][kmm][otar][dli]") { constexpr uint32_t SU_RSI = 0x654321U; @@ -578,7 +612,9 @@ TEST_CASE("P25 OTAR Rekey Commands batch four keys per KMM", "[p25][kmm][otar][r REQUIRE(rekey != nullptr); REQUIRE(rekey->getKeysets().size() == 1U); CHECK(rekey->getKeysets()[0U].keys().size() == expectedCounts[i]); - CHECK(rekey->getMessageNumber() == i); + // A new RK3 uses Rule 1 message-number validation. The SU initializes + // MNL to zero, so the first valid outbound KMF message number is one. + CHECK(rekey->getMessageNumber() == i + 1U); CHECK(rekey->getComplete() == (i + 1U == frames.size())); CHECK(frames[i].size() <= 287U); }