You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
2365 lines
87 KiB
2365 lines
87 KiB
// SPDX-License-Identifier: GPL-2.0-only
|
|
/*
|
|
* Digital Voice Modem - TG Patch
|
|
* 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 "common/dmr/DMRDefines.h"
|
|
#include "common/dmr/data/EMB.h"
|
|
#include "common/dmr/data/NetData.h"
|
|
#include "common/dmr/lc/FullLC.h"
|
|
#include "common/dmr/SlotType.h"
|
|
#include "common/p25/P25Defines.h"
|
|
#include "common/p25/data/LowSpeedData.h"
|
|
#include "common/p25/dfsi/DFSIDefines.h"
|
|
#include "common/p25/dfsi/LC.h"
|
|
#include "common/p25/lc/LC.h"
|
|
#include "common/p25/Sync.h"
|
|
#include "common/p25/P25Utils.h"
|
|
#include "common/network/RTPHeader.h"
|
|
#include "common/network/udp/Socket.h"
|
|
#include "common/Clock.h"
|
|
#include "common/StopWatch.h"
|
|
#include "common/Thread.h"
|
|
#include "common/Log.h"
|
|
#include "common/Utils.h"
|
|
#include "patch/ActivityLog.h"
|
|
#include "HostPatch.h"
|
|
#include "PatchMain.h"
|
|
|
|
using namespace network;
|
|
using namespace network::frame;
|
|
using namespace network::udp;
|
|
|
|
#include <cstdio>
|
|
#include <algorithm>
|
|
#include <functional>
|
|
#include <random>
|
|
|
|
#if !defined(_WIN32)
|
|
#include <unistd.h>
|
|
#include <pwd.h>
|
|
#endif // !defined(_WIN32)
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Constants
|
|
// ---------------------------------------------------------------------------
|
|
|
|
#define TEK_DES "des"
|
|
#define TEK_AES "aes"
|
|
#define TEK_ARC4 "arc4"
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Static Class Members
|
|
// ---------------------------------------------------------------------------
|
|
|
|
std::mutex HostPatch::s_networkMutex;
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Public Class Members
|
|
// ---------------------------------------------------------------------------
|
|
|
|
/* Initializes a new instance of the HostPatch class. */
|
|
|
|
HostPatch::HostPatch(const std::string& confFile) :
|
|
m_confFile(confFile),
|
|
m_conf(),
|
|
m_network(nullptr),
|
|
m_srcTGId(0U),
|
|
m_srcSlot(1U),
|
|
m_dstTGId(0U),
|
|
m_dstSlot(1U),
|
|
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),
|
|
m_netLDU1(nullptr),
|
|
m_gotNetLDU2(false),
|
|
m_netLDU2(nullptr),
|
|
m_identity(),
|
|
m_digiMode(1U),
|
|
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),
|
|
m_tekSrcEnable(false),
|
|
m_tekSrcAlgoId(P25DEF::ALGO_UNENCRYPT),
|
|
m_tekSrcKeyId(0U),
|
|
m_tekDstEnable(false),
|
|
m_tekDstAlgoId(P25DEF::ALGO_UNENCRYPT),
|
|
m_tekDstKeyId(0U),
|
|
m_requestedSrcTek(false),
|
|
m_requestedDstTek(false),
|
|
m_p25SrcCrypto(nullptr),
|
|
m_p25DstCrypto(nullptr),
|
|
m_netId(P25DEF::WACN_STD_DEFAULT),
|
|
m_sysId(P25DEF::SID_STD_DEFAULT),
|
|
m_running(false),
|
|
m_trace(false),
|
|
m_debug(false)
|
|
{
|
|
m_netLDU1 = new uint8_t[9U * 25U];
|
|
m_netLDU2 = new uint8_t[9U * 25U];
|
|
|
|
::memset(m_netLDU1, 0x00U, 9U * 25U);
|
|
resetWithNullAudio(m_netLDU1, false);
|
|
::memset(m_netLDU2, 0x00U, 9U * 25U);
|
|
resetWithNullAudio(m_netLDU2, false);
|
|
|
|
m_p25SrcCrypto = new p25::crypto::P25Crypto();
|
|
m_p25DstCrypto = new p25::crypto::P25Crypto();
|
|
}
|
|
|
|
/* Finalizes a instance of the HostPatch class. */
|
|
|
|
HostPatch::~HostPatch()
|
|
{
|
|
delete[] m_netLDU1;
|
|
delete[] m_netLDU2;
|
|
delete m_p25SrcCrypto;
|
|
delete m_p25DstCrypto;
|
|
}
|
|
|
|
/* Executes the main FNE processing loop. */
|
|
|
|
int HostPatch::run()
|
|
{
|
|
bool ret = false;
|
|
try {
|
|
ret = yaml::Parse(m_conf, m_confFile.c_str());
|
|
if (!ret) {
|
|
::fatal("cannot read the configuration file, %s\n", m_confFile.c_str());
|
|
}
|
|
}
|
|
catch (yaml::OperationException const& e) {
|
|
::fatal("cannot read the configuration file - %s (%s)", m_confFile.c_str(), e.message());
|
|
}
|
|
|
|
bool m_daemon = m_conf["daemon"].as<bool>(false);
|
|
if (m_daemon && g_foreground)
|
|
m_daemon = false;
|
|
|
|
// initialize system logging
|
|
yaml::Node logConf = m_conf["log"];
|
|
ret = ::LogInitialise(logConf["filePath"].as<std::string>(), logConf["fileRoot"].as<std::string>(),
|
|
logConf["fileLevel"].as<uint32_t>(0U), logConf["displayLevel"].as<uint32_t>(0U));
|
|
if (!ret) {
|
|
::fatal("unable to open the log file\n");
|
|
}
|
|
|
|
ret = ::ActivityLogInitialise(logConf["activityFilePath"].as<std::string>(), logConf["fileRoot"].as<std::string>());
|
|
if (!ret) {
|
|
::fatal("unable to open the activity log file\n");
|
|
}
|
|
|
|
#if !defined(_WIN32)
|
|
// handle POSIX process forking
|
|
if (m_daemon) {
|
|
// create new process
|
|
pid_t pid = ::fork();
|
|
if (pid == -1) {
|
|
::fprintf(stderr, "%s: Couldn't fork() , exiting\n", g_progExe.c_str());
|
|
::LogFinalise();
|
|
return EXIT_FAILURE;
|
|
}
|
|
else if (pid != 0) {
|
|
::LogFinalise();
|
|
exit(EXIT_SUCCESS);
|
|
}
|
|
|
|
// create new session and process group
|
|
if (::setsid() == -1) {
|
|
::fprintf(stderr, "%s: Couldn't setsid(), exiting\n", g_progExe.c_str());
|
|
::LogFinalise();
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
// set the working directory to the root directory
|
|
if (::chdir("/") == -1) {
|
|
::fprintf(stderr, "%s: Couldn't cd /, exiting\n", g_progExe.c_str());
|
|
::LogFinalise();
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
::close(STDIN_FILENO);
|
|
::close(STDOUT_FILENO);
|
|
::close(STDERR_FILENO);
|
|
}
|
|
#endif // !defined(_WIN32)
|
|
|
|
::LogInfo(__BANNER__ "\r\n" __PROG_NAME__ " " __VER__ " (built " __BUILD__ ")\r\n" \
|
|
"Copyright (c) 2025 Bryan Biedenkapp, N2PLL and DVMProject (https://github.com/dvmproject) Authors.\r\n" \
|
|
"Portions Copyright (c) 2015-2021 by Jonathan Naylor, G4KLX and others\r\n" \
|
|
">> Talkgroup Patch\r\n");
|
|
|
|
// read base parameters from configuration
|
|
ret = readParams();
|
|
if (!ret)
|
|
return EXIT_FAILURE;
|
|
|
|
yaml::Node systemConf = m_conf["system"];
|
|
|
|
// initialize peer networking
|
|
ret = createNetwork();
|
|
if (!ret)
|
|
return EXIT_FAILURE;
|
|
|
|
// initialize MMDVM P25 reflector networking
|
|
if (m_mmdvmP25Reflector) {
|
|
ret = createMMDVMP25Network();
|
|
if (!ret)
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
/*
|
|
** Initialize Threads
|
|
*/
|
|
|
|
if (!Thread::runAsThread(this, threadNetworkProcess))
|
|
return EXIT_FAILURE;
|
|
|
|
if (m_mmdvmP25Reflector) {
|
|
if (!Thread::runAsThread(this, threadMMDVMProcess))
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
::LogInfoEx(LOG_HOST, "Patch is up and running");
|
|
|
|
m_running = true;
|
|
|
|
StopWatch stopWatch;
|
|
stopWatch.start();
|
|
|
|
// main execution loop
|
|
while (!g_killed) {
|
|
uint32_t ms = stopWatch.elapsed();
|
|
|
|
ms = stopWatch.elapsed();
|
|
stopWatch.start();
|
|
|
|
// ------------------------------------------------------
|
|
// -- Network Clocking --
|
|
// ------------------------------------------------------
|
|
|
|
if (m_network != nullptr) {
|
|
std::lock_guard<std::mutex> lock(HostPatch::s_networkMutex);
|
|
m_network->clock(ms);
|
|
}
|
|
|
|
if (m_mmdvmP25Reflector) {
|
|
std::lock_guard<std::mutex> lock(HostPatch::s_networkMutex);
|
|
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);
|
|
}
|
|
|
|
::LogSetNetwork(nullptr);
|
|
if (m_network != nullptr) {
|
|
m_network->close();
|
|
delete m_network;
|
|
}
|
|
|
|
return EXIT_SUCCESS;
|
|
}
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Private Class Members
|
|
// ---------------------------------------------------------------------------
|
|
|
|
/* Reads basic configuration parameters from the YAML configuration file. */
|
|
|
|
bool HostPatch::readParams()
|
|
{
|
|
yaml::Node systemConf = m_conf["system"];
|
|
|
|
m_identity = systemConf["identity"].as<std::string>();
|
|
|
|
m_netId = (uint32_t)::strtoul(systemConf["netId"].as<std::string>("BB800").c_str(), NULL, 16);
|
|
m_netId = p25::P25Utils::netId(m_netId);
|
|
if (m_netId == 0xBEE00) {
|
|
::fatal("error 4\n");
|
|
}
|
|
|
|
m_sysId = (uint32_t)::strtoul(systemConf["sysId"].as<std::string>("001").c_str(), NULL, 16);
|
|
m_sysId = p25::P25Utils::sysId(m_sysId);
|
|
|
|
/*
|
|
** Site Data
|
|
*/
|
|
int8_t lto = (int8_t)systemConf["localTimeOffset"].as<int32_t>(0);
|
|
p25::SiteData siteData = p25::SiteData(m_netId, m_sysId, 1U, 1U, 0U, 0U, 1U, P25DEF::ServiceClass::VOICE, lto);
|
|
siteData.setNetActive(true);
|
|
|
|
p25::lc::LC::setSiteData(siteData);
|
|
|
|
m_digiMode = (uint8_t)systemConf["digiMode"].as<uint32_t>(1U);
|
|
if (m_digiMode < TX_MODE_DMR)
|
|
m_digiMode = TX_MODE_DMR;
|
|
if (m_digiMode > TX_MODE_P25)
|
|
m_digiMode = TX_MODE_P25;
|
|
|
|
m_grantDemand = systemConf["grantDemand"].as<bool>(false);
|
|
|
|
m_mmdvmP25Reflector = systemConf["mmdvmP25Reflector"].as<bool>(false);
|
|
|
|
if (m_mmdvmP25Reflector && m_digiMode != TX_MODE_P25) {
|
|
LogError(LOG_HOST, "Patch does not currently support MMDVM patching in any mode other then P25.");
|
|
return false;
|
|
}
|
|
|
|
m_dropTimeMS = (uint16_t)systemConf["dropTimeMs"].as<uint32_t>(1000U);
|
|
m_callDropTime = Timer(1000U, 0U, m_dropTimeMS);
|
|
|
|
m_trace = systemConf["trace"].as<bool>(false);
|
|
m_debug = systemConf["debug"].as<bool>(false);
|
|
|
|
LogInfo("General Parameters");
|
|
LogInfo(" System Id: $%03X", m_sysId);
|
|
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) {
|
|
LogInfo(" Debug: yes");
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Initializes network connectivity. */
|
|
|
|
bool HostPatch::createNetwork()
|
|
{
|
|
yaml::Node networkConf = m_conf["network"];
|
|
|
|
std::string address = networkConf["address"].as<std::string>();
|
|
uint16_t port = (uint16_t)networkConf["port"].as<uint32_t>(TRAFFIC_DEFAULT_PORT);
|
|
uint16_t local = (uint16_t)networkConf["local"].as<uint32_t>(0U);
|
|
uint32_t id = networkConf["id"].as<uint32_t>(1000U);
|
|
std::string password = networkConf["password"].as<std::string>();
|
|
bool allowDiagnosticTransfer = networkConf["allowDiagnosticTransfer"].as<bool>(false);
|
|
bool packetDump = networkConf["packetDump"].as<bool>(false);
|
|
bool debug = networkConf["debug"].as<bool>(false);
|
|
|
|
m_srcTGId = (uint32_t)networkConf["sourceTGID"].as<uint32_t>(1U);
|
|
m_srcSlot = (uint8_t)networkConf["sourceSlot"].as<uint32_t>(1U);
|
|
m_dstTGId = (uint32_t)networkConf["destinationTGID"].as<uint32_t>(1U);
|
|
m_dstSlot = (uint8_t)networkConf["destinationSlot"].as<uint32_t>(1U);
|
|
|
|
// source TEK parameters
|
|
m_tekSrcEnable = false;
|
|
yaml::Node srcTekConf = networkConf["srcTek"];
|
|
bool tekSrcEnable = srcTekConf["enable"].as<bool>(false);
|
|
std::string tekSrcAlgo = srcTekConf["tekAlgo"].as<std::string>();
|
|
std::transform(tekSrcAlgo.begin(), tekSrcAlgo.end(), tekSrcAlgo.begin(), ::tolower);
|
|
m_tekSrcKeyId = (uint32_t)::strtoul(srcTekConf["tekKeyId"].as<std::string>("0").c_str(), NULL, 16);
|
|
if (tekSrcEnable && m_tekSrcKeyId > 0U) {
|
|
if (tekSrcAlgo == TEK_AES)
|
|
m_tekSrcAlgoId = P25DEF::ALGO_AES_256;
|
|
else if (tekSrcAlgo == TEK_ARC4)
|
|
m_tekSrcAlgoId = P25DEF::ALGO_ARC4;
|
|
else if (tekSrcAlgo == TEK_DES)
|
|
m_tekSrcAlgoId = P25DEF::ALGO_DES;
|
|
else {
|
|
::LogError(LOG_HOST, "Invalid TEK algorithm specified, must be \"aes\" or \"arc4\".");
|
|
m_tekSrcAlgoId = P25DEF::ALGO_UNENCRYPT;
|
|
m_tekSrcKeyId = 0U;
|
|
}
|
|
}
|
|
|
|
if (!tekSrcEnable)
|
|
m_tekSrcAlgoId = P25DEF::ALGO_UNENCRYPT;
|
|
if (m_tekSrcAlgoId == P25DEF::ALGO_UNENCRYPT)
|
|
m_tekSrcKeyId = 0U;
|
|
if (m_tekSrcAlgoId != P25DEF::ALGO_UNENCRYPT)
|
|
m_tekSrcEnable = true;
|
|
|
|
// destination TEK parameters
|
|
m_tekDstEnable = false;
|
|
yaml::Node dstTekConf = networkConf["dstTek"];
|
|
bool tekDstEnable = dstTekConf["enable"].as<bool>(false);
|
|
std::string tekDstAlgo = dstTekConf["tekAlgo"].as<std::string>();
|
|
std::transform(tekDstAlgo.begin(), tekDstAlgo.end(), tekDstAlgo.begin(), ::tolower);
|
|
m_tekDstKeyId = (uint32_t)::strtoul(dstTekConf["tekKeyId"].as<std::string>("0").c_str(), NULL, 16);
|
|
if (tekDstEnable && m_tekDstKeyId > 0U) {
|
|
if (tekDstAlgo == TEK_AES)
|
|
m_tekDstAlgoId = P25DEF::ALGO_AES_256;
|
|
else if (tekDstAlgo == TEK_ARC4)
|
|
m_tekDstAlgoId = P25DEF::ALGO_ARC4;
|
|
else if (tekDstAlgo == TEK_DES)
|
|
m_tekDstAlgoId = P25DEF::ALGO_DES;
|
|
else {
|
|
::LogError(LOG_HOST, "Invalid TEK algorithm specified, must be \"aes\" or \"arc4\".");
|
|
m_tekDstAlgoId = P25DEF::ALGO_UNENCRYPT;
|
|
m_tekDstKeyId = 0U;
|
|
}
|
|
}
|
|
|
|
if (!tekDstEnable)
|
|
m_tekDstAlgoId = P25DEF::ALGO_UNENCRYPT;
|
|
if (m_tekDstAlgoId == P25DEF::ALGO_UNENCRYPT)
|
|
m_tekDstKeyId = 0U;
|
|
if (m_tekDstAlgoId != P25DEF::ALGO_UNENCRYPT)
|
|
m_tekDstEnable = true;
|
|
|
|
m_twoWayPatch = networkConf["twoWay"].as<bool>(false);
|
|
|
|
// make sure our destination ID is sane
|
|
if (m_srcTGId == 0U) {
|
|
::LogError(LOG_HOST, "Patch source TGID cannot be set to 0.");
|
|
return false;
|
|
}
|
|
|
|
if (m_dstTGId == 0U) {
|
|
::LogError(LOG_HOST, "Patch destination TGID cannot be set to 0.");
|
|
return false;
|
|
}
|
|
|
|
if (m_srcTGId == m_dstTGId) {
|
|
::LogError(LOG_HOST, "Patch source TGID and destination TGID cannot be the same.");
|
|
return false;
|
|
}
|
|
|
|
// make sure we're range checked
|
|
switch (m_digiMode) {
|
|
case TX_MODE_DMR:
|
|
{
|
|
if (m_srcTGId > 16777215) {
|
|
::LogError(LOG_HOST, "Patch source TGID cannot be greater than 16777215.");
|
|
return false;
|
|
}
|
|
|
|
if (m_dstTGId > 16777215) {
|
|
::LogError(LOG_HOST, "Patch source TGID cannot be greater than 16777215.");
|
|
return false;
|
|
}
|
|
}
|
|
break;
|
|
case TX_MODE_P25:
|
|
{
|
|
if (m_srcTGId > 65535) {
|
|
::LogError(LOG_HOST, "Patch source TGID cannot be greater than 65535.");
|
|
return false;
|
|
}
|
|
|
|
if (m_dstTGId > 65535) {
|
|
::LogError(LOG_HOST, "Patch destination TGID cannot be greater than 65535.");
|
|
return false;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
bool encrypted = networkConf["encrypted"].as<bool>(false);
|
|
std::string key = networkConf["presharedKey"].as<std::string>();
|
|
uint8_t presharedKey[AES_WRAPPED_PCKT_KEY_LEN];
|
|
if (!key.empty()) {
|
|
if (key.size() == 32) {
|
|
// bryanb: shhhhhhh....dirty nasty hacks
|
|
key = key.append(key); // since the key is 32 characters (16 hex pairs), double it on itself for 64 characters (32 hex pairs)
|
|
LogWarning(LOG_HOST, "Half-length network preshared encryption key detected, doubling key on itself.");
|
|
}
|
|
|
|
if (key.size() == 64) {
|
|
if ((key.find_first_not_of("0123456789abcdefABCDEF", 2) == std::string::npos)) {
|
|
const char* keyPtr = key.c_str();
|
|
::memset(presharedKey, 0x00U, AES_WRAPPED_PCKT_KEY_LEN);
|
|
|
|
for (uint8_t i = 0; i < AES_WRAPPED_PCKT_KEY_LEN; i++) {
|
|
char t[4] = { keyPtr[0], keyPtr[1], 0 };
|
|
presharedKey[i] = (uint8_t)::strtoul(t, NULL, 16);
|
|
keyPtr += 2 * sizeof(char);
|
|
}
|
|
}
|
|
else {
|
|
LogWarning(LOG_HOST, "Invalid characters in the network preshared encryption key. Encryption disabled.");
|
|
encrypted = false;
|
|
}
|
|
}
|
|
else {
|
|
LogWarning(LOG_HOST, "Invalid network preshared encryption key length, key should be 32 hex pairs, or 64 characters. Encryption disabled.");
|
|
encrypted = false;
|
|
}
|
|
}
|
|
|
|
if (id > 999999999U) {
|
|
::LogError(LOG_HOST, "Network Peer ID cannot be greater then 999999999.");
|
|
return false;
|
|
}
|
|
|
|
LogInfo("Network Parameters");
|
|
LogInfo(" Peer ID: %u", id);
|
|
LogInfo(" Address: %s", address.c_str());
|
|
LogInfo(" Port: %u", port);
|
|
if (local > 0U)
|
|
LogInfo(" Local: %u", local);
|
|
else
|
|
LogInfo(" Local: random");
|
|
|
|
LogInfo(" Encrypted: %s", encrypted ? "yes" : "no");
|
|
|
|
LogInfo(" Source TGID: %u", m_srcTGId);
|
|
LogInfo(" Source DMR Slot: %u", m_srcSlot);
|
|
|
|
LogInfo(" Source Traffic Encrypted: %s", tekSrcEnable ? "yes" : "no");
|
|
if (tekSrcEnable) {
|
|
LogInfo(" Source TEK Algorithm: %s", tekSrcAlgo.c_str());
|
|
LogInfo(" Source TEK Key ID: $%04X", m_tekSrcKeyId);
|
|
}
|
|
|
|
LogInfo(" Destination TGID: %u", m_dstTGId);
|
|
LogInfo(" Destination DMR Slot: %u", m_dstSlot);
|
|
|
|
LogInfo(" Destination Traffic Encrypted: %s", tekDstEnable ? "yes" : "no");
|
|
if (tekDstEnable) {
|
|
LogInfo(" Destination TEK Algorithm: %s", tekDstAlgo.c_str());
|
|
LogInfo(" Destination TEK Key ID: $%04X", m_tekDstKeyId);
|
|
}
|
|
|
|
LogInfo(" Two-Way Patch: %s", m_twoWayPatch ? "yes" : "no");
|
|
|
|
if (packetDump) {
|
|
LogInfo(" Packet Dump: yes");
|
|
}
|
|
|
|
if (debug) {
|
|
LogInfo(" Debug: yes");
|
|
}
|
|
|
|
bool dmr = false, p25 = false;
|
|
switch (m_digiMode) {
|
|
case TX_MODE_DMR:
|
|
dmr = true;
|
|
break;
|
|
case TX_MODE_P25:
|
|
p25 = true;
|
|
break;
|
|
}
|
|
|
|
// initialize networking
|
|
m_network = new PeerNetwork(address, port, local, id, password, true, debug, dmr, p25, false, true, true, true, allowDiagnosticTransfer, true, false);
|
|
|
|
m_network->setPacketDump(packetDump);
|
|
m_network->setMetadata(m_identity, 0U, 0U, 0.0F, 0.0F, 0, 0, 0, 0.0F, 0.0F, 0, "");
|
|
m_network->setConventional(true);
|
|
m_network->setKeyResponseCallback([=](p25::kmm::KeyItem ki, uint8_t algId, uint8_t keyLength) {
|
|
processTEKResponse(&ki, algId, keyLength);
|
|
});
|
|
|
|
if (encrypted) {
|
|
m_network->setPresharedKey(presharedKey);
|
|
}
|
|
|
|
m_network->enable(true);
|
|
bool ret = m_network->open();
|
|
if (!ret) {
|
|
delete m_network;
|
|
m_network = nullptr;
|
|
LogError(LOG_HOST, "failed to initialize traffic networking!");
|
|
return false;
|
|
}
|
|
|
|
::LogSetNetwork(m_network);
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Initializes MMDVM network connectivity. */
|
|
|
|
bool HostPatch::createMMDVMP25Network()
|
|
{
|
|
yaml::Node networkConf = m_conf["network"];
|
|
|
|
std::string address = networkConf["mmdvmGatewayAddress"].as<std::string>();
|
|
uint16_t port = (uint16_t)networkConf["mmdvmGatewayPort"].as<uint32_t>(42020U);
|
|
uint16_t localPort = (uint16_t)networkConf["localGatewayPort"].as<uint32_t>(32010U);
|
|
bool debug = networkConf["debug"].as<bool>(false);
|
|
|
|
LogInfo("MMDVM Network Parameters");
|
|
LogInfo(" Address: %s", address.c_str());
|
|
LogInfo(" Port: %u", port);
|
|
LogInfo(" Local Port: %u", localPort);
|
|
|
|
if (debug) {
|
|
LogInfo(" Debug: yes");
|
|
}
|
|
|
|
// initialize networking
|
|
m_mmdvmP25Net = new mmdvm::P25Network(address, port, localPort, debug);
|
|
|
|
bool ret = m_mmdvmP25Net->open();
|
|
if (!ret) {
|
|
delete m_mmdvmP25Net;
|
|
m_mmdvmP25Net = nullptr;
|
|
LogError(LOG_HOST, "failed to initialize MMDVM networking!");
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Helper to process DMR network traffic. */
|
|
|
|
void HostPatch::processDMRNetwork(uint8_t* buffer, uint32_t length)
|
|
{
|
|
assert(buffer != nullptr);
|
|
using namespace dmr;
|
|
using namespace dmr::defines;
|
|
|
|
if (m_digiMode != TX_MODE_DMR) {
|
|
m_network->resetDMR(1U);
|
|
m_network->resetDMR(2U);
|
|
return;
|
|
}
|
|
|
|
// process network message header
|
|
uint32_t seqNo = buffer[4U];
|
|
|
|
uint32_t srcId = GET_UINT24(buffer, 5U);
|
|
uint32_t dstId = GET_UINT24(buffer, 8U);
|
|
|
|
uint8_t controlByte = buffer[14U];
|
|
|
|
FLCO::E flco = (buffer[15U] & 0x40U) == 0x40U ? FLCO::PRIVATE : FLCO::GROUP;
|
|
|
|
uint32_t slotNo = (buffer[15U] & 0x80U) == 0x80U ? 2U : 1U;
|
|
|
|
if (slotNo > 3U) {
|
|
LogError(LOG_DMR, "DMR, invalid slot, slotNo = %u", slotNo);
|
|
m_network->resetDMR(1U);
|
|
m_network->resetDMR(2U);
|
|
return;
|
|
}
|
|
|
|
// DMO mode slot disabling
|
|
if (slotNo == 1U && !m_network->getDuplex()) {
|
|
LogError(LOG_DMR, "DMR/DMO, invalid slot, slotNo = %u", slotNo);
|
|
m_network->resetDMR(1U);
|
|
return;
|
|
}
|
|
|
|
// Individual slot disabling
|
|
if (slotNo == 1U && !m_network->getDMRSlot1()) {
|
|
LogError(LOG_DMR, "DMR, invalid slot, slot 1 disabled, slotNo = %u", slotNo);
|
|
m_network->resetDMR(1U);
|
|
return;
|
|
}
|
|
if (slotNo == 2U && !m_network->getDMRSlot2()) {
|
|
LogError(LOG_DMR, "DMR, invalid slot, slot 2 disabled, slotNo = %u", slotNo);
|
|
m_network->resetDMR(2U);
|
|
return;
|
|
}
|
|
|
|
bool dataSync = (buffer[15U] & 0x20U) == 0x20U;
|
|
bool voiceSync = (buffer[15U] & 0x10U) == 0x10U;
|
|
|
|
if (m_debug) {
|
|
LogDebug(LOG_NET, "DMR, seqNo = %u, srcId = %u, dstId = %u, flco = $%02X, slotNo = %u, len = %u", seqNo, srcId, dstId, flco, slotNo, length);
|
|
}
|
|
|
|
// process raw DMR data bytes
|
|
UInt8Array data = std::unique_ptr<uint8_t[]>(new uint8_t[DMR_FRAME_LENGTH_BYTES]);
|
|
::memset(data.get(), 0x00U, DMR_FRAME_LENGTH_BYTES);
|
|
DataType::E dataType = DataType::VOICE_SYNC;
|
|
|
|
if (dataSync) {
|
|
dataType = (DataType::E)(buffer[15U] & 0x0FU);
|
|
::memcpy(data.get(), buffer + 20U, DMR_FRAME_LENGTH_BYTES);
|
|
}
|
|
else if (voiceSync) {
|
|
::memcpy(data.get(), buffer + 20U, DMR_FRAME_LENGTH_BYTES);
|
|
}
|
|
else {
|
|
dataType = DataType::VOICE;
|
|
::memcpy(data.get(), buffer + 20U, DMR_FRAME_LENGTH_BYTES);
|
|
}
|
|
|
|
if (flco == FLCO::GROUP) {
|
|
if (srcId == 0) {
|
|
m_network->resetDMR(slotNo);
|
|
return;
|
|
}
|
|
|
|
// ensure destination ID matches and slot matches
|
|
if (dstId != m_srcTGId && dstId != m_dstTGId) {
|
|
m_network->resetDMR(slotNo);
|
|
return;
|
|
}
|
|
if (slotNo != m_srcSlot && slotNo != m_dstSlot) {
|
|
m_network->resetDMR(slotNo);
|
|
return;
|
|
}
|
|
|
|
uint32_t actualDstId = m_dstTGId;
|
|
if (m_twoWayPatch) {
|
|
if (dstId == m_dstTGId)
|
|
actualDstId = m_srcTGId;
|
|
} else {
|
|
if (dstId == m_dstTGId) {
|
|
m_network->resetDMR(slotNo);
|
|
return;
|
|
}
|
|
}
|
|
|
|
// is this a new call stream?
|
|
if (m_network->getDMRStreamId(slotNo) != m_rxStreamId) {
|
|
m_callInProgress = true;
|
|
|
|
uint64_t now = std::chrono::duration_cast<std::chrono::milliseconds>(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);
|
|
}
|
|
|
|
if (dataSync && (dataType == DataType::TERMINATOR_WITH_LC)) {
|
|
// generate DMR network frame
|
|
data::NetData dmrData;
|
|
dmrData.setSlotNo(m_dstSlot);
|
|
dmrData.setDataType(DataType::TERMINATOR_WITH_LC);
|
|
dmrData.setSrcId(srcId);
|
|
dmrData.setDstId(actualDstId);
|
|
dmrData.setFLCO(flco);
|
|
dmrData.setControl(controlByte);
|
|
|
|
uint8_t n = data[15U] & 0x0FU;
|
|
|
|
dmrData.setN(n);
|
|
dmrData.setSeqNo(seqNo);
|
|
dmrData.setBER(0U);
|
|
dmrData.setRSSI(0U);
|
|
|
|
dmrData.setData(data.get());
|
|
|
|
m_network->writeDMRTerminator(dmrData, &seqNo, &n, m_dmrEmbeddedData);
|
|
resetDMRCall(srcId, dmrData.getSlotNo());
|
|
return;
|
|
}
|
|
|
|
m_rxStreamId = m_network->getDMRStreamId(slotNo);
|
|
m_callDropTime.start();
|
|
|
|
uint8_t* buffer = nullptr;
|
|
|
|
// if we can, use the LC from the voice header as to keep all options intact
|
|
if (dataSync && (dataType == DataType::VOICE_LC_HEADER)) {
|
|
lc::LC lc = lc::LC();
|
|
lc::FullLC fullLC = lc::FullLC();
|
|
lc = *fullLC.decode(data.get(), DataType::VOICE_LC_HEADER);
|
|
|
|
LogInfoEx(LOG_HOST, DMR_DT_VOICE_LC_HEADER ", slot = %u, srcId = %u, dstId = %u, FLCO = $%02X", m_srcSlot,
|
|
lc.getSrcId(), lc.getDstId(), flco);
|
|
|
|
// send DMR voice header
|
|
buffer = new uint8_t[DMR_FRAME_LENGTH_BYTES];
|
|
|
|
lc.setDstId(actualDstId);
|
|
m_dmrEmbeddedData.setLC(lc);
|
|
|
|
// generate the Slot TYpe
|
|
SlotType slotType = SlotType();
|
|
slotType.setDataType(DataType::VOICE_LC_HEADER);
|
|
slotType.encode(buffer);
|
|
|
|
fullLC.encode(lc, buffer, DataType::VOICE_LC_HEADER);
|
|
|
|
// generate DMR network frame
|
|
data::NetData dmrData;
|
|
dmrData.setSlotNo(m_dstSlot);
|
|
dmrData.setDataType(DataType::VOICE_LC_HEADER);
|
|
dmrData.setSrcId(srcId);
|
|
dmrData.setDstId(actualDstId);
|
|
dmrData.setFLCO(flco);
|
|
dmrData.setControl(controlByte);
|
|
if (m_grantDemand) {
|
|
dmrData.setControl(0x80U); // DMR remote grant demand flag
|
|
} else {
|
|
dmrData.setControl(0U);
|
|
}
|
|
|
|
uint8_t n = data[15U] & 0x0FU;
|
|
|
|
dmrData.setN(n);
|
|
dmrData.setSeqNo(seqNo);
|
|
dmrData.setBER(0U);
|
|
dmrData.setRSSI(0U);
|
|
|
|
dmrData.setData(buffer);
|
|
|
|
m_network->writeDMR(dmrData, false);
|
|
delete[] buffer;
|
|
}
|
|
|
|
// if we can, use the PI LC from the PI voice header as to keep all options intact
|
|
if (dataSync && (dataType == DataType::VOICE_PI_HEADER)) {
|
|
lc::PrivacyLC lc = lc::PrivacyLC();
|
|
lc::FullLC fullLC = lc::FullLC();
|
|
lc = *fullLC.decodePI(data.get());
|
|
|
|
LogInfoEx(LOG_HOST, DMR_DT_VOICE_PI_HEADER ", slot = %u, algId = %u, kId = %u, dstId = %u", m_srcSlot,
|
|
lc.getAlgId(), lc.getKId(), lc.getDstId());
|
|
|
|
// send DMR voice header
|
|
buffer = new uint8_t[DMR_FRAME_LENGTH_BYTES];
|
|
|
|
lc.setDstId(actualDstId);
|
|
|
|
// generate the Slot TYpe
|
|
SlotType slotType = SlotType();
|
|
slotType.setDataType(DataType::VOICE_PI_HEADER);
|
|
slotType.encode(buffer);
|
|
|
|
fullLC.encodePI(lc, buffer);
|
|
|
|
// generate DMR network frame
|
|
data::NetData dmrData;
|
|
dmrData.setSlotNo(m_dstSlot);
|
|
dmrData.setDataType(DataType::VOICE_PI_HEADER);
|
|
dmrData.setSrcId(srcId);
|
|
dmrData.setDstId(actualDstId);
|
|
dmrData.setFLCO(flco);
|
|
dmrData.setControl(controlByte);
|
|
if (m_grantDemand) {
|
|
dmrData.setControl(0x80U); // DMR remote grant demand flag
|
|
} else {
|
|
dmrData.setControl(0U);
|
|
}
|
|
|
|
uint8_t n = data[15U] & 0x0FU;
|
|
|
|
dmrData.setN(n);
|
|
dmrData.setSeqNo(seqNo);
|
|
dmrData.setBER(0U);
|
|
dmrData.setRSSI(0U);
|
|
|
|
dmrData.setData(buffer);
|
|
|
|
m_network->writeDMR(dmrData, false);
|
|
delete[] buffer;
|
|
}
|
|
|
|
if (dataType == DataType::VOICE_SYNC || dataType == DataType::VOICE) {
|
|
// send DMR voice
|
|
buffer = new uint8_t[DMR_FRAME_LENGTH_BYTES];
|
|
::memcpy(buffer, data.get(), DMR_FRAME_LENGTH_BYTES);
|
|
|
|
uint8_t n = data[15U] & 0x0FU;
|
|
|
|
DataType::E dataType = DataType::VOICE_SYNC;
|
|
if (n == 0)
|
|
dataType = DataType::VOICE_SYNC;
|
|
else {
|
|
dataType = DataType::VOICE;
|
|
|
|
uint8_t lcss = m_dmrEmbeddedData.getData(buffer, n);
|
|
|
|
// generated embedded signalling
|
|
data::EMB emb = data::EMB();
|
|
emb.setColorCode(0U);
|
|
emb.setLCSS(lcss);
|
|
emb.encode(buffer);
|
|
}
|
|
|
|
LogInfoEx(LOG_HOST, DMR_DT_VOICE ", srcId = %u, dstId = %u, slot = %u, seqNo = %u", srcId, dstId, m_srcSlot, seqNo);
|
|
|
|
// generate DMR network frame
|
|
data::NetData dmrData;
|
|
dmrData.setSlotNo(m_dstSlot);
|
|
dmrData.setDataType(dataType);
|
|
dmrData.setSrcId(srcId);
|
|
dmrData.setDstId(actualDstId);
|
|
dmrData.setFLCO(flco);
|
|
dmrData.setN(n);
|
|
dmrData.setSeqNo(seqNo);
|
|
dmrData.setBER(0U);
|
|
dmrData.setRSSI(0U);
|
|
|
|
dmrData.setData(buffer);
|
|
|
|
m_network->writeDMR(dmrData, false);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Helper to process P25 network traffic. */
|
|
|
|
void HostPatch::processP25Network(uint8_t* buffer, uint32_t length)
|
|
{
|
|
assert(buffer != nullptr);
|
|
using namespace p25;
|
|
using namespace p25::defines;
|
|
using namespace p25::dfsi::defines;
|
|
|
|
if (m_digiMode != TX_MODE_P25) {
|
|
m_network->resetP25();
|
|
return;
|
|
}
|
|
|
|
bool grantDemand = (buffer[14U] & network::NET_CTRL_GRANT_DEMAND) == network::NET_CTRL_GRANT_DEMAND;
|
|
bool grantDenial = (buffer[14U] & network::NET_CTRL_GRANT_DENIAL) == network::NET_CTRL_GRANT_DENIAL;
|
|
bool unitToUnit = (buffer[14U] & network::NET_CTRL_U2U) == network::NET_CTRL_U2U;
|
|
|
|
// process network message header
|
|
DUID::E duid = (DUID::E)buffer[22U];
|
|
uint8_t MFId = buffer[15U];
|
|
|
|
if (duid == DUID::HDU || duid == DUID::TSDU || duid == DUID::PDU) {
|
|
m_network->resetP25();
|
|
return;
|
|
}
|
|
|
|
// process raw P25 data bytes
|
|
UInt8Array data;
|
|
uint8_t frameLength = buffer[23U];
|
|
if (duid == DUID::PDU) {
|
|
frameLength = length;
|
|
data = std::unique_ptr<uint8_t[]>(new uint8_t[length]);
|
|
::memset(data.get(), 0x00U, length);
|
|
::memcpy(data.get(), buffer, length);
|
|
}
|
|
else {
|
|
if (frameLength <= 24) {
|
|
data = std::unique_ptr<uint8_t[]>(new uint8_t[frameLength]);
|
|
::memset(data.get(), 0x00U, frameLength);
|
|
}
|
|
else {
|
|
data = std::unique_ptr<uint8_t[]>(new uint8_t[frameLength]);
|
|
::memset(data.get(), 0x00U, frameLength);
|
|
::memcpy(data.get(), buffer + 24U, frameLength);
|
|
}
|
|
}
|
|
|
|
// handle LDU, TDU or TSDU frame
|
|
uint8_t lco = buffer[4U];
|
|
|
|
uint32_t srcId = GET_UINT24(buffer, 5U);
|
|
uint32_t dstId = GET_UINT24(buffer, 8U);
|
|
|
|
uint8_t lsd1 = buffer[20U];
|
|
uint8_t lsd2 = buffer[21U];
|
|
|
|
FrameType::E frameType = (FrameType::E)buffer[180U];
|
|
|
|
lc::LC control;
|
|
data::LowSpeedData lsd;
|
|
|
|
control.setLCO(lco);
|
|
control.setSrcId(srcId);
|
|
control.setDstId(dstId);
|
|
control.setMFId(MFId);
|
|
|
|
if (!control.isStandardMFId()) {
|
|
control.setLCO(LCO::GROUP);
|
|
}
|
|
else {
|
|
if (control.getLCO() == LCO::GROUP_UPDT || control.getLCO() == LCO::RFSS_STS_BCAST) {
|
|
control.setLCO(LCO::GROUP);
|
|
}
|
|
}
|
|
|
|
lsd.setLSD1(lsd1);
|
|
lsd.setLSD2(lsd2);
|
|
|
|
if ((duid == DUID::TDU) || (duid == DUID::TDULC)) {
|
|
// ensure destination ID matches
|
|
if (dstId != m_srcTGId && dstId != m_dstTGId) {
|
|
// 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();
|
|
return;
|
|
}
|
|
|
|
// ensure destination ID matches
|
|
if (dstId != m_srcTGId && dstId != m_dstTGId) {
|
|
m_network->resetP25();
|
|
return;
|
|
}
|
|
|
|
bool reverseCall = false; // is the traffic flow reversed? (i.e. destination -> source instead of source -> destination)
|
|
bool skipCrypto = false;
|
|
uint32_t actualDstId = m_srcTGId;
|
|
|
|
if (!m_mmdvmP25Reflector) {
|
|
actualDstId = m_dstTGId;
|
|
if (m_twoWayPatch) {
|
|
// is this a reverse call?
|
|
if (dstId == m_dstTGId) {
|
|
actualDstId = m_srcTGId;
|
|
reverseCall = true;
|
|
}
|
|
} else {
|
|
if (dstId == m_dstTGId) {
|
|
m_network->resetP25();
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
// is this a new call stream?
|
|
uint16_t callKID = 0U;
|
|
if (m_network->getP25StreamId() != m_rxStreamId && ((duid != DUID::TDU) && (duid != DUID::TDULC))) {
|
|
m_callInProgress = true;
|
|
|
|
// if this is the beginning of a call and we have a valid HDU frame, extract the algo ID
|
|
uint8_t frameType = buffer[180U];
|
|
if (frameType == FrameType::HDU_VALID) {
|
|
m_callAlgoId = buffer[181U];
|
|
callKID = GET_UINT16(buffer, 182U);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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];
|
|
}
|
|
|
|
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::milliseconds>(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) {
|
|
p25::lc::LC lc = p25::lc::LC();
|
|
lc.setLCO(P25DEF::LCO::GROUP);
|
|
lc.setDstId(dstId);
|
|
lc.setSrcId(srcId);
|
|
|
|
p25::data::LowSpeedData lsd = p25::data::LowSpeedData();
|
|
|
|
uint8_t controlByte = network::NET_CTRL_GRANT_DEMAND; // Grant Demand Flag
|
|
if (m_callAlgoId != ALGO_UNENCRYPT)
|
|
controlByte |= network::NET_CTRL_GRANT_ENCRYPT; // Grant Encrypt Flag
|
|
m_network->writeP25TDU(lc, lsd, controlByte);
|
|
}
|
|
}
|
|
|
|
m_rxStreamId = m_network->getP25StreamId();
|
|
m_callDropTime.start();
|
|
|
|
uint8_t* netLDU = new uint8_t[9U * 25U];
|
|
::memset(netLDU, 0x00U, 9U * 25U);
|
|
|
|
if (m_debug)
|
|
{
|
|
// dump encryption MI's
|
|
uint8_t mi[MI_LENGTH_BYTES];
|
|
::memset(mi, 0x00U, MI_LENGTH_BYTES);
|
|
m_p25SrcCrypto->getMI(mi);
|
|
|
|
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]);
|
|
|
|
::memset(mi, 0x00U, MI_LENGTH_BYTES);
|
|
m_p25DstCrypto->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]);
|
|
}
|
|
|
|
int count = 0;
|
|
switch (duid)
|
|
{
|
|
case DUID::LDU1:
|
|
if ((data[0U] == DFSIFrameType::LDU1_VOICE1) && (data[22U] == DFSIFrameType::LDU1_VOICE2) &&
|
|
(data[36U] == DFSIFrameType::LDU1_VOICE3) && (data[53U] == DFSIFrameType::LDU1_VOICE4) &&
|
|
(data[70U] == DFSIFrameType::LDU1_VOICE5) && (data[87U] == DFSIFrameType::LDU1_VOICE6) &&
|
|
(data[104U] == DFSIFrameType::LDU1_VOICE7) && (data[121U] == DFSIFrameType::LDU1_VOICE8) &&
|
|
(data[138U] == DFSIFrameType::LDU1_VOICE9)) {
|
|
|
|
dfsi::LC dfsiLC = dfsi::LC(control, lsd);
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU1_VOICE1);
|
|
dfsiLC.decodeLDU1(data.get() + count, netLDU + 10U);
|
|
count += DFSI_LDU1_VOICE1_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU1_VOICE2);
|
|
dfsiLC.decodeLDU1(data.get() + count, netLDU + 26U);
|
|
count += DFSI_LDU1_VOICE2_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU1_VOICE3);
|
|
dfsiLC.decodeLDU1(data.get() + count, netLDU + 55U);
|
|
count += DFSI_LDU1_VOICE3_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU1_VOICE4);
|
|
dfsiLC.decodeLDU1(data.get() + count, netLDU + 80U);
|
|
count += DFSI_LDU1_VOICE4_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU1_VOICE5);
|
|
dfsiLC.decodeLDU1(data.get() + count, netLDU + 105U);
|
|
count += DFSI_LDU1_VOICE5_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU1_VOICE6);
|
|
dfsiLC.decodeLDU1(data.get() + count, netLDU + 130U);
|
|
count += DFSI_LDU1_VOICE6_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU1_VOICE7);
|
|
dfsiLC.decodeLDU1(data.get() + count, netLDU + 155U);
|
|
count += DFSI_LDU1_VOICE7_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU1_VOICE8);
|
|
dfsiLC.decodeLDU1(data.get() + count, netLDU + 180U);
|
|
count += DFSI_LDU1_VOICE8_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU1_VOICE9);
|
|
dfsiLC.decodeLDU1(data.get() + count, netLDU + 204U);
|
|
count += DFSI_LDU1_VOICE9_FRAME_LENGTH_BYTES;
|
|
|
|
LogInfoEx(LOG_NET, P25_LDU1_STR " audio, srcId = %u, dstId = %u", srcId, dstId);
|
|
|
|
control = lc::LC(*dfsiLC.control());
|
|
|
|
control.setSrcId(srcId);
|
|
control.setDstId(actualDstId);
|
|
|
|
// 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);
|
|
}
|
|
}
|
|
|
|
// 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);
|
|
|
|
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);
|
|
|
|
control.setMI(mi);
|
|
} else {
|
|
control.setAlgId(ALGO_UNENCRYPT);
|
|
control.setKId(0U);
|
|
}
|
|
}
|
|
} else {
|
|
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);
|
|
|
|
uint8_t mi[MI_LENGTH_BYTES];
|
|
::memset(mi, 0x00U, MI_LENGTH_BYTES);
|
|
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;
|
|
m_netLC = control;
|
|
|
|
writeNet_LDU1(false);
|
|
} else {
|
|
m_network->writeP25LDU1(control, lsd, netLDU, frameType);
|
|
}
|
|
}
|
|
break;
|
|
case DUID::LDU2:
|
|
if ((data[0U] == DFSIFrameType::LDU2_VOICE10) && (data[22U] == DFSIFrameType::LDU2_VOICE11) &&
|
|
(data[36U] == DFSIFrameType::LDU2_VOICE12) && (data[53U] == DFSIFrameType::LDU2_VOICE13) &&
|
|
(data[70U] == DFSIFrameType::LDU2_VOICE14) && (data[87U] == DFSIFrameType::LDU2_VOICE15) &&
|
|
(data[104U] == DFSIFrameType::LDU2_VOICE16) && (data[121U] == DFSIFrameType::LDU2_VOICE17) &&
|
|
(data[138U] == DFSIFrameType::LDU2_VOICE18)) {
|
|
|
|
dfsi::LC dfsiLC = dfsi::LC(control, lsd);
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU2_VOICE10);
|
|
dfsiLC.decodeLDU2(data.get() + count, netLDU + 10U);
|
|
count += DFSI_LDU2_VOICE10_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU2_VOICE11);
|
|
dfsiLC.decodeLDU2(data.get() + count, netLDU + 26U);
|
|
count += DFSI_LDU2_VOICE11_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU2_VOICE12);
|
|
dfsiLC.decodeLDU2(data.get() + count, netLDU + 55U);
|
|
count += DFSI_LDU2_VOICE12_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU2_VOICE13);
|
|
dfsiLC.decodeLDU2(data.get() + count, netLDU + 80U);
|
|
count += DFSI_LDU2_VOICE13_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU2_VOICE14);
|
|
dfsiLC.decodeLDU2(data.get() + count, netLDU + 105U);
|
|
count += DFSI_LDU2_VOICE14_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU2_VOICE15);
|
|
dfsiLC.decodeLDU2(data.get() + count, netLDU + 130U);
|
|
count += DFSI_LDU2_VOICE15_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU2_VOICE16);
|
|
dfsiLC.decodeLDU2(data.get() + count, netLDU + 155U);
|
|
count += DFSI_LDU2_VOICE16_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU2_VOICE17);
|
|
dfsiLC.decodeLDU2(data.get() + count, netLDU + 180U);
|
|
count += DFSI_LDU2_VOICE17_FRAME_LENGTH_BYTES;
|
|
|
|
dfsiLC.setFrameType(DFSIFrameType::LDU2_VOICE18);
|
|
dfsiLC.decodeLDU2(data.get() + count, netLDU + 204U);
|
|
count += DFSI_LDU2_VOICE18_FRAME_LENGTH_BYTES;
|
|
|
|
LogInfoEx(LOG_NET, P25_LDU2_STR " audio, algo = $%02X, kid = $%04X", dfsiLC.control()->getAlgId(), dfsiLC.control()->getKId());
|
|
|
|
control = lc::LC(*dfsiLC.control());
|
|
|
|
control.setSrcId(srcId);
|
|
control.setDstId(actualDstId);
|
|
|
|
// 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]);
|
|
}
|
|
|
|
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();
|
|
|
|
// 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_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);
|
|
|
|
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 {
|
|
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;
|
|
m_netLC = control;
|
|
|
|
writeNet_LDU2(false);
|
|
} else {
|
|
m_network->writeP25LDU2(control, lsd, netLDU);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case DUID::HDU:
|
|
case DUID::PDU:
|
|
case DUID::TDU:
|
|
case DUID::TDULC:
|
|
case DUID::TSDU:
|
|
case DUID::VSELP1:
|
|
case DUID::VSELP2:
|
|
default:
|
|
// this makes GCC happy
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* 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::milliseconds>(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;
|
|
|
|
if (m_callDstId == 0U) {
|
|
LogWarning(LOG_HOST, "P25, resetP25Call(), callDstId is zero, cannot send TDU");
|
|
|
|
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();
|
|
return;
|
|
}
|
|
|
|
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::milliseconds>(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_callDstId = 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)
|
|
{
|
|
assert(ldu != nullptr);
|
|
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;
|
|
|
|
//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);
|
|
|
|
// 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);
|
|
break;
|
|
case 1:
|
|
::memcpy(imbe, ldu + 26U, RAW_IMBE_LENGTH_BYTES);
|
|
break;
|
|
case 2:
|
|
::memcpy(imbe, ldu + 55U, RAW_IMBE_LENGTH_BYTES);
|
|
break;
|
|
case 3:
|
|
::memcpy(imbe, ldu + 80U, RAW_IMBE_LENGTH_BYTES);
|
|
break;
|
|
case 4:
|
|
::memcpy(imbe, ldu + 105U, RAW_IMBE_LENGTH_BYTES);
|
|
break;
|
|
case 5:
|
|
::memcpy(imbe, ldu + 130U, RAW_IMBE_LENGTH_BYTES);
|
|
break;
|
|
case 6:
|
|
::memcpy(imbe, ldu + 155U, RAW_IMBE_LENGTH_BYTES);
|
|
break;
|
|
case 7:
|
|
::memcpy(imbe, ldu + 180U, RAW_IMBE_LENGTH_BYTES);
|
|
break;
|
|
case 8:
|
|
::memcpy(imbe, ldu + 204U, RAW_IMBE_LENGTH_BYTES);
|
|
break;
|
|
}
|
|
|
|
// Utils::dump(1U, "P25, HostPatch::cryptP25AudioFrame(), IMBE", imbe, RAW_IMBE_LENGTH_BYTES);
|
|
|
|
/*
|
|
** 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);
|
|
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 && 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;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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);
|
|
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 && 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;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Helper to process a FNE KMM TEK response. */
|
|
|
|
void HostPatch::processTEKResponse(p25::kmm::KeyItem* ki, uint8_t algId, uint8_t keyLength)
|
|
{
|
|
if (ki == nullptr)
|
|
return;
|
|
|
|
if (algId == m_tekSrcAlgoId && ki->kId() == m_tekSrcKeyId) {
|
|
LogInfoEx(LOG_HOST, "Source TEK loaded, algId = $%02X, kId = $%04X, sln = $%04X", algId, ki->kId(), ki->sln());
|
|
UInt8Array tek = std::make_unique<uint8_t[]>(keyLength);
|
|
ki->getKey(tek.get());
|
|
|
|
m_p25SrcCrypto->setTEKAlgoId(algId);
|
|
m_p25SrcCrypto->setTEKKeyId(ki->kId());
|
|
m_p25SrcCrypto->setKey(tek.get(), keyLength);
|
|
}
|
|
|
|
if (algId == m_tekDstAlgoId && ki->kId() == m_tekDstKeyId) {
|
|
LogInfoEx(LOG_HOST, "Destination TEK loaded, algId = $%02X, kId = $%04X, sln = $%04X", algId, ki->kId(), ki->sln());
|
|
UInt8Array tek = std::make_unique<uint8_t[]>(keyLength);
|
|
ki->getKey(tek.get());
|
|
|
|
m_p25DstCrypto->setTEKAlgoId(algId);
|
|
m_p25DstCrypto->setTEKKeyId(ki->kId());
|
|
m_p25DstCrypto->setKey(tek.get(), keyLength);
|
|
}
|
|
}
|
|
|
|
/* Helper to check for an unflushed LDU1 packet. */
|
|
|
|
void HostPatch::checkNet_LDU1()
|
|
{
|
|
if (m_netState == RS_NET_IDLE)
|
|
return;
|
|
|
|
// check for an unflushed LDU1
|
|
if ((m_netLDU1[10U] != 0x00U || m_netLDU1[26U] != 0x00U || m_netLDU1[55U] != 0x00U ||
|
|
m_netLDU1[80U] != 0x00U || m_netLDU1[105U] != 0x00U || m_netLDU1[130U] != 0x00U ||
|
|
m_netLDU1[155U] != 0x00U || m_netLDU1[180U] != 0x00U || m_netLDU1[204U] != 0x00U) &&
|
|
m_gotNetLDU1)
|
|
writeNet_LDU1(false);
|
|
}
|
|
|
|
/* Helper to write a network P25 LDU1 packet. */
|
|
|
|
void HostPatch::writeNet_LDU1(bool toFNE)
|
|
{
|
|
using namespace p25;
|
|
using namespace p25::defines;
|
|
using namespace p25::dfsi::defines;
|
|
|
|
if (toFNE) {
|
|
if (m_netState == RS_NET_IDLE) {
|
|
m_callInProgress = true;
|
|
|
|
uint64_t now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
|
|
m_rxStartTime = now;
|
|
|
|
uint8_t lco = m_netLDU1[51U];
|
|
uint8_t mfId = m_netLDU1[52U];
|
|
uint32_t dstId = GET_UINT24(m_netLDU1, 76U);
|
|
uint32_t srcId = GET_UINT24(m_netLDU1, 101U);
|
|
|
|
LogInfoEx(LOG_HOST, "MMDVM P25, call start, srcId = %u, dstId = %u", srcId, dstId);
|
|
|
|
lc::LC lc = lc::LC();
|
|
m_netLC = lc;
|
|
m_netLC.setLCO(lco);
|
|
m_netLC.setMFId(mfId);
|
|
m_netLC.setDstId(dstId);
|
|
m_netLC.setSrcId(srcId);
|
|
|
|
if (m_grantDemand) {
|
|
p25::lc::LC lc = p25::lc::LC();
|
|
lc.setLCO(P25DEF::LCO::GROUP);
|
|
lc.setDstId(dstId);
|
|
lc.setSrcId(srcId);
|
|
|
|
p25::data::LowSpeedData lsd = p25::data::LowSpeedData();
|
|
|
|
uint8_t controlByte = network::NET_CTRL_GRANT_DEMAND; // Grant Demand Flag
|
|
m_network->writeP25TDU(lc, lsd, controlByte);
|
|
}
|
|
}
|
|
|
|
p25::data::LowSpeedData lsd = p25::data::LowSpeedData();
|
|
lsd.setLSD1(m_netLDU1[201U]);
|
|
lsd.setLSD2(m_netLDU1[202U]);
|
|
|
|
LogInfoEx(LOG_NET, "MMDVM " P25_LDU1_STR " audio, srcId = %u, dstId = %u", m_netLC.getSrcId(), m_netLC.getDstId());
|
|
|
|
if (m_debug)
|
|
Utils::dump(1U, "P25, HostPatch::writeNet_LDU1(), MMDVM -> DVM LDU1", m_netLDU1, 9U * 25U);
|
|
|
|
m_network->writeP25LDU1(m_netLC, lsd, m_netLDU1, FrameType::DATA_UNIT);
|
|
|
|
m_netState = RS_NET_AUDIO;
|
|
resetWithNullAudio(m_netLDU1, false);
|
|
m_gotNetLDU1 = false;
|
|
}
|
|
else {
|
|
if (m_debug)
|
|
Utils::dump(1U, "P25, HostPatch::writeNet_LDU1(), DVM -> MMDVM LDU1", m_netLDU1, 9U * 25U);
|
|
|
|
// add the Low Speed Data
|
|
p25::data::LowSpeedData lsd = p25::data::LowSpeedData();
|
|
lsd.setLSD1(m_netLDU1[201U]);
|
|
lsd.setLSD2(m_netLDU1[202U]);
|
|
|
|
m_mmdvmP25Net->writeLDU1(m_netLDU1, m_netLC, lsd, false);
|
|
|
|
resetWithNullAudio(m_netLDU1, false);
|
|
m_gotNetLDU1 = false;
|
|
}
|
|
}
|
|
|
|
/* Helper to check for an unflushed LDU2 packet. */
|
|
|
|
void HostPatch::checkNet_LDU2()
|
|
{
|
|
if (m_netState == RS_NET_IDLE)
|
|
return;
|
|
|
|
// check for an unflushed LDU2
|
|
if ((m_netLDU2[10U] != 0x00U || m_netLDU2[26U] != 0x00U || m_netLDU2[55U] != 0x00U ||
|
|
m_netLDU2[80U] != 0x00U || m_netLDU2[105U] != 0x00U || m_netLDU2[130U] != 0x00U ||
|
|
m_netLDU2[155U] != 0x00U || m_netLDU2[180U] != 0x00U || m_netLDU2[204U] != 0x00U) &&
|
|
m_gotNetLDU2) {
|
|
writeNet_LDU2(false);
|
|
}
|
|
}
|
|
|
|
/* Helper to write a network P25 LDU2 packet. */
|
|
|
|
void HostPatch::writeNet_LDU2(bool toFNE)
|
|
{
|
|
using namespace p25;
|
|
using namespace p25::defines;
|
|
using namespace p25::dfsi::defines;
|
|
|
|
if (toFNE) {
|
|
p25::data::LowSpeedData lsd = p25::data::LowSpeedData();
|
|
lsd.setLSD1(m_netLDU2[201U]);
|
|
lsd.setLSD2(m_netLDU2[202U]);
|
|
|
|
LogInfoEx(LOG_NET, "MMDVM " P25_LDU2_STR " audio");
|
|
|
|
if (m_debug)
|
|
Utils::dump(1U, "P25, HostPatch::writeNet_LDU2(), MMDVM -> DVM LDU2", m_netLDU2, 9U * 25U);
|
|
|
|
m_network->writeP25LDU2(m_netLC, lsd, m_netLDU2);
|
|
|
|
resetWithNullAudio(m_netLDU2, false);
|
|
m_gotNetLDU2 = false;
|
|
}
|
|
else {
|
|
if (m_debug)
|
|
Utils::dump(1U, "P25, HostPatch::writeNet_LDU2(), DVM -> MMDVM LDU2", m_netLDU2, 9U * 25U);
|
|
|
|
// add the Low Speed Data
|
|
p25::data::LowSpeedData lsd = p25::data::LowSpeedData();
|
|
lsd.setLSD1(m_netLDU2[201U]);
|
|
lsd.setLSD2(m_netLDU2[202U]);
|
|
|
|
m_mmdvmP25Net->writeLDU2(m_netLDU2, m_netLC, lsd, false);
|
|
|
|
resetWithNullAudio(m_netLDU2, false);
|
|
m_gotNetLDU2 = false;
|
|
}
|
|
}
|
|
|
|
/* Entry point to network processing thread. */
|
|
|
|
void* HostPatch::threadNetworkProcess(void* arg)
|
|
{
|
|
thread_t* th = (thread_t*)arg;
|
|
if (th != nullptr) {
|
|
#if defined(_WIN32)
|
|
::CloseHandle(th->thread);
|
|
#else
|
|
::pthread_detach(th->thread);
|
|
#endif // defined(_WIN32)
|
|
|
|
std::string threadName("patch:net-process");
|
|
HostPatch* patch = static_cast<HostPatch*>(th->obj);
|
|
if (patch == nullptr) {
|
|
g_killed = true;
|
|
LogError(LOG_HOST, "[FAIL] %s", threadName.c_str());
|
|
}
|
|
|
|
if (g_killed) {
|
|
delete th;
|
|
return nullptr;
|
|
}
|
|
|
|
LogInfoEx(LOG_HOST, "[ OK ] %s", threadName.c_str());
|
|
#ifdef _GNU_SOURCE
|
|
::pthread_setname_np(th->thread, threadName.c_str());
|
|
#endif // _GNU_SOURCE
|
|
|
|
while (!g_killed) {
|
|
if (!patch->m_running) {
|
|
Thread::sleep(1U);
|
|
continue;
|
|
}
|
|
|
|
if (patch->m_network->getStatus() == NET_STAT_RUNNING) {
|
|
// check if we need to request a TEK for the source TGID
|
|
if (patch->m_tekSrcAlgoId != P25DEF::ALGO_UNENCRYPT && patch->m_tekSrcKeyId > 0U) {
|
|
if (patch->m_p25SrcCrypto->getTEKLength() == 0U && !patch->m_requestedSrcTek) {
|
|
patch->m_requestedSrcTek = true;
|
|
LogInfoEx(LOG_HOST, "Patch source TGID encryption enabled, requesting TEK from network.");
|
|
patch->m_network->writeKeyReq(patch->m_tekSrcKeyId, patch->m_tekSrcAlgoId);
|
|
}
|
|
}
|
|
|
|
// check if we need to request a TEK for the destination TGID
|
|
if (patch->m_tekDstAlgoId != P25DEF::ALGO_UNENCRYPT && patch->m_tekDstKeyId > 0U) {
|
|
if (patch->m_p25DstCrypto->getTEKLength() == 0U && !patch->m_requestedDstTek) {
|
|
patch->m_requestedDstTek = true;
|
|
LogInfoEx(LOG_HOST, "Patch destination TGID encryption enabled, requesting TEK from network.");
|
|
patch->m_network->writeKeyReq(patch->m_tekDstKeyId, patch->m_tekDstAlgoId);
|
|
}
|
|
}
|
|
}
|
|
|
|
uint32_t length = 0U;
|
|
bool netReadRet = false;
|
|
if (patch->m_digiMode == TX_MODE_DMR) {
|
|
std::lock_guard<std::mutex> lock(HostPatch::s_networkMutex);
|
|
UInt8Array dmrBuffer = patch->m_network->readDMR(netReadRet, length);
|
|
if (netReadRet) {
|
|
patch->processDMRNetwork(dmrBuffer.get(), length);
|
|
}
|
|
}
|
|
|
|
if (patch->m_digiMode == TX_MODE_P25) {
|
|
std::lock_guard<std::mutex> lock(HostPatch::s_networkMutex);
|
|
UInt8Array p25Buffer = patch->m_network->readP25(netReadRet, length);
|
|
if (netReadRet) {
|
|
patch->processP25Network(p25Buffer.get(), length);
|
|
}
|
|
}
|
|
|
|
Thread::sleep(1U);
|
|
}
|
|
|
|
LogInfoEx(LOG_HOST, "[STOP] %s", threadName.c_str());
|
|
delete th;
|
|
}
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
/* Entry point to MMDVM network processing thread. */
|
|
|
|
void* HostPatch::threadMMDVMProcess(void* arg)
|
|
{
|
|
using namespace p25;
|
|
using namespace p25::defines;
|
|
using namespace p25::dfsi::defines;
|
|
|
|
thread_t* th = (thread_t*)arg;
|
|
if (th != nullptr) {
|
|
#if defined(_WIN32)
|
|
::CloseHandle(th->thread);
|
|
#else
|
|
::pthread_detach(th->thread);
|
|
#endif // defined(_WIN32)
|
|
|
|
std::string threadName("patch:mmdvm-net-process");
|
|
HostPatch* patch = static_cast<HostPatch*>(th->obj);
|
|
if (patch == nullptr) {
|
|
g_killed = true;
|
|
LogError(LOG_HOST, "[FAIL] %s", threadName.c_str());
|
|
}
|
|
|
|
if (g_killed) {
|
|
delete th;
|
|
return nullptr;
|
|
}
|
|
|
|
LogInfoEx(LOG_HOST, "[ OK ] %s", threadName.c_str());
|
|
#ifdef _GNU_SOURCE
|
|
::pthread_setname_np(th->thread, threadName.c_str());
|
|
#endif // _GNU_SOURCE
|
|
|
|
StopWatch stopWatch;
|
|
stopWatch.start();
|
|
|
|
while (!g_killed) {
|
|
if (!patch->m_running) {
|
|
Thread::sleep(1U);
|
|
continue;
|
|
}
|
|
|
|
uint32_t ms = stopWatch.elapsed();
|
|
|
|
ms = stopWatch.elapsed();
|
|
stopWatch.start();
|
|
|
|
if (patch->m_digiMode == TX_MODE_P25) {
|
|
std::lock_guard<std::mutex> lock(HostPatch::s_networkMutex);
|
|
|
|
DECLARE_UINT8_ARRAY(buffer, 100U);
|
|
uint32_t len = patch->m_mmdvmP25Net->read(buffer, 100U);
|
|
if (len != 0U) {
|
|
switch (buffer[0U]) {
|
|
// LDU1
|
|
case DFSIFrameType::LDU1_VOICE1:
|
|
::memcpy(patch->m_netLDU1 + 0U, buffer, 22U);
|
|
break;
|
|
case DFSIFrameType::LDU1_VOICE2:
|
|
::memcpy(patch->m_netLDU1 + 25U, buffer, 14U);
|
|
break;
|
|
case DFSIFrameType::LDU1_VOICE3:
|
|
::memcpy(patch->m_netLDU1 + 50U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU1_VOICE4:
|
|
::memcpy(patch->m_netLDU1 + 75U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU1_VOICE5:
|
|
::memcpy(patch->m_netLDU1 + 100U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU1_VOICE6:
|
|
::memcpy(patch->m_netLDU1 + 125U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU1_VOICE7:
|
|
::memcpy(patch->m_netLDU1 + 150U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU1_VOICE8:
|
|
::memcpy(patch->m_netLDU1 + 175U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU1_VOICE9:
|
|
::memcpy(patch->m_netLDU1 + 200U, buffer, 16U);
|
|
patch->checkNet_LDU2();
|
|
|
|
if (patch->m_netState != RS_NET_IDLE) {
|
|
patch->m_gotNetLDU1 = true;
|
|
patch->writeNet_LDU1(true);
|
|
}
|
|
break;
|
|
|
|
// LDU2
|
|
case DFSIFrameType::LDU2_VOICE10:
|
|
::memcpy(patch->m_netLDU2 + 0U, buffer, 22U);
|
|
break;
|
|
case DFSIFrameType::LDU2_VOICE11:
|
|
::memcpy(patch->m_netLDU2 + 25U, buffer, 14U);
|
|
break;
|
|
case DFSIFrameType::LDU2_VOICE12:
|
|
::memcpy(patch->m_netLDU2 + 50U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU2_VOICE13:
|
|
::memcpy(patch->m_netLDU2 + 75U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU2_VOICE14:
|
|
::memcpy(patch->m_netLDU2 + 100U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU2_VOICE15:
|
|
::memcpy(patch->m_netLDU2 + 125U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU2_VOICE16:
|
|
::memcpy(patch->m_netLDU2 + 150U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU2_VOICE17:
|
|
::memcpy(patch->m_netLDU2 + 175U, buffer, 17U);
|
|
break;
|
|
case DFSIFrameType::LDU2_VOICE18:
|
|
::memcpy(patch->m_netLDU2 + 200U, buffer, 16U);
|
|
if (patch->m_netState == RS_NET_IDLE) {
|
|
patch->writeNet_LDU1(true);
|
|
} else {
|
|
patch->checkNet_LDU1();
|
|
}
|
|
|
|
patch->writeNet_LDU2(true);
|
|
break;
|
|
|
|
case 0x80U:
|
|
{
|
|
patch->m_netState = RS_NET_IDLE;
|
|
|
|
p25::data::LowSpeedData lsd = p25::data::LowSpeedData();
|
|
|
|
LogInfoEx(LOG_HOST, "MMDVM " P25_TDU_STR);
|
|
|
|
uint8_t controlByte = 0x00U;
|
|
patch->m_network->writeP25TDU(patch->m_netLC, lsd, controlByte);
|
|
|
|
if (patch->m_rxStartTime > 0U) {
|
|
uint64_t now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
|
|
uint64_t diff = now - patch->m_rxStartTime;
|
|
|
|
LogInfoEx(LOG_HOST, "MMDVM P25, call end, srcId = %u, dstId = %u, dur = %us", patch->m_netLC.getSrcId(), patch->m_netLC.getDstId(), diff / 1000U);
|
|
}
|
|
|
|
patch->m_rxStartTime = 0U;
|
|
patch->m_rxStreamId = 0U;
|
|
|
|
patch->m_callInProgress = false;
|
|
patch->m_rxStartTime = 0U;
|
|
patch->m_rxStreamId = 0U;
|
|
}
|
|
break;
|
|
|
|
case 0xF0U:
|
|
case 0xF1U:
|
|
// these are MMDVM control bytes -- we ignore these
|
|
break;
|
|
|
|
default:
|
|
LogError(LOG_NET, "unknown opcode from MMDVM gateway $%02X", buffer[0U]);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (ms < 5U)
|
|
Thread::sleep(5U);
|
|
}
|
|
|
|
LogInfoEx(LOG_HOST, "[STOP] %s", threadName.c_str());
|
|
delete th;
|
|
}
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
/* Helper to reset IMBE buffer with null frames. */
|
|
|
|
void HostPatch::resetWithNullAudio(uint8_t* data, bool encrypted)
|
|
{
|
|
if (data == nullptr)
|
|
return;
|
|
|
|
// clear buffer for next sequence
|
|
::memset(data, 0x00U, 9U * 25U);
|
|
|
|
// fill with null
|
|
if (!encrypted) {
|
|
::memcpy(data + 10U, P25DEF::NULL_IMBE, 11U);
|
|
::memcpy(data + 26U, P25DEF::NULL_IMBE, 11U);
|
|
::memcpy(data + 55U, P25DEF::NULL_IMBE, 11U);
|
|
|
|
::memcpy(data + 80U, P25DEF::NULL_IMBE, 11U);
|
|
::memcpy(data + 105U, P25DEF::NULL_IMBE, 11U);
|
|
::memcpy(data + 130U, P25DEF::NULL_IMBE, 11U);
|
|
|
|
::memcpy(data + 155U, P25DEF::NULL_IMBE, 11U);
|
|
::memcpy(data + 180U, P25DEF::NULL_IMBE, 11U);
|
|
::memcpy(data + 204U, P25DEF::NULL_IMBE, 11U);
|
|
}
|
|
else {
|
|
::memcpy(data + 10U, P25DEF::ENCRYPTED_NULL_IMBE, 11U);
|
|
::memcpy(data + 26U, P25DEF::ENCRYPTED_NULL_IMBE, 11U);
|
|
::memcpy(data + 55U, P25DEF::ENCRYPTED_NULL_IMBE, 11U);
|
|
|
|
::memcpy(data + 80U, P25DEF::ENCRYPTED_NULL_IMBE, 11U);
|
|
::memcpy(data + 105U, P25DEF::ENCRYPTED_NULL_IMBE, 11U);
|
|
::memcpy(data + 130U, P25DEF::ENCRYPTED_NULL_IMBE, 11U);
|
|
|
|
::memcpy(data + 155U, P25DEF::ENCRYPTED_NULL_IMBE, 11U);
|
|
::memcpy(data + 180U, P25DEF::ENCRYPTED_NULL_IMBE, 11U);
|
|
::memcpy(data + 204U, P25DEF::ENCRYPTED_NULL_IMBE, 11U);
|
|
}
|
|
}
|