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

still not prime-time ready for anyone to use -- but I am over here implementing some fixes because my testcases are blowing up;

Browse Source
pull/121/merge
Bryan Biedenkapp 2 weeks ago
parent fe0350473b
commit 90ca8f1a6a
4 changed files with 151 additions and 34 deletions
Show Stats Download Patch File Download Diff File

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

@ -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<uint8_t[]>(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<KMMHello*>(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<KMMDeregistrationCommand*>(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);

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

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

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

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

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

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

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.