From 75e808c90cad2c23b48593295bbc8861e065db94 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Tue, 30 Dec 2025 16:00:40 -0500 Subject: [PATCH] add dropped call support to dvmpatch; refactor/rewrite dvmpatch encryption support; --- configs/patch-config.example.yml | 3 + src/bridge/HostBridge.cpp | 13 +- src/patch/HostPatch.cpp | 747 +++++++++++++++++++++++-------- src/patch/HostPatch.h | 17 + 4 files changed, 595 insertions(+), 185 deletions(-) diff --git a/configs/patch-config.example.yml b/configs/patch-config.example.yml index 03fa576e..48ad1515 100644 --- a/configs/patch-config.example.yml +++ b/configs/patch-config.example.yml @@ -87,6 +87,9 @@ network: # Traffic Encryption Key ID (Hex) tekKeyId: 1 + # Amount of time (ms) from loss of active call before ending call. + dropTimeMs: 1000 + # Flag indicating whether or not the patch is two-way. # NOTE: If false (one-way patch from source to destination), and patching clear to # encrypted traffic, only the destination TEK will be used for encryption. The clear diff --git a/src/bridge/HostBridge.cpp b/src/bridge/HostBridge.cpp index 1b35a60c..75fb12e4 100644 --- a/src/bridge/HostBridge.cpp +++ b/src/bridge/HostBridge.cpp @@ -2232,6 +2232,15 @@ void HostBridge::decodeP25AudioFrame(uint8_t* ldu, uint32_t srcId, uint32_t dstI using namespace p25; using namespace p25::defines; + { + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + m_p25Crypto->getMI(mi); + + LogInfoEx(LOG_NET, "Crypto, (D) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + } + // decode 9 IMBE codewords into PCM samples for (int n = 0; n < 9; n++) { uint8_t imbe[RAW_IMBE_LENGTH_BYTES]; @@ -2265,7 +2274,7 @@ void HostBridge::decodeP25AudioFrame(uint8_t* ldu, uint32_t srcId, uint32_t dstI break; } - // Utils::dump(1U, "HostBridge::decodeP25AudioFrame(), IMBE", imbe, RAW_IMBE_LENGTH_BYTES); + Utils::dump(1U, "HostBridge::decodeP25AudioFrame(), IMBE", imbe, RAW_IMBE_LENGTH_BYTES); if (m_tekAlgoId != P25DEF::ALGO_UNENCRYPT && m_tekKeyId > 0U && m_p25Crypto->getTEKLength() > 0U) { switch (m_tekAlgoId) { @@ -2284,6 +2293,8 @@ void HostBridge::decodeP25AudioFrame(uint8_t* ldu, uint32_t srcId, uint32_t dstI } } + Utils::dump(1U, "HostBridge::decodeP25AudioFrame(), Decrypted IMBE", imbe, RAW_IMBE_LENGTH_BYTES); + short samples[AUDIO_SAMPLES_LENGTH]; int errs = 0; #if defined(_WIN32) diff --git a/src/patch/HostPatch.cpp b/src/patch/HostPatch.cpp index 218d5a24..63730e96 100644 --- a/src/patch/HostPatch.cpp +++ b/src/patch/HostPatch.cpp @@ -76,6 +76,8 @@ HostPatch::HostPatch(const std::string& confFile) : m_twoWayPatch(false), m_mmdvmP25Reflector(false), m_mmdvmP25Net(nullptr), + m_dropTimeMS(1U), + m_callDropTime(1000U, 0U, 1000U), m_netState(RS_NET_IDLE), m_netLC(), m_gotNetLDU1(false), @@ -87,6 +89,8 @@ HostPatch::HostPatch(const std::string& confFile) : m_dmrEmbeddedData(), m_grantDemand(false), m_callInProgress(false), + m_callDstId(0U), + m_callSlotNo(0U), m_callAlgoId(P25DEF::ALGO_UNENCRYPT), m_rxStartTime(0U), m_rxStreamId(0U), @@ -259,6 +263,23 @@ int HostPatch::run() m_mmdvmP25Net->clock(ms); } + if (m_callDropTime.isRunning()) + m_callDropTime.clock(ms); + if (m_callDropTime.isRunning() && m_callDropTime.hasExpired() && m_callInProgress) { + switch (m_digiMode) { + case TX_MODE_DMR: + resetDMRCall(DMRDEF::WUID_ALL, m_callSlotNo); + break; + + case TX_MODE_P25: + resetP25Call(P25DEF::WUID_FNE); + break; + + default: + break; + } + } + if (ms < 2U) Thread::sleep(1U); } @@ -317,6 +338,9 @@ bool HostPatch::readParams() return false; } + m_dropTimeMS = (uint16_t)systemConf["dropTimeMs"].as(1000U); + m_callDropTime = Timer(1000U, 0U, m_dropTimeMS); + m_trace = systemConf["trace"].as(false); m_debug = systemConf["debug"].as(false); @@ -325,6 +349,7 @@ bool HostPatch::readParams() LogInfo(" P25 Network Id: $%05X", m_netId); LogInfo(" Digital Mode: %s", m_digiMode == TX_MODE_DMR ? "DMR" : "P25"); LogInfo(" Grant Demands: %s", m_grantDemand ? "yes" : "no"); + LogInfo(" Drop Time: %ums", m_dropTimeMS); LogInfo(" MMDVM P25 Reflector Patch: %s", m_mmdvmP25Reflector ? "yes" : "no"); if (m_debug) { @@ -710,6 +735,9 @@ void HostPatch::processDMRNetwork(uint8_t* buffer, uint32_t length) uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); m_rxStartTime = now; + m_callDstId = actualDstId; + m_callSlotNo = slotNo; + LogInfoEx(LOG_HOST, "DMR, call start, srcId = %u, dstId = %u, slot = %u", srcId, dstId, slotNo); } @@ -733,23 +761,12 @@ void HostPatch::processDMRNetwork(uint8_t* buffer, uint32_t length) dmrData.setData(data.get()); m_network->writeDMRTerminator(dmrData, &seqNo, &n, m_dmrEmbeddedData); - m_network->resetDMR(dmrData.getSlotNo()); - - if (m_rxStartTime > 0U) { - uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - uint64_t diff = now - m_rxStartTime; - - LogInfoEx(LOG_HOST, "DMR, call end, srcId = %u, dstId = %u, dur = %us", srcId, dstId, diff / 1000U); - } - - m_callInProgress = false; - m_rxStartTime = 0U; - m_rxStreamId = 0U; - m_network->resetDMR(slotNo); + resetDMRCall(srcId, dmrData.getSlotNo()); return; } m_rxStreamId = m_network->getDMRStreamId(slotNo); + m_callDropTime.start(); uint8_t* buffer = nullptr; @@ -972,6 +989,17 @@ void HostPatch::processP25Network(uint8_t* buffer, uint32_t length) lsd.setLSD1(lsd1); lsd.setLSD2(lsd2); + if ((duid == DUID::TDU) || (duid == DUID::TDULC)) { + // ignore TDU's that are grant demands + if (grantDemand) { + m_network->resetP25(); + return; + } + + resetP25Call(srcId); + return; + } + if (control.getLCO() == LCO::GROUP) { if (srcId == 0) { m_network->resetP25(); @@ -984,21 +1012,17 @@ void HostPatch::processP25Network(uint8_t* buffer, uint32_t length) return; } - bool reverseEncrypt = false; - bool tekEnable = m_tekSrcEnable; + bool reverseCall = false; // is the traffic flow reversed? (i.e. destination -> source instead of source -> destination) + bool skipCrypto = false; uint32_t actualDstId = m_srcTGId; - uint8_t tekAlgoId = m_tekSrcAlgoId; - uint16_t tekKeyId = m_tekSrcKeyId; if (!m_mmdvmP25Reflector) { actualDstId = m_dstTGId; if (m_twoWayPatch) { + // is this a reverse call? if (dstId == m_dstTGId) { actualDstId = m_srcTGId; - tekEnable = m_tekDstEnable; - tekAlgoId = m_tekDstAlgoId; - tekKeyId = m_tekDstKeyId; - reverseEncrypt = true; + reverseCall = true; } } else { if (dstId == m_dstTGId) { @@ -1017,37 +1041,160 @@ void HostPatch::processP25Network(uint8_t* buffer, uint32_t length) uint8_t frameType = buffer[180U]; if (frameType == FrameType::HDU_VALID) { m_callAlgoId = buffer[181U]; - if (tekEnable && m_callAlgoId != ALGO_UNENCRYPT) { - callKID = GET_UINT16(buffer, 182U); + callKID = GET_UINT16(buffer, 182U); + } - if (m_callAlgoId != tekAlgoId && callKID != tekKeyId) { - m_callAlgoId = ALGO_UNENCRYPT; - m_callInProgress = false; + if (m_twoWayPatch) { + if (m_callAlgoId == m_tekSrcAlgoId && m_callAlgoId == m_tekDstAlgoId && callKID == m_tekSrcKeyId && callKID == m_tekDstKeyId) { + // both TEK's are the same, no need to process both + skipCrypto = true; + } - LogWarning(LOG_HOST, "P25, call ignored, using different encryption parameters, callAlgoId = $%02X, callKID = $%04X, tekAlgoId = $%02X, tekKID = $%04X", m_callAlgoId, callKID, tekAlgoId, tekKeyId); - m_network->resetP25(); - return; + if (!skipCrypto) { + if (reverseCall) { + // is the incoming call encrypted? + if (m_callAlgoId != ALGO_UNENCRYPT) { + if (m_tekDstEnable && m_callAlgoId != m_tekDstAlgoId && callKID != m_tekDstKeyId) { + m_callAlgoId = ALGO_UNENCRYPT; + m_callInProgress = false; + + LogWarning(LOG_HOST, "P25, call ignored, using different encryption parameters, callAlgoId = $%02X, callKID = $%04X, tekAlgoId = $%02X, tekKID = $%04X", m_callAlgoId, callKID, m_tekDstAlgoId, m_tekDstKeyId); + m_network->resetP25(); + return; + } else { + if (m_tekDstEnable) { + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { + mi[i] = buffer[184U + i]; + } + + LogInfoEx(LOG_NET, P25_HDU_STR ", (D) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + + m_p25DstCrypto->setMI(mi); + m_p25DstCrypto->generateKeystream(); + } + + if (m_tekSrcEnable && m_tekSrcAlgoId != ALGO_UNENCRYPT && m_tekSrcKeyId != 0U) { + // setup source crypto + m_p25SrcCrypto->generateMI(); + + uint8_t miSrc[MI_LENGTH_BYTES]; + ::memset(miSrc, 0x00U, MI_LENGTH_BYTES); + m_p25SrcCrypto->getMI(miSrc); + LogInfoEx(LOG_NET, P25_HDU_STR ", (S) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + miSrc[0U], miSrc[1U], miSrc[2U], miSrc[3U], miSrc[4U], miSrc[5U], miSrc[6U], miSrc[7U], miSrc[8U]); + + m_p25SrcCrypto->generateKeystream(); + } + } + } else { + if (m_tekSrcEnable && m_tekSrcAlgoId != ALGO_UNENCRYPT && m_tekSrcKeyId != 0U) { + // setup source crypto + m_p25SrcCrypto->generateMI(); + + uint8_t miSrc[MI_LENGTH_BYTES]; + ::memset(miSrc, 0x00U, MI_LENGTH_BYTES); + m_p25SrcCrypto->getMI(miSrc); + LogInfoEx(LOG_NET, P25_HDU_STR ", (S) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + miSrc[0U], miSrc[1U], miSrc[2U], miSrc[3U], miSrc[4U], miSrc[5U], miSrc[6U], miSrc[7U], miSrc[8U]); + + m_p25SrcCrypto->generateKeystream(); + } + } } else { + // is the incoming call encrypted? + if (m_callAlgoId != ALGO_UNENCRYPT) { + if (m_tekSrcEnable && m_callAlgoId != m_tekSrcAlgoId && callKID != m_tekSrcKeyId) { + m_callAlgoId = ALGO_UNENCRYPT; + m_callInProgress = false; + + LogWarning(LOG_HOST, "P25, call ignored, using different encryption parameters, callAlgoId = $%02X, callKID = $%04X, tekAlgoId = $%02X, tekKID = $%04X", m_callAlgoId, callKID, m_tekSrcAlgoId, m_tekSrcKeyId); + m_network->resetP25(); + return; + } else { + if (m_tekSrcEnable) { + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { + mi[i] = buffer[184U + i]; + } + + LogInfoEx(LOG_NET, P25_HDU_STR ", (S) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + + m_p25SrcCrypto->setMI(mi); + m_p25SrcCrypto->generateKeystream(); + } + + if (m_tekDstEnable && m_tekDstAlgoId != ALGO_UNENCRYPT && m_tekDstKeyId != 0U) { + // setup destination crypto + m_p25DstCrypto->generateMI(); + + uint8_t miDst[MI_LENGTH_BYTES]; + ::memset(miDst, 0x00U, MI_LENGTH_BYTES); + m_p25DstCrypto->getMI(miDst); + LogInfoEx(LOG_NET, P25_HDU_STR ", (D) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + miDst[0U], miDst[1U], miDst[2U], miDst[3U], miDst[4U], miDst[5U], miDst[6U], miDst[7U], miDst[8U]); + + m_p25DstCrypto->generateKeystream(); + } + } + } else { + if (m_tekDstEnable && m_tekDstAlgoId != ALGO_UNENCRYPT && m_tekDstKeyId != 0U) { + // setup destination crypto + m_p25DstCrypto->generateMI(); + + uint8_t miDst[MI_LENGTH_BYTES]; + ::memset(miDst, 0x00U, MI_LENGTH_BYTES); + m_p25DstCrypto->getMI(miDst); + LogInfoEx(LOG_NET, P25_HDU_STR ", (D) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + miDst[0U], miDst[1U], miDst[2U], miDst[3U], miDst[4U], miDst[5U], miDst[6U], miDst[7U], miDst[8U]); + + m_p25DstCrypto->generateKeystream(); + } + } + } + } + } else { + // is the incoming call encrypted? + if (m_callAlgoId != ALGO_UNENCRYPT) { + if (m_tekSrcEnable && m_tekSrcAlgoId != ALGO_UNENCRYPT && m_tekSrcKeyId != 0U) { uint8_t mi[MI_LENGTH_BYTES]; ::memset(mi, 0x00U, MI_LENGTH_BYTES); for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { mi[i] = buffer[184U + i]; } - if (reverseEncrypt) { - m_p25DstCrypto->setMI(mi); - m_p25DstCrypto->generateKeystream(); - } else { - m_p25SrcCrypto->setMI(mi); - m_p25SrcCrypto->generateKeystream(); - } + LogInfoEx(LOG_NET, P25_HDU_STR ", (S) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + + m_p25SrcCrypto->setMI(mi); + m_p25SrcCrypto->generateKeystream(); } } + + // if this is a one-way patch, and the destination is encrypted, prepare the destination crypto + if (m_tekDstEnable && m_tekDstAlgoId != ALGO_UNENCRYPT && m_tekDstKeyId != 0U) { + m_p25DstCrypto->generateMI(); + + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + m_p25DstCrypto->getMI(mi); + + LogInfoEx(LOG_NET, P25_HDU_STR ", (D) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + + m_p25DstCrypto->generateKeystream(); + } } uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); m_rxStartTime = now; + m_callDstId = actualDstId; + LogInfoEx(LOG_HOST, "P25, call start, srcId = %u, dstId = %u", srcId, dstId); if (m_grantDemand) { @@ -1065,59 +1212,29 @@ void HostPatch::processP25Network(uint8_t* buffer, uint32_t length) } } - if ((duid == DUID::TDU) || (duid == DUID::TDULC)) { - // ignore TDU's that are grant demands - if (grantDemand) { - m_network->resetP25(); - return; - } - - p25::lc::LC lc = p25::lc::LC(); - lc.setLCO(P25DEF::LCO::GROUP); - lc.setDstId(actualDstId); - lc.setSrcId(srcId); - - p25::data::LowSpeedData lsd = p25::data::LowSpeedData(); - - LogInfoEx(LOG_HOST, P25_TDU_STR); - - if (m_mmdvmP25Reflector) { - m_mmdvmP25Net->writeTDU(); - } - else { - uint8_t controlByte = 0x00U; - m_network->writeP25TDU(lc, lsd, controlByte); - } - - if (m_rxStartTime > 0U) { - uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - uint64_t diff = now - m_rxStartTime; + m_rxStreamId = m_network->getP25StreamId(); + m_callDropTime.start(); - LogInfoEx(LOG_HOST, "P25, call end, srcId = %u, dstId = %u, dur = %us", srcId, dstId, diff / 1000U); - } + uint8_t* netLDU = new uint8_t[9U * 25U]; + ::memset(netLDU, 0x00U, 9U * 25U); - m_rxStartTime = 0U; - m_rxStreamId = 0U; + if (m_debug) + { + // dump encryption MI's + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + m_p25SrcCrypto->getMI(mi); - m_callInProgress = false; - m_callAlgoId = ALGO_UNENCRYPT; - m_rxStartTime = 0U; - m_rxStreamId = 0U; + LogInfoEx(LOG_NET, "Crypto, (S) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); - m_p25SrcCrypto->clearMI(); - m_p25SrcCrypto->resetKeystream(); - m_p25DstCrypto->clearMI(); - m_p25DstCrypto->resetKeystream(); + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + m_p25DstCrypto->getMI(mi); - m_network->resetP25(); - return; + LogInfoEx(LOG_NET, "Crypto, (D) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); } - m_rxStreamId = m_network->getP25StreamId(); - - uint8_t* netLDU = new uint8_t[9U * 25U]; - ::memset(netLDU, 0x00U, 9U * 25U); - int count = 0; switch (duid) { @@ -1168,54 +1285,61 @@ void HostPatch::processP25Network(uint8_t* buffer, uint32_t length) LogInfoEx(LOG_NET, P25_LDU1_STR " audio, srcId = %u, dstId = %u", srcId, dstId); - if (tekEnable && tekAlgoId != ALGO_UNENCRYPT && tekKeyId != 0U) { - cryptP25AudioFrame(netLDU, reverseEncrypt, 1U); - } else { - if (!m_twoWayPatch && m_tekDstEnable && m_tekDstAlgoId != ALGO_UNENCRYPT && m_tekDstKeyId != 0U) { - // for one-way patches, if the destination TEK is enabled, use it - cryptP25AudioFrame(netLDU, false, 1U); - } - } - control = lc::LC(*dfsiLC.control()); control.setSrcId(srcId); control.setDstId(actualDstId); - // if this is the beginning of a call and we have a valid HDU frame, extract the algo ID - if (frameType == FrameType::HDU_VALID) { - uint8_t algoId = buffer[181U]; - if (algoId != ALGO_UNENCRYPT) { - uint16_t kid = GET_UINT16(buffer, 182U); - - uint8_t mi[MI_LENGTH_BYTES]; - ::memset(mi, 0x00U, MI_LENGTH_BYTES); - for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { - mi[i] = buffer[184U + i]; + // is this a two-way patch? + if (m_twoWayPatch) { + // perform cross-encryption if needed + if (!skipCrypto) { + if (reverseCall) { + if (m_debug) + LogDebug(LOG_NET, "P25, cross-encrypting LDU1 audio, decrypt using destination TEK ($%04X), encrypt using source TEK ($%04X)", m_tekDstKeyId, m_tekSrcKeyId); + cryptP25AudioFrame(netLDU, reverseCall, 1U); + } else { + if (m_debug) + LogDebug(LOG_NET, "P25, cross-encrypting LDU1 audio, decrypt using source TEK ($%04X), encrypt using destination TEK ($%04X)", m_tekSrcKeyId, m_tekDstKeyId); + cryptP25AudioFrame(netLDU, reverseCall, 1U); } - - control.setAlgId(algoId); - control.setKId(kid); - control.setMI(mi); } - } - // the previous is nice and all -- but if we're cross-encrypting, we need to use the TEK - if (tekEnable && tekAlgoId != ALGO_UNENCRYPT && tekKeyId != 0U) { - control.setAlgId(tekAlgoId); - control.setKId(tekKeyId); + // set the algo ID and key ID + if (reverseCall) { + if (m_tekSrcEnable) { + control.setAlgId(m_tekSrcAlgoId); + control.setKId(m_tekSrcKeyId); - uint8_t mi[MI_LENGTH_BYTES]; - ::memset(mi, 0x00U, MI_LENGTH_BYTES); - if (!reverseEncrypt) - m_p25SrcCrypto->getMI(mi); - else - m_p25DstCrypto->getMI(mi); + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + m_p25SrcCrypto->getMI(mi); + + control.setMI(mi); + } else { + control.setAlgId(ALGO_UNENCRYPT); + control.setKId(0U); + } + } else { + if (m_tekDstEnable) { + control.setAlgId(m_tekDstAlgoId); + control.setKId(m_tekDstKeyId); - control.setMI(mi); + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + m_p25DstCrypto->getMI(mi); + + control.setMI(mi); + } else { + control.setAlgId(ALGO_UNENCRYPT); + control.setKId(0U); + } + } } else { - if (!m_twoWayPatch && m_tekDstEnable && m_tekDstAlgoId != ALGO_UNENCRYPT && m_tekDstKeyId != 0U) { + if (m_tekDstEnable && m_tekDstAlgoId != ALGO_UNENCRYPT && m_tekDstKeyId != 0U) { // for one-way patches, if the destination TEK is enabled, use it + cryptP25AudioFrame(netLDU, false, 1U); + control.setAlgId(m_tekDstAlgoId); control.setKId(m_tekDstKeyId); @@ -1224,9 +1348,20 @@ void HostPatch::processP25Network(uint8_t* buffer, uint32_t length) m_p25DstCrypto->getMI(mi); control.setMI(mi); + } else { + if (m_tekSrcEnable && m_tekSrcAlgoId != ALGO_UNENCRYPT && m_tekSrcKeyId != 0U) { + // for one-way patches, if the source TEK is enabled, use it to decrypt + cryptP25AudioFrame(netLDU, false, 1U); + } + + control.setAlgId(ALGO_UNENCRYPT); + control.setKId(0U); } } + if (m_debug) + LogDebug(LOG_NET, P25_LDU1_STR ", algoId = $%02X, kId = $%04X, reverseCall = %u", control.getAlgId(), control.getKId(), reverseCall); + if (m_mmdvmP25Reflector) { ::memcpy(m_netLDU1, netLDU, 9U * 25U); m_gotNetLDU1 = true; @@ -1285,36 +1420,122 @@ void HostPatch::processP25Network(uint8_t* buffer, uint32_t length) LogInfoEx(LOG_NET, P25_LDU2_STR " audio, algo = $%02X, kid = $%04X", dfsiLC.control()->getAlgId(), dfsiLC.control()->getKId()); - if (tekEnable && tekAlgoId != ALGO_UNENCRYPT && tekKeyId != 0U) { - cryptP25AudioFrame(netLDU, reverseEncrypt, 2U); - } else { - if (!m_twoWayPatch && m_tekDstEnable && m_tekDstAlgoId != ALGO_UNENCRYPT && m_tekDstKeyId != 0U) { - // for one-way patches, if the destination TEK is enabled, use it - cryptP25AudioFrame(netLDU, false, 2U); - } - } - control = lc::LC(*dfsiLC.control()); control.setSrcId(srcId); control.setDstId(actualDstId); - // set the algo ID and key ID - if (tekEnable && tekAlgoId != ALGO_UNENCRYPT && tekKeyId != 0U) { - control.setAlgId(tekAlgoId); - control.setKId(tekKeyId); + // is this a two-way patch? + if (m_twoWayPatch) { + // perform cross-encryption if needed + if (!skipCrypto) { + if (reverseCall) { + if (m_debug) + LogDebug(LOG_NET, "P25, cross-encrypting LDU2 audio, decrypt using destination TEK ($%04X), encrypt using source TEK ($%04X)", m_tekDstKeyId, m_tekSrcKeyId); + cryptP25AudioFrame(netLDU, reverseCall, 2U); + + // update destination crypto + if (m_tekDstEnable) { + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + control.getMI(mi); + m_p25DstCrypto->setMI(mi); + m_p25DstCrypto->generateKeystream(); + + LogInfoEx(LOG_NET, P25_LDU2_STR ", (D) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + } - uint8_t mi[MI_LENGTH_BYTES]; - ::memset(mi, 0x00U, MI_LENGTH_BYTES); - if (!reverseEncrypt) - m_p25SrcCrypto->getMI(mi); - else - m_p25DstCrypto->getMI(mi); + if (m_tekSrcEnable) { + // setup source crypto + m_p25SrcCrypto->generateNextMI(); + + // generate new keystream + m_p25SrcCrypto->generateKeystream(); + } + } else { + if (m_debug) + LogDebug(LOG_NET, "P25, cross-encrypting LDU2 audio, decrypt using source TEK ($%04X), encrypt using destination TEK ($%04X)", m_tekSrcKeyId, m_tekDstKeyId); + cryptP25AudioFrame(netLDU, reverseCall, 2U); + + // update source crypto + if (m_tekSrcEnable) { + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + control.getMI(mi); + m_p25SrcCrypto->setMI(mi); + m_p25SrcCrypto->generateKeystream(); + + LogInfoEx(LOG_NET, P25_LDU2_STR ", (S) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + } + + if (m_tekDstEnable) { + // setup destination crypto + m_p25DstCrypto->generateNextMI(); - control.setMI(mi); + // generate new keystream + m_p25DstCrypto->generateKeystream(); + } + } + } + + // set the algo ID and key ID + if (reverseCall) { + if (m_tekSrcEnable) { + control.setAlgId(m_tekSrcAlgoId); + control.setKId(m_tekSrcKeyId); + + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + m_p25SrcCrypto->getMI(mi); + + LogInfoEx(LOG_NET, P25_LDU2_STR ", (S) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + + control.setMI(mi); + } else { + control.setAlgId(ALGO_UNENCRYPT); + control.setKId(0U); + } + } else { + if (m_tekDstEnable) { + control.setAlgId(m_tekDstAlgoId); + control.setKId(m_tekDstKeyId); + + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + m_p25DstCrypto->getMI(mi); + + LogInfoEx(LOG_NET, P25_LDU2_STR ", (D) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + + control.setMI(mi); + } else { + control.setAlgId(ALGO_UNENCRYPT); + control.setKId(0U); + } + } } else { - if (!m_twoWayPatch && m_tekDstEnable && m_tekDstAlgoId != ALGO_UNENCRYPT && m_tekDstKeyId != 0U) { + if (m_tekDstEnable && m_tekDstAlgoId != ALGO_UNENCRYPT && m_tekDstKeyId != 0U) { // for one-way patches, if the destination TEK is enabled, use it + cryptP25AudioFrame(netLDU, false, 2U); + + // update source crypto + if (m_tekSrcEnable) { + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + control.getMI(mi); + m_p25SrcCrypto->setMI(mi); + m_p25SrcCrypto->generateKeystream(); + } + + // setup destination crypto + m_p25DstCrypto->generateNextMI(); + + // generate new keystream + m_p25DstCrypto->generateKeystream(); + control.setAlgId(m_tekDstAlgoId); control.setKId(m_tekDstKeyId); @@ -1322,10 +1543,34 @@ void HostPatch::processP25Network(uint8_t* buffer, uint32_t length) ::memset(mi, 0x00U, MI_LENGTH_BYTES); m_p25DstCrypto->getMI(mi); + LogInfoEx(LOG_NET, P25_LDU2_STR ", (D) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + control.setMI(mi); + } else { + if (m_tekSrcEnable && m_tekSrcAlgoId != ALGO_UNENCRYPT && m_tekSrcKeyId != 0U) { + // for one-way patches, if the source TEK is enabled, use it to decrypt + cryptP25AudioFrame(netLDU, false, 2U); + + // update source crypto + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + control.getMI(mi); + m_p25SrcCrypto->setMI(mi); + m_p25SrcCrypto->generateKeystream(); + + LogInfoEx(LOG_NET, P25_LDU2_STR ", (S) Enc Sync, MI = %02X %02X %02X %02X %02X %02X %02X %02X %02X", + mi[0U], mi[1U], mi[2U], mi[3U], mi[4U], mi[5U], mi[6U], mi[7U], mi[8U]); + } + + control.setAlgId(ALGO_UNENCRYPT); + control.setKId(0U); } } + if (m_debug) + LogDebug(LOG_NET, P25_LDU2_STR ", algoId = $%02X, kId = $%04X, reverseCall = %u", control.getAlgId(), control.getKId(), reverseCall); + if (m_mmdvmP25Reflector) { ::memcpy(m_netLDU2, netLDU, 9U * 25U); m_gotNetLDU2 = true; @@ -1352,6 +1597,86 @@ void HostPatch::processP25Network(uint8_t* buffer, uint32_t length) } } +/* Helper to reset DMR call state. */ + +void HostPatch::resetDMRCall(uint32_t srcId, uint8_t slotNo) +{ + bool stuckTermination = m_callDropTime.isRunning() && m_callDropTime.hasExpired(); + + m_network->resetDMR(slotNo); + + if (m_rxStartTime > 0U) { + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + uint64_t diff = now - m_rxStartTime; + + if (stuckTermination) + LogInfoEx(LOG_HOST, "DMR, call end (T), srcId = %u, dstId = %u, dur = %us", srcId, m_callDstId, diff / 1000U); + else + LogInfoEx(LOG_HOST, "DMR, call end, srcId = %u, dstId = %u, dur = %us", srcId, m_callDstId, diff / 1000U); + } + + m_callInProgress = false; + m_rxStartTime = 0U; + m_rxStreamId = 0U; + + m_callDropTime.stop(); +} + +/* Helper to reset P25 call state. */ + +void HostPatch::resetP25Call(uint32_t srcId) +{ + bool stuckTermination = m_callDropTime.isRunning() && m_callDropTime.hasExpired(); + + using namespace p25; + using namespace p25::defines; + using namespace p25::dfsi::defines; + + p25::lc::LC lc = p25::lc::LC(); + lc.setLCO(P25DEF::LCO::GROUP); + lc.setDstId(m_callDstId); + lc.setSrcId(srcId); + + p25::data::LowSpeedData lsd = p25::data::LowSpeedData(); + + LogInfoEx(LOG_HOST, P25_TDU_STR); + + if (m_mmdvmP25Reflector) { + m_mmdvmP25Net->writeTDU(); + } + else { + uint8_t controlByte = 0x00U; + m_network->writeP25TDU(lc, lsd, controlByte); + } + + if (m_rxStartTime > 0U) { + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + uint64_t diff = now - m_rxStartTime; + + if (stuckTermination) + LogInfoEx(LOG_HOST, "P25, call end (T), srcId = %u, dstId = %u, dur = %us", srcId, m_callDstId, diff / 1000U); + else + LogInfoEx(LOG_HOST, "P25, call end, srcId = %u, dstId = %u, dur = %us", srcId, m_callDstId, diff / 1000U); + } + + m_rxStartTime = 0U; + m_rxStreamId = 0U; + + m_callInProgress = false; + m_callAlgoId = ALGO_UNENCRYPT; + m_rxStartTime = 0U; + m_rxStreamId = 0U; + + m_p25SrcCrypto->clearMI(); + m_p25SrcCrypto->resetKeystream(); + m_p25DstCrypto->clearMI(); + m_p25DstCrypto->resetKeystream(); + + m_callDropTime.stop(); + + m_network->resetP25(); +} + /* Helper to cross encrypt P25 network traffic audio frames. */ void HostPatch::cryptP25AudioFrame(uint8_t* ldu, bool reverseEncrypt, uint8_t p25N) @@ -1360,21 +1685,22 @@ void HostPatch::cryptP25AudioFrame(uint8_t* ldu, bool reverseEncrypt, uint8_t p2 using namespace p25; using namespace p25::defines; + if (!m_tekSrcEnable && !m_tekDstEnable) + return; + uint8_t tekSrcAlgoId = m_tekSrcAlgoId; uint16_t tekSrcKeyId = m_tekSrcKeyId; uint8_t tekDstAlgoId = m_tekDstAlgoId; uint16_t tekDstKeyId = m_tekDstKeyId; - if (reverseEncrypt) { - tekSrcAlgoId = m_tekDstAlgoId; - tekSrcKeyId = m_tekDstKeyId; - tekDstAlgoId = m_tekSrcAlgoId; - tekDstKeyId = m_tekSrcKeyId; - } + //LogDebugEx(LOG_HOST, "HostPatch::cryptP25AudioFrame()", "p25N = %u, srcAlgoId = $%02X, srcKeyId = $%04X, dstAlgoId = $%02X, dstKeyId = $%04X, reverseEncrypt = %u", p25N, + // m_p25SrcCrypto->getTEKAlgoId(), m_p25SrcCrypto->getTEKKeyId(), m_p25DstCrypto->getTEKAlgoId(), m_p25DstCrypto->getTEKKeyId(), reverseEncrypt); - // decode 9 IMBE codewords into PCM samples + // process 9 IMBE codewords for (int n = 0; n < 9; n++) { uint8_t imbe[RAW_IMBE_LENGTH_BYTES]; + + // extract IMBE codeword n switch (n) { case 0: ::memcpy(imbe, ldu + 10U, RAW_IMBE_LENGTH_BYTES); @@ -1407,9 +1733,14 @@ void HostPatch::cryptP25AudioFrame(uint8_t* ldu, bool reverseEncrypt, uint8_t p2 // Utils::dump(1U, "P25, HostPatch::cryptP25AudioFrame(), IMBE", imbe, RAW_IMBE_LENGTH_BYTES); - // first -- decrypt the IMBE codeword - if (tekSrcAlgoId != P25DEF::ALGO_UNENCRYPT && tekSrcKeyId > 0U) { - if (!reverseEncrypt && m_p25SrcCrypto->getTEKLength() > 0U) { + /* + ** Stage 1 -- decrypt the IMBE codeword + */ + + if (!reverseEncrypt && tekSrcAlgoId != P25DEF::ALGO_UNENCRYPT && tekSrcKeyId > 0U) { + if (m_p25SrcCrypto->getTEKLength() > 0U) { + if (m_debug) + LogDebugEx(LOG_HOST, "HostPatch::cryptP25AudioFrame()", "decrypting (S) IMBE codeword, n = %u, algoId = $%02X, kId = $%04X, reverseEncrypt = %u", n, m_p25SrcCrypto->getTEKAlgoId(), m_p25SrcCrypto->getTEKKeyId(), reverseEncrypt); switch (tekSrcAlgoId) { case P25DEF::ALGO_AES_256: m_p25SrcCrypto->cryptAES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); @@ -1424,29 +1755,40 @@ void HostPatch::cryptP25AudioFrame(uint8_t* ldu, bool reverseEncrypt, uint8_t p2 LogError(LOG_HOST, "Unsupported TEK algorithm, tekAlgoId = $%02X", tekSrcAlgoId); break; } - } else { - if (reverseEncrypt && m_p25DstCrypto->getTEKLength() > 0U) { - switch (tekDstAlgoId) { - case P25DEF::ALGO_AES_256: - m_p25DstCrypto->cryptAES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); - break; - case P25DEF::ALGO_ARC4: - m_p25DstCrypto->cryptARC4_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); - break; - case P25DEF::ALGO_DES: - m_p25DstCrypto->cryptDES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); - break; - default: - LogError(LOG_HOST, "Unsupported TEK algorithm, tekAlgoId = $%02X", tekDstAlgoId); - break; - } + } + } + + if (reverseEncrypt && tekDstAlgoId != P25DEF::ALGO_UNENCRYPT && tekDstKeyId > 0U) { + if (m_p25DstCrypto->getTEKLength() > 0U) { + if (m_debug) + LogDebugEx(LOG_HOST, "HostPatch::cryptP25AudioFrame()", "decrypting (D) IMBE codeword, n = %u, algoId = $%02X, kId = $%04X, reverseEncrypt = %u", n, m_p25DstCrypto->getTEKAlgoId(), m_p25DstCrypto->getTEKKeyId(), reverseEncrypt); + switch (tekDstAlgoId) { + case P25DEF::ALGO_AES_256: + m_p25DstCrypto->cryptAES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); + break; + case P25DEF::ALGO_ARC4: + m_p25DstCrypto->cryptARC4_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); + break; + case P25DEF::ALGO_DES: + m_p25DstCrypto->cryptDES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); + break; + default: + LogError(LOG_HOST, "Unsupported TEK algorithm, tekAlgoId = $%02X", tekDstAlgoId); + break; } } } - // second -- reencrypt the IMBE codeword - if (tekDstAlgoId != P25DEF::ALGO_UNENCRYPT && tekDstKeyId > 0U) { - if (!reverseEncrypt && m_p25DstCrypto->getTEKLength() > 0U) { + // Utils::dump(1U, "P25, HostPatch::cryptP25AudioFrame(), Decrypted IMBE", imbe, RAW_IMBE_LENGTH_BYTES); + + /* + ** Stage 2 -- (re-)encrypt the IMBE codeword + */ + + if (!reverseEncrypt && tekDstAlgoId != P25DEF::ALGO_UNENCRYPT && tekDstKeyId > 0U) { + if (m_p25DstCrypto->getTEKLength() > 0U) { + if (m_debug) + LogDebugEx(LOG_HOST, "HostPatch::cryptP25AudioFrame()", "encrypting (D) IMBE codeword, n = %u, algoId = $%02X, kId = $%04X, reverseEncrypt = %u", n, m_p25DstCrypto->getTEKAlgoId(), m_p25DstCrypto->getTEKKeyId(), reverseEncrypt); switch (tekDstAlgoId) { case P25DEF::ALGO_AES_256: m_p25DstCrypto->cryptAES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); @@ -1461,25 +1803,62 @@ void HostPatch::cryptP25AudioFrame(uint8_t* ldu, bool reverseEncrypt, uint8_t p2 LogError(LOG_HOST, "Unsupported TEK algorithm, tekAlgoId = $%02X", tekDstAlgoId); break; } - } else { - if (reverseEncrypt && m_p25SrcCrypto->getTEKLength() > 0U) { - switch (tekSrcAlgoId) { - case P25DEF::ALGO_AES_256: - m_p25SrcCrypto->cryptAES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); - break; - case P25DEF::ALGO_ARC4: - m_p25SrcCrypto->cryptARC4_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); - break; - case P25DEF::ALGO_DES: - m_p25SrcCrypto->cryptDES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); - break; - default: - LogError(LOG_HOST, "Unsupported TEK algorithm, tekAlgoId = $%02X", tekSrcAlgoId); - break; - } + } + } + + if (reverseEncrypt && tekSrcAlgoId != P25DEF::ALGO_UNENCRYPT && tekSrcKeyId > 0U) { + if (m_p25SrcCrypto->getTEKLength() > 0U) { + if (m_debug) + LogDebugEx(LOG_HOST, "HostPatch::cryptP25AudioFrame()", "encrypting (S) IMBE codeword, n = %u, algoId = $%02X, kId = $%04X, reverseEncrypt = %u", n, m_p25SrcCrypto->getTEKAlgoId(), m_p25SrcCrypto->getTEKKeyId(), reverseEncrypt); + switch (tekSrcAlgoId) { + case P25DEF::ALGO_AES_256: + m_p25SrcCrypto->cryptAES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); + break; + case P25DEF::ALGO_ARC4: + m_p25SrcCrypto->cryptARC4_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); + break; + case P25DEF::ALGO_DES: + m_p25SrcCrypto->cryptDES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); + break; + default: + LogError(LOG_HOST, "Unsupported TEK algorithm, tekAlgoId = $%02X", tekSrcAlgoId); + break; } } } + + // Utils::dump(1U, "P25, HostPatch::cryptP25AudioFrame(), Encrypted IMBE", imbe, RAW_IMBE_LENGTH_BYTES); + + // store the processed IMBE codeword back into the LDU + switch (n) { + case 0: + ::memcpy(ldu + 10U, imbe, RAW_IMBE_LENGTH_BYTES); + break; + case 1: + ::memcpy(ldu + 26U, imbe, RAW_IMBE_LENGTH_BYTES); + break; + case 2: + ::memcpy(ldu + 55U, imbe, RAW_IMBE_LENGTH_BYTES); + break; + case 3: + ::memcpy(ldu + 80U, imbe, RAW_IMBE_LENGTH_BYTES); + break; + case 4: + ::memcpy(ldu + 105U, imbe, RAW_IMBE_LENGTH_BYTES); + break; + case 5: + ::memcpy(ldu + 130U, imbe, RAW_IMBE_LENGTH_BYTES); + break; + case 6: + ::memcpy(ldu + 155U, imbe, RAW_IMBE_LENGTH_BYTES); + break; + case 7: + ::memcpy(ldu + 180U, imbe, RAW_IMBE_LENGTH_BYTES); + break; + case 8: + ::memcpy(ldu + 204U, imbe, RAW_IMBE_LENGTH_BYTES); + break; + } } } diff --git a/src/patch/HostPatch.h b/src/patch/HostPatch.h index 9c3bf237..74b65cbf 100644 --- a/src/patch/HostPatch.h +++ b/src/patch/HostPatch.h @@ -79,6 +79,9 @@ private: bool m_mmdvmP25Reflector; mmdvm::P25Network* m_mmdvmP25Net; + uint16_t m_dropTimeMS; + Timer m_callDropTime; + RPT_NET_STATE m_netState; p25::lc::LC m_netLC; bool m_gotNetLDU1; @@ -95,6 +98,8 @@ private: bool m_grantDemand; bool m_callInProgress; + uint32_t m_callDstId; + uint8_t m_callSlotNo; uint8_t m_callAlgoId; uint64_t m_rxStartTime; uint32_t m_rxStreamId; @@ -150,6 +155,18 @@ private: */ void processP25Network(uint8_t* buffer, uint32_t length); + /** + * @brief Helper to reset DMR call state. + * @param srcId Source ID. + * @param slotNo DMR slot. + */ + void resetDMRCall(uint32_t srcId, uint8_t slotNo); + /** + * @brief Helper to reset P25 call state. + * @param srcId Source ID. + */ + void resetP25Call(uint32_t srcId); + /** * @brief Helper to cross encrypt P25 network traffic audio frames. * @param ldu