diff --git a/src/bridge/HostBridge.cpp b/src/bridge/HostBridge.cpp index 3e55971d..0b4b0def 100644 --- a/src/bridge/HostBridge.cpp +++ b/src/bridge/HostBridge.cpp @@ -22,8 +22,6 @@ #include "common/p25/P25Utils.h" #include "common/network/RTPHeader.h" #include "common/network/udp/Socket.h" -#include "common/AESCrypto.h" -#include "common/RC4Crypto.h" #include "common/Log.h" #include "common/StopWatch.h" #include "common/Thread.h" @@ -302,12 +300,8 @@ HostBridge::HostBridge(const std::string& confFile) : m_udpUsrp(false), m_tekAlgoId(p25::defines::ALGO_UNENCRYPT), m_tekKeyId(0U), - m_tek(nullptr), - m_tekLength(0U), m_requestedTek(false), - m_keystream(nullptr), - m_keystreamPos(0U), - m_mi(nullptr), + m_p25Crypto(nullptr), m_srcId(p25::defines::WUID_FNE), m_srcIdOverride(0U), m_overrideSrcIdFromMDC(false), @@ -370,8 +364,7 @@ HostBridge::HostBridge(const std::string& confFile) : m_debug(false), m_rtpSeqNo(0U), m_rtpTimestamp(INVALID_TS), - m_usrpSeqNo(0U), - m_random() + m_usrpSeqNo(0U) #if defined(_WIN32) , m_decoderState(nullptr), @@ -402,12 +395,7 @@ HostBridge::HostBridge(const std::string& confFile) : ::memset(m_netLDU1, 0x00U, 9U * 25U); ::memset(m_netLDU2, 0x00U, 9U * 25U); - m_mi = new uint8_t[p25::defines::MI_LENGTH_BYTES]; - ::memset(m_mi, 0x00U, p25::defines::MI_LENGTH_BYTES); - - std::random_device rd; - std::mt19937 mt(rd()); - m_random = mt; + m_p25Crypto = new p25::crypto::P25Crypto(); } /* Finalizes a instance of the HostBridge class. */ @@ -417,9 +405,7 @@ HostBridge::~HostBridge() delete[] m_ambeBuffer; delete[] m_netLDU1; delete[] m_netLDU2; - if (m_keystream != nullptr) - delete[] m_keystream; - delete[] m_mi; + delete m_p25Crypto; } /* Executes the main FNE processing loop. */ @@ -2032,12 +2018,14 @@ void HostBridge::processP25Network(uint8_t* buffer, uint32_t length) LogWarning(LOG_HOST, "P25, call ignored, using different encryption parameters, callAlgoId = $%02X, callKID = $%04X, tekAlgoId = $%02X, tekKID = $%04X", m_callAlgoId, callKID, m_tekAlgoId, m_tekKeyId); return; } else { - ::memset(m_mi, 0x00U, MI_LENGTH_BYTES); + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { - m_mi[i] = buffer[184U + i]; + mi[i] = buffer[184U + i]; } - generateKeystream(); + m_p25Crypto->setMI(mi); + m_p25Crypto->generateKeystream(); } } } @@ -2220,10 +2208,13 @@ void HostBridge::processP25Network(uint8_t* buffer, uint32_t length) // copy out the MI for the next super frame if (dfsiLC.control()->getAlgId() == m_tekAlgoId && dfsiLC.control()->getKId() == m_tekKeyId) { - dfsiLC.control()->getMI(m_mi); - generateKeystream(); + uint8_t mi[MI_LENGTH_BYTES]; + dfsiLC.control()->getMI(mi); + + m_p25Crypto->setMI(mi); + m_p25Crypto->generateKeystream(); } else { - ::memset(m_mi, 0x00U, MI_LENGTH_BYTES); + m_p25Crypto->clearMI(); } } break; @@ -2287,13 +2278,13 @@ void HostBridge::decodeP25AudioFrame(uint8_t* ldu, uint32_t srcId, uint32_t dstI // Utils::dump(1U, "IMBE", imbe, RAW_IMBE_LENGTH_BYTES); - if (m_tekAlgoId != p25::defines::ALGO_UNENCRYPT && m_tekKeyId > 0U && m_tek != nullptr) { + if (m_tekAlgoId != p25::defines::ALGO_UNENCRYPT && m_tekKeyId > 0U && m_p25Crypto->getTEKLength() > 0U) { switch (m_tekAlgoId) { case p25::defines::ALGO_AES_256: - cryptAES_P25IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); + m_p25Crypto->cryptAES_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); break; case p25::defines::ALGO_ARC4: - cryptARC4_P25IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); + m_p25Crypto->cryptARC4_IMBE(imbe, (p25N == 1U) ? DUID::LDU1 : DUID::LDU2); break; default: LogError(LOG_HOST, "unsupported TEK algorithm, tekAlgoId = $%02X", m_tekAlgoId); @@ -2488,32 +2479,22 @@ void HostBridge::encodeP25AudioFrame(uint8_t* pcm, uint32_t forcedSrcId, uint32_ // Utils::dump(1U, "Encoded IMBE", imbe, RAW_IMBE_LENGTH_BYTES); - if (m_tekAlgoId != p25::defines::ALGO_UNENCRYPT && m_tekKeyId > 0U && m_tek != nullptr && m_mi != nullptr) { + if (m_tekAlgoId != p25::defines::ALGO_UNENCRYPT && m_tekKeyId > 0U && m_p25Crypto->getTEKLength() > 0U) { // generate initial MI for the HDU - if (m_p25N == 0U && m_keystream == nullptr) { - bool hasMI = false; - for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { - if (m_mi[i] != 0x00U) - hasMI = true; - } - - if (!hasMI) { - for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { - std::uniform_int_distribution dist(0x00U, 0xFFU); - m_mi[i] = (uint8_t)dist(m_random); - } - - generateKeystream(); + if (m_p25N == 0U && !m_p25Crypto->hasValidKeystream()) { + if (!m_p25Crypto->hasValidMI()) { + m_p25Crypto->generateMI(); + m_p25Crypto->generateKeystream(); } } // perform crypto switch (m_tekAlgoId) { case p25::defines::ALGO_AES_256: - cryptAES_P25IMBE(imbe, (m_p25N < 9U) ? DUID::LDU1 : DUID::LDU2); + m_p25Crypto->cryptAES_IMBE(imbe, (m_p25N < 9U) ? DUID::LDU1 : DUID::LDU2); break; case p25::defines::ALGO_ARC4: - cryptARC4_P25IMBE(imbe, (m_p25N < 9U) ? DUID::LDU1 : DUID::LDU2); + m_p25Crypto->cryptARC4_IMBE(imbe, (m_p25N < 9U) ? DUID::LDU1 : DUID::LDU2); break; default: LogError(LOG_HOST, "unsupported TEK algorithm, tekAlgoId = $%02X", m_tekAlgoId); @@ -2522,14 +2503,10 @@ void HostBridge::encodeP25AudioFrame(uint8_t* pcm, uint32_t forcedSrcId, uint32_ // if we're on the last block of the LDU2 -- generate the next MI if (m_p25N == 17U) { - uint8_t nextMI[MI_LENGTH_BYTES]; - ::memset(nextMI, 0x00U, MI_LENGTH_BYTES); - - getNextMI(m_mi, nextMI); - ::memcpy(m_mi, nextMI, MI_LENGTH_BYTES); + m_p25Crypto->generateNextMI(); // generate new keystream - generateKeystream(); + m_p25Crypto->generateKeystream(); } } @@ -2614,7 +2591,10 @@ void HostBridge::encodeP25AudioFrame(uint8_t* pcm, uint32_t forcedSrcId, uint32_ lc.setAlgId(m_tekAlgoId); lc.setKId(m_tekKeyId); - lc.setMI(m_mi); + + uint8_t mi[MI_LENGTH_BYTES]; + m_p25Crypto->getMI(mi); + lc.setMI(mi); data::LowSpeedData lsd = data::LowSpeedData(); @@ -2800,12 +2780,8 @@ void HostBridge::callEnd(uint32_t srcId, uint32_t dstId) m_rtpSeqNo = 0U; m_rtpTimestamp = INVALID_TS; - ::memset(m_mi, 0x00U, p25::defines::MI_LENGTH_BYTES); - if (m_keystream != nullptr) { - delete[] m_keystream; - m_keystream = nullptr; - m_keystreamPos = 0U; - } + m_p25Crypto->clearMI(); + m_p25Crypto->resetKeystream(); } /* Helper to process a FNE KMM TEK response. */ @@ -2817,208 +2793,17 @@ void HostBridge::processTEKResponse(p25::kmm::KeyItem* ki, uint8_t algId, uint8_ if (algId == m_tekAlgoId && ki->kId() == m_tekKeyId) { LogMessage(LOG_HOST, "TEK loaded, algId = $%02X, kId = $%04X, sln = $%04X", algId, ki->kId(), ki->sln()); - m_tek = std::make_unique(keyLength); - ki->getKey(m_tek.get()); - m_tekLength = keyLength; - } - else - { - m_tekAlgoId = p25::defines::ALGO_UNENCRYPT; - m_tekKeyId = 0U; - m_tekLength = 0U; - } -} - -/* Given the last MI, generate the next MI using LFSR. */ - -void HostBridge::getNextMI(uint8_t lastMI[9U], uint8_t nextMI[9U]) -{ - uint8_t carry, i; - std::copy(lastMI, lastMI + 9, nextMI); - - for (uint8_t cycle = 0; cycle < 64; cycle++) { - // calculate bit 0 for the next cycle - carry = ((nextMI[0] >> 7) ^ (nextMI[0] >> 5) ^ (nextMI[2] >> 5) ^ - (nextMI[3] >> 5) ^ (nextMI[4] >> 2) ^ (nextMI[6] >> 6)) & - 0x01; + UInt8Array tek = std::make_unique(keyLength); + ki->getKey(tek.get()); - // shift all the list elements, except the last one - for (i = 0; i < 7; i++) { - // grab high bit from the next element and use it as our low bit - nextMI[i] = ((nextMI[i] & 0x7F) << 1) | (nextMI[i + 1] >> 7); - } - - // shift last element, then copy the bit 0 we calculated in - nextMI[7] = ((nextMI[i] & 0x7F) << 1) | carry; + m_p25Crypto->setTEKAlgoId(algId); + m_p25Crypto->setTEKKeyId(ki->kId()); + m_p25Crypto->setKey(tek.get(), keyLength); } -} - -/* Helper to generate the encryption keystream. */ - -void HostBridge::generateKeystream() -{ - using namespace p25::defines; - using namespace crypto; - - if (m_tek == nullptr) - return; - if (m_tekLength == 0U) - return; - if (m_mi == nullptr) - return; - - m_keystreamPos = 0U; - - // generate keystream - switch (m_tekAlgoId) { - case p25::defines::ALGO_AES_256: - { - if (m_keystream == nullptr) - m_keystream = new uint8_t[240U]; - ::memset(m_keystream, 0x00U, 240U); - - uint8_t* iv = expandMIToIV(); - - AES aes = AES(AESKeyLength::AES_256); - - uint8_t input[16U]; - ::memset(input, 0x00U, 16U); - ::memcpy(input, iv, 16U); - - for (uint32_t i = 0U; i < (240U / 16U); i++) { - uint8_t* output = aes.encryptECB(input, 16U, m_tek.get()); - ::memcpy(m_keystream + (i * 16U), output, 16U); - ::memcpy(input, output, 16U); - } - - delete[] iv; - } - break; - case p25::defines::ALGO_ARC4: - { - if (m_keystream == nullptr) - m_keystream = new uint8_t[469U]; - ::memset(m_keystream, 0x00U, 469U); - - uint8_t padding = (uint8_t)::fmax(5U - m_tekLength, 0U); - uint8_t adpKey[13U]; - ::memset(adpKey, 0x00U, 13U); - - uint8_t i = 0U; - for (i = 0U; i < padding; i++) - adpKey[i] = 0x00U; - - for (; i < 5U; i++) - adpKey[i] = (m_tekLength > 0U) ? m_tek[i - padding] : 0x00U; - - for (i = 5U; i < 13U; i++) - adpKey[i] = m_mi[i - 5U]; - - // generate ARC4 keystream - RC4 rc4 = RC4(); - m_keystream = rc4.keystream(469U, adpKey, 13U); - } - break; - default: - LogError(LOG_HOST, "unsupported TEK algorithm, algId = $%02X", m_tekAlgoId); - break; - } -} - -/* */ - -uint64_t HostBridge::stepLFSR(uint64_t& lfsr) -{ - uint64_t ovBit = (lfsr >> 63U) & 0x01U; - - // compute feedback bit using polynomial: x^64 + x^62 + x^46 + x^38 + x^27 + x^15 + 1 - uint64_t fbBit = ((lfsr >> 63U) ^ (lfsr >> 61U) ^ (lfsr >> 45U) ^ (lfsr >> 37U) ^ - (lfsr >> 26U) ^ (lfsr >> 14U)) & 0x01U; - - // shift LFSR left and insert feedback bit - lfsr = (lfsr << 1) | fbBit; - return ovBit; -} - -/* Expands the 9-byte MI into a proper 16-byte IV. */ - -uint8_t* HostBridge::expandMIToIV() -{ - // this should never happen... - if (m_mi == nullptr) - return nullptr; - - uint8_t* iv = new uint8_t[16U]; - ::memset(iv, 0x00U, 16U); - - // copy first 64-bits of the MI info LFSR - uint64_t lfsr = 0U; - for (uint8_t i = 0U; i < 8U; i++) { - lfsr = (lfsr << 8U) | m_mi[i]; - } - - uint64_t overflow = 0U; - for (uint8_t i = 0U; i < 64U; i++) { - overflow = (overflow << 1U) | stepLFSR(lfsr); - } - - // copy expansion and LFSR into IV - for (int i = 7; i >= 0; i--) { - iv[i] = (uint8_t)(overflow & 0xFFU); - overflow >>= 8U; - } - - for (int i = 15; i >= 8; i--) { - iv[i] = (uint8_t)(lfsr & 0xFFU); - lfsr >>= 8U; - } - - return iv; -} - -/* Helper to crypt IMBE audio using AES-256. */ - -void HostBridge::cryptAES_P25IMBE(uint8_t* imbe, p25::defines::DUID::E duid) -{ - using namespace p25::defines; - using namespace crypto; - - if (m_keystream == nullptr) - return; - - uint32_t offset = 16U; - if (duid == DUID::LDU2) { - offset += 101U; - } - - offset += (m_keystreamPos * RAW_IMBE_LENGTH_BYTES) + RAW_IMBE_LENGTH_BYTES + ((m_keystreamPos < 8U) ? 0U : 2U); - m_keystreamPos = (m_keystreamPos + 1U) % 9U; - - for (uint8_t i = 0U; i < RAW_IMBE_LENGTH_BYTES; i++) { - imbe[i] ^= m_keystream[offset + i]; - } -} - -/* Helper to crypt IMBE audio using ARC4. */ - -void HostBridge::cryptARC4_P25IMBE(uint8_t* imbe, p25::defines::DUID::E duid) -{ - using namespace p25::defines; - using namespace crypto; - - if (m_keystream == nullptr) - return; - - uint32_t offset = 256U; - if (duid == DUID::LDU2) { - offset += 101U; - } - - offset += (m_keystreamPos * RAW_IMBE_LENGTH_BYTES) + 267U + ((m_keystreamPos < 8U) ? 0U : 2U); - m_keystreamPos = (m_keystreamPos + 1U) % 9U; - - for (uint8_t i = 0U; i < RAW_IMBE_LENGTH_BYTES; i++) { - imbe[i] ^= m_keystream[offset + i]; + else { + m_p25Crypto->setTEKAlgoId(P25DEF::ALGO_UNENCRYPT); + m_p25Crypto->setTEKKeyId(0U); + m_p25Crypto->clearKey(); } } @@ -3211,7 +2996,7 @@ void* HostBridge::threadNetworkProcess(void* arg) if (bridge->m_network->getStatus() == NET_STAT_RUNNING) { if (bridge->m_tekAlgoId != p25::defines::ALGO_UNENCRYPT && bridge->m_tekKeyId > 0U) { - if (bridge->m_tek == nullptr && !bridge->m_requestedTek) { + if (bridge->m_p25Crypto->getTEKLength() == 0U && !bridge->m_requestedTek) { bridge->m_requestedTek = true; LogMessage(LOG_HOST, "Bridge encryption enabled, requesting TEK from network."); bridge->m_network->writeKeyReq(bridge->m_tekKeyId, bridge->m_tekAlgoId); diff --git a/src/bridge/HostBridge.h b/src/bridge/HostBridge.h index 7232c650..d36f8531 100644 --- a/src/bridge/HostBridge.h +++ b/src/bridge/HostBridge.h @@ -20,6 +20,7 @@ #include "common/dmr/data/EmbeddedData.h" #include "common/dmr/lc/LC.h" #include "common/dmr/lc/PrivacyLC.h" +#include "common/p25/Crypto.h" #include "common/network/udp/Socket.h" #include "common/yaml/Yaml.h" #include "common/RingBuffer.h" @@ -35,7 +36,6 @@ #include #include #include -#include #if defined(_WIN32) #define WIN32_LEAN_AND_MEAN @@ -169,13 +169,9 @@ private: uint8_t m_tekAlgoId; uint16_t m_tekKeyId; - UInt8Array m_tek; - uint8_t m_tekLength; bool m_requestedTek; - uint8_t* m_keystream; - uint32_t m_keystreamPos; - uint8_t* m_mi; + p25::crypto::P25Crypto* m_p25Crypto; uint32_t m_srcId; uint32_t m_srcIdOverride; @@ -262,8 +258,6 @@ private: uint32_t m_usrpSeqNo; - std::mt19937 m_random; - static std::mutex m_audioMutex; static std::mutex m_networkMutex; @@ -498,43 +492,6 @@ private: */ void processTEKResponse(p25::kmm::KeyItem* ki, uint8_t algId, uint8_t keyLength); - /** - * @brief Given the last MI, generate the next MI using LFSR. - * @param lastMI Last MI received. - * @param nextMI Next MI. - */ - void getNextMI(uint8_t lastMI[9U], uint8_t nextMI[9U]); - - /** - * @brief Helper to generate the encryption keystream. - */ - void generateKeystream(); - - /** - * @brief - * @param lfsr - * @return uint64_t - */ - uint64_t stepLFSR(uint64_t& lfsr); - /** - * @brief Expands the 9-byte MI into a proper 16-byte IV. - * @return uint8_t* Buffer containing expanded 16-byte IV. - */ - uint8_t* expandMIToIV(); - - /** - * @brief Helper to crypt P25 IMBE audio using AES-256. - * @param imbe Buffer containing IMBE to crypt. - * @param duid P25 DUID. - */ - void cryptAES_P25IMBE(uint8_t* imbe, p25::defines::DUID::E duid); - /** - * @brief Helper to crypt P25 IMBE audio using ARC4. - * @param imbe Buffer containing IMBE to crypt. - * @param duid P25 DUID. - */ - void cryptARC4_P25IMBE(uint8_t* imbe, p25::defines::DUID::E duid); - /** * @brief Entry point to audio processing thread. * @param arg Instance of the thread_t structure. diff --git a/src/common/p25/Crypto.cpp b/src/common/p25/Crypto.cpp new file mode 100644 index 00000000..81653ea2 --- /dev/null +++ b/src/common/p25/Crypto.cpp @@ -0,0 +1,366 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Common Library + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2025 Bryan Biedenkapp, N2PLL + * + */ +#include "Defines.h" +#include "p25/kmm/KeysetItem.h" +#include "p25/P25Defines.h" +#include "p25/Crypto.h" +#include "AESCrypto.h" +#include "RC4Crypto.h" +#include "Log.h" +#include "Utils.h" + +using namespace ::crypto; +using namespace p25; +using namespace p25::defines; +using namespace p25::crypto; + +#include + +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +#define MAX_ENC_KEY_LENGTH_BYTES 32U + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/* Initializes a new instance of the P25Crypto class. */ + +P25Crypto::P25Crypto() : + m_tekAlgoId(ALGO_UNENCRYPT), + m_tekKeyId(0U), + m_tekLength(0U), + m_keystream(nullptr), + m_keystreamPos(0U), + m_mi(nullptr), + m_tek(nullptr), + m_random() +{ + m_mi = new uint8_t[MI_LENGTH_BYTES]; + ::memset(m_mi, 0x00U, MI_LENGTH_BYTES); + + std::random_device rd; + std::mt19937 mt(rd()); + m_random = mt; +} + +/* Finalizes a instance of the P25Crypto class. */ + +P25Crypto::~P25Crypto() +{ + if (m_keystream != nullptr) + delete[] m_keystream; + delete[] m_mi; +} + +/* Helper given to generate a new initial seed MI. */ + +void P25Crypto::generateMI() +{ + for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { + std::uniform_int_distribution dist(0x00U, 0xFFU); + m_mi[i] = (uint8_t)dist(m_random); + } +} + +/* Given the last MI, generate the next MI using LFSR. */ + +void P25Crypto::generateNextMI() +{ + uint8_t carry, i; + + uint8_t nextMI[9U]; + ::memcpy(nextMI, m_mi, MI_LENGTH_BYTES); + + for (uint8_t cycle = 0; cycle < 64; cycle++) { + // calculate bit 0 for the next cycle + carry = ((nextMI[0] >> 7) ^ (nextMI[0] >> 5) ^ (nextMI[2] >> 5) ^ + (nextMI[3] >> 5) ^ (nextMI[4] >> 2) ^ (nextMI[6] >> 6)) & + 0x01; + + // shift all the list elements, except the last one + for (i = 0; i < 7; i++) { + // grab high bit from the next element and use it as our low bit + nextMI[i] = ((nextMI[i] & 0x7F) << 1) | (nextMI[i + 1] >> 7); + } + + // shift last element, then copy the bit 0 we calculated in + nextMI[7] = ((nextMI[i] & 0x7F) << 1) | carry; + } + + ::memcpy(m_mi, nextMI, MI_LENGTH_BYTES); +} + +/* Helper to check if there is a valid encryption keystream. */ + +bool P25Crypto::hasValidKeystream() +{ + if (m_tek == nullptr) + return false; + if (m_tekLength == 0U) + return false; + if (m_keystream == nullptr) + return false; + + return true; +} + +/* Helper to generate the encryption keystream. */ + +void P25Crypto::generateKeystream() +{ + if (m_tek == nullptr) + return; + if (m_tekLength == 0U) + return; + if (m_mi == nullptr) + return; + + m_keystreamPos = 0U; + + // generate keystream + switch (m_tekAlgoId) { + case ALGO_AES_256: + { + if (m_keystream == nullptr) + m_keystream = new uint8_t[240U]; + ::memset(m_keystream, 0x00U, 240U); + + uint8_t* iv = expandMIToIV(); + + AES aes = AES(AESKeyLength::AES_256); + + uint8_t input[16U]; + ::memset(input, 0x00U, 16U); + ::memcpy(input, iv, 16U); + + for (uint32_t i = 0U; i < (240U / 16U); i++) { + uint8_t* output = aes.encryptECB(input, 16U, m_tek.get()); + ::memcpy(m_keystream + (i * 16U), output, 16U); + ::memcpy(input, output, 16U); + } + + delete[] iv; + } + break; + case ALGO_ARC4: + { + if (m_keystream == nullptr) + m_keystream = new uint8_t[469U]; + ::memset(m_keystream, 0x00U, 469U); + + uint8_t padding = (uint8_t)::fmax(5U - m_tekLength, 0U); + uint8_t adpKey[13U]; + ::memset(adpKey, 0x00U, 13U); + + uint8_t i = 0U; + for (i = 0U; i < padding; i++) + adpKey[i] = 0x00U; + + for (; i < 5U; i++) + adpKey[i] = (m_tekLength > 0U) ? m_tek[i - padding] : 0x00U; + + for (i = 5U; i < 13U; i++) + adpKey[i] = m_mi[i - 5U]; + + // generate ARC4 keystream + RC4 rc4 = RC4(); + m_keystream = rc4.keystream(469U, adpKey, 13U); + } + break; + default: + LogError(LOG_P25, "unsupported crypto algorithm, algId = $%02X", m_tekAlgoId); + break; + } +} + +/* Helper to reset the encryption keystream. */ + +void P25Crypto::resetKeystream() +{ + ::memset(m_mi, 0x00U, MI_LENGTH_BYTES); + if (m_keystream != nullptr) { + delete[] m_keystream; + m_keystream = nullptr; + m_keystreamPos = 0U; + } +} + +/* Helper to crypt IMBE audio using AES-256. */ + +void P25Crypto::cryptAES_IMBE(uint8_t* imbe, DUID::E duid) +{ + if (m_keystream == nullptr) + return; + + uint32_t offset = 16U; + if (duid == DUID::LDU2) { + offset += 101U; + } + + offset += (m_keystreamPos * RAW_IMBE_LENGTH_BYTES) + RAW_IMBE_LENGTH_BYTES + ((m_keystreamPos < 8U) ? 0U : 2U); + m_keystreamPos = (m_keystreamPos + 1U) % 9U; + + for (uint8_t i = 0U; i < RAW_IMBE_LENGTH_BYTES; i++) { + imbe[i] ^= m_keystream[offset + i]; + } +} + +/* Helper to crypt IMBE audio using ARC4. */ + +void P25Crypto::cryptARC4_IMBE(uint8_t* imbe, DUID::E duid) +{ + if (m_keystream == nullptr) + return; + + uint32_t offset = 256U; + if (duid == DUID::LDU2) { + offset += 101U; + } + + offset += (m_keystreamPos * RAW_IMBE_LENGTH_BYTES) + 267U + ((m_keystreamPos < 8U) ? 0U : 2U); + m_keystreamPos = (m_keystreamPos + 1U) % 9U; + + for (uint8_t i = 0U; i < RAW_IMBE_LENGTH_BYTES; i++) { + imbe[i] ^= m_keystream[offset + i]; + } +} + +/* Helper to check if there is a valid encryption message indicator. */ + +bool P25Crypto::hasValidMI() +{ + bool hasMI = false; + for (uint8_t i = 0; i < MI_LENGTH_BYTES; i++) { + if (m_mi[i] != 0x00U) + hasMI = true; + } + + return hasMI; +} + +/* Sets the encryption message indicator. */ + +void P25Crypto::setMI(const uint8_t* mi) +{ + assert(mi != nullptr); + + ::memcpy(m_mi, mi, MI_LENGTH_BYTES); +} + +/* Gets the encryption message indicator. */ + +void P25Crypto::getMI(uint8_t* mi) const +{ + assert(mi != nullptr); + + ::memcpy(mi, m_mi, MI_LENGTH_BYTES); +} + +/* Clears the stored encryption message indicator. */ + +void P25Crypto::clearMI() +{ + ::memset(m_mi, 0x00U, MI_LENGTH_BYTES); +} + +/* Sets the encryption key. */ + +void P25Crypto::setKey(const uint8_t* key, uint8_t len) +{ + assert(key != nullptr); + + m_tekLength = len; + if (m_tek != nullptr) + m_tek.reset(); + + m_tek = std::make_unique(len); + ::memset(m_tek.get(), 0x00U, MAX_ENC_KEY_LENGTH_BYTES); + ::memset(m_tek.get(), 0x00U, m_tekLength); + ::memcpy(m_tek.get(), key, len); +} + +/* Gets the encryption key. */ + +void P25Crypto::getKey(uint8_t* key) const +{ + assert(key != nullptr); + + ::memcpy(key, m_tek.get(), m_tekLength); +} + +/* Clears the stored encryption key. */ + +void P25Crypto::clearKey() +{ + m_tekLength = 0U; + if (m_tek != nullptr) + m_tek.reset(); + + m_tek = std::make_unique(MAX_ENC_KEY_LENGTH_BYTES); + ::memset(m_tek.get(), 0x00U, MAX_ENC_KEY_LENGTH_BYTES); +} + +// --------------------------------------------------------------------------- +// Private Class Members +// --------------------------------------------------------------------------- + +/* */ + +uint64_t P25Crypto::stepLFSR(uint64_t& lfsr) +{ + uint64_t ovBit = (lfsr >> 63U) & 0x01U; + + // compute feedback bit using polynomial: x^64 + x^62 + x^46 + x^38 + x^27 + x^15 + 1 + uint64_t fbBit = ((lfsr >> 63U) ^ (lfsr >> 61U) ^ (lfsr >> 45U) ^ (lfsr >> 37U) ^ + (lfsr >> 26U) ^ (lfsr >> 14U)) & 0x01U; + + // shift LFSR left and insert feedback bit + lfsr = (lfsr << 1) | fbBit; + return ovBit; +} + +/* Expands the 9-byte MI into a proper 16-byte IV. */ + +uint8_t* P25Crypto::expandMIToIV() +{ + // this should never happen... + if (m_mi == nullptr) + return nullptr; + + uint8_t* iv = new uint8_t[16U]; + ::memset(iv, 0x00U, 16U); + + // copy first 64-bits of the MI info LFSR + uint64_t lfsr = 0U; + for (uint8_t i = 0U; i < 8U; i++) { + lfsr = (lfsr << 8U) | m_mi[i]; + } + + uint64_t overflow = 0U; + for (uint8_t i = 0U; i < 64U; i++) { + overflow = (overflow << 1U) | stepLFSR(lfsr); + } + + // copy expansion and LFSR into IV + for (int i = 7; i >= 0; i--) { + iv[i] = (uint8_t)(overflow & 0xFFU); + overflow >>= 8U; + } + + for (int i = 15; i >= 8; i--) { + iv[i] = (uint8_t)(lfsr & 0xFFU); + lfsr >>= 8U; + } + + return iv; +} diff --git a/src/common/p25/Crypto.h b/src/common/p25/Crypto.h new file mode 100644 index 00000000..630b4695 --- /dev/null +++ b/src/common/p25/Crypto.h @@ -0,0 +1,165 @@ +// SPDX-License-Identifier: MIT +/* + * Digital Voice Modem - Common Library + * MIT Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2025 Bryan Biedenkapp, N2PLL + * + */ +/** + * @defgroup p25_crypto Project 25 Cryptography + * @brief Defines and implements cryptography routines for P25. + * @ingroup p25 + * + * @file Crypto.h + * @ingroup p25 + * @file Crypto.cpp + * @ingroup p25 + */ +#if !defined(__P25_CRYPTO_H__) +#define __P25_CRYPTO_H__ + +#include "common/Defines.h" +#include "common/p25/P25Defines.h" +#include "common/Utils.h" + +#include + +namespace p25 +{ + namespace crypto + { + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Project 25 Cryptography. + * @ingroup p25_crypto + */ + class HOST_SW_API P25Crypto { + public: + /** + * @brief Initializes a new instance of the P25Crypto class. + */ + P25Crypto(); + /** + * @brief Finalizes a instance of the P25Crypto class. + */ + ~P25Crypto(); + + /** + * @brief Helper given to generate a new initial seed MI. + * @param mi + */ + void generateMI(); + /** + * @brief Helper given the last MI, generate the next MI using LFSR. + */ + void generateNextMI(); + + /** + * @brief Helper to check if there is a valid encryption keystream. + * @return bool True, if there is a valid keystream, otherwise false. + */ + bool hasValidKeystream(); + /** + * @brief Helper to generate the encryption keystream. + */ + void generateKeystream(); + /** + * @brief Helper to reset the encryption keystream. + */ + void resetKeystream(); + + /** + * @brief Helper to crypt P25 IMBE audio using AES-256. + * @param imbe Buffer containing IMBE to crypt. + * @param duid P25 DUID. + */ + void cryptAES_IMBE(uint8_t* imbe, P25DEF::DUID::E duid); + /** + * @brief Helper to crypt P25 IMBE audio using ARC4. + * @param imbe Buffer containing IMBE to crypt. + * @param duid P25 DUID. + */ + void cryptARC4_IMBE(uint8_t* imbe, P25DEF::DUID::E duid); + + /** + * @brief Helper to check if there is a valid encryption message indicator. + * @return bool True, if there is a valid encryption message indicator, otherwise false. + */ + bool hasValidMI(); + /** + * @brief Sets the encryption message indicator. + * @param[in] mi Buffer containing the 9-byte Message Indicator. + */ + void setMI(const uint8_t* mi); + /** + * @brief Gets the encryption message indicator. + * @param[out] mi Buffer containing the 9-byte Message Indicator. + */ + void getMI(uint8_t* mi) const; + /** + * @brief Clears the encryption message indicator. + */ + void clearMI(); + + /** + * @brief Sets the encryption key. + * @param[in] mi Buffer containing the encryption key. + * @param[in] len Length of the key. + */ + void setKey(const uint8_t* key, uint8_t len); + /** + * @brief Gets the encryption key, + * @param[out] mi Buffer containing the encryption key. + */ + void getKey(uint8_t* key) const; + /** + * @brief Clears the stored encryption key. + */ + void clearKey(); + + public: + /** + * @brief Traffic Encryption Key Algorithm ID. + */ + __PROPERTY(uint8_t, tekAlgoId, TEKAlgoId); + /** + * @brief Traffic Encryption Key ID. + */ + __PROPERTY(uint16_t, tekKeyId, TEKKeyId); + + /** + * @brief Traffic Encryption Key Length. + */ + __READONLY_PROPERTY(uint8_t, tekLength, TEKLength); + + private: + uint8_t* m_keystream; + uint32_t m_keystreamPos; + + uint8_t* m_mi; + + UInt8Array m_tek; + + std::mt19937 m_random; + + /** + * @brief + * @param lfsr + * @return uint64_t + */ + uint64_t stepLFSR(uint64_t& lfsr); + /** + * @brief Expands the 9-byte MI into a proper 16-byte IV. + * @return uint8_t* Buffer containing expanded 16-byte IV. + */ + uint8_t* expandMIToIV(); + }; + } // namespace crypto +} // namespace p25 + +#endif // __P25_CRYPTO_H__ diff --git a/src/common/p25/kmm/KeysetItem.h b/src/common/p25/kmm/KeysetItem.h index d3c556e5..059fb26e 100644 --- a/src/common/p25/kmm/KeysetItem.h +++ b/src/common/p25/kmm/KeysetItem.h @@ -91,10 +91,7 @@ namespace p25 { assert(key != nullptr); m_keyLength = keyLength; - if (m_keyMaterial == nullptr) { - ::memset(m_keyMaterial, 0x00U, MAX_ENC_KEY_LENGTH_BYTES); - ::memset(m_keyMaterial, 0x00U, m_keyLength); - } + ::memset(m_keyMaterial, 0x00U, MAX_ENC_KEY_LENGTH_BYTES); ::memcpy(m_keyMaterial, key, keyLength); }