Digital_Voice_Modem_Project
/
dvmhost
mirror of https://github.com/DVMProject/dvmhost.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

harden PDU delivery and batch OTAR rekey commands;

Browse Source
pull/133/head
Bryan Biedenkapp 1 month ago
parent c59e93199c
commit 1cb1281839
13 changed files with 486 additions and 130 deletions
Show Stats Download Patch File Download Diff File

27
src/common/p25/data/Assembler.cpp
Unescape View File

@ -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;

222
src/fne/network/P25OTARService.cpp
Unescape View File

@ -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<std::vector<uint8_t>> 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<uint8_t>& 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<std::vector<uint8_t>> 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<uint8_t[]>(payloadSize + 14U);
::memset(datagram.get(), 0x00U, payloadSize + 14U);
UInt8Array datagram = std::make_unique<uint8_t[]>(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<uint8_t>& 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<std::vector<uint8_t>>* 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<std::vector<uint8_t>>* 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<KeysetItem> 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<p25::kmm::KeyItem> 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<KeysetItem>{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<uint8_t> 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<uint8_t[]>(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<uint8_t[]>(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. */

10
src/fne/network/P25OTARService.h
Unescape View File

@ -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<std::vector<uint8_t>>* 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<std::vector<uint8_t>>* additionalResponses = nullptr);
/**
* @brief Helper used to return a Registration-Command KMM to the calling SU.

108
src/fne/network/callhandler/packetdata/P25PacketData.cpp
Unescape View File

@ -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<const UserContext*>(userContext);
UserContext* context = const_cast<UserContext*>(static_cast<const UserContext*>(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. */

23
src/fne/network/callhandler/packetdata/P25PacketData.h
Unescape View File

@ -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<uint16_t, uint8_t*> 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);
/**

25
src/host/p25/Control.cpp
Unescape View File

@ -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. */

2
src/host/p25/Control.h
Unescape View File

@ -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.

54
src/host/p25/packet/Data.cpp
Unescape View File

@ -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. */

10
src/host/p25/packet/Data.h
Unescape View File

@ -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.

31
tests/fne/FNETestHooks.cpp
Unescape View File

@ -94,6 +94,37 @@ std::unique_ptr<uint8_t[]> 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<std::vector<uint8_t>> FNETestHooks::buildOTARRekey(TrafficNetwork& network,
uint32_t llId, uint32_t kmmRSI)
{
std::vector<std::vector<uint8_t>> frames;
if (network.m_p25OTARService == nullptr)
return frames;
P25OTARService::KMMAuthContext auth;
uint32_t firstLength = 0U;
std::vector<std::vector<uint8_t>> 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<uint8_t>& 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.

9
tests/fne/FNETestHooks.h
Unescape View File

@ -98,6 +98,15 @@ public:
const std::vector<uint8_t>& 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<std::vector<uint8_t>> 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.

52
tests/p25/P25_KMM_Tests.cpp
Unescape View File

@ -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<uint16_t> 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<std::vector<uint8_t>> 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<KMMFrame> decoded = KMMFactory::create(frames[i].data());
REQUIRE(decoded != nullptr);
KMMRekeyCommand* rekey = dynamic_cast<KMMRekeyCommand*>(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]")
{

43
tests/p25/PDU_Confirmed_Large_Test.cpp
Unescape View File

@ -20,6 +20,7 @@ using namespace p25::data;
#include <catch2/catch_test_macros.hpp>
#include <stdlib.h>
#include <time.h>
#include <vector>
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<uint8_t> 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<uint8_t> decoded(source.size());
decoder.getUserData(decoded.data());
CHECK(decoded == source);
Assembler invalidLength;
CHECK_FALSE(invalidLength.disassemble(block, sizeof(block) - 1U, true));
}

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.