reorganize code make things cleaner -- move P25 crypto into its own common class for reuse purposes;
Bryan Biedenkapp
11 months ago
parent
03de7fdb0b
commit
2d3aeb5307
@ -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 <cassert>
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// 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<uint32_t> 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<uint8_t[]>(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<uint8_t[]>(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;
|
||||
}
|
||||
@ -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 <random>
|
||||
|
||||
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__
|
||||
Loading…
Reference in new issue