From 9050ad7885e1d3fdd152cd7f9b5851a9e9c09407 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Mon, 24 Jun 2024 11:48:42 -0400 Subject: [PATCH] this should address the issues with #53, dvmhost and dvmdfsi *should* transparently pass unknown non-standard MFId LCs in the voice LDU1 frames; correct some logging irregularities in dvmdfsi; --- src/common/p25/dfsi/LC.cpp | 120 ++++++++++++++++++----- src/common/p25/dfsi/LC.h | 7 +- src/common/p25/lc/LC.cpp | 149 +++++++++++++++++------------ src/common/p25/lc/LC.h | 11 ++- src/dfsi/network/CallData.cpp | 25 ++++- src/dfsi/network/CallData.h | 13 ++- src/dfsi/network/SerialService.cpp | 126 ++++++++++++++++++------ src/host/p25/packet/Voice.cpp | 34 +++++-- 8 files changed, 351 insertions(+), 134 deletions(-) diff --git a/src/common/p25/dfsi/LC.cpp b/src/common/p25/dfsi/LC.cpp index 77688e91..ee1efa79 100644 --- a/src/common/p25/dfsi/LC.cpp +++ b/src/common/p25/dfsi/LC.cpp @@ -33,15 +33,20 @@ using namespace p25::dfsi::defines; /// Initializes a new instance of the LC class. /// LC::LC() : + m_frameType(DFSIFrameType::LDU1_VOICE1), m_rssi(0U), m_control(nullptr), m_lsd(nullptr), m_rs(), + m_rsBuffer(nullptr), m_mi(nullptr) { m_mi = new uint8_t[MI_LENGTH_BYTES]; ::memset(m_mi, 0x00U, MI_LENGTH_BYTES); + m_rsBuffer = new uint8_t[P25_LDU_LC_FEC_LENGTH_BYTES]; + ::memset(m_rsBuffer, 0x00U, P25_LDU_LC_FEC_LENGTH_BYTES); + m_control = new lc::LC(); m_lsd = new data::LowSpeedData(); } @@ -76,6 +81,9 @@ LC::~LC() delete m_lsd; } delete[] m_mi; + if (m_rsBuffer != nullptr) { + delete[] m_rsBuffer; + } } /// @@ -115,7 +123,7 @@ bool LC::decodeLDU1(const uint8_t* data, uint8_t* imbe) assert(data != nullptr); assert(imbe != nullptr); - m_frameType = data[0U]; // Frame Type + m_frameType = (DFSIFrameType::E)data[0U]; // Frame Type // different frame types mean different things switch (m_frameType) @@ -131,6 +139,12 @@ bool LC::decodeLDU1(const uint8_t* data, uint8_t* imbe) m_lsd = new data::LowSpeedData(); } + if (m_rsBuffer != nullptr) { + delete m_rsBuffer; + } + m_rsBuffer = new uint8_t[P25_LDU_LC_FEC_LENGTH_BYTES]; + ::memset(m_rsBuffer, 0x00U, P25_LDU_LC_FEC_LENGTH_BYTES); + m_rssi = data[6U]; // RSSI ::memcpy(imbe, data + 10U, RAW_IMBE_LENGTH_BYTES); // IMBE } @@ -142,41 +156,69 @@ bool LC::decodeLDU1(const uint8_t* data, uint8_t* imbe) break; case DFSIFrameType::LDU1_VOICE3: { - m_control->setLCO(data[1U]); // LCO - m_control->setMFId(data[2U]); // MFId - uint8_t serviceOptions = (uint8_t)(data[3U]); // Service Options - m_control->setEmergency((serviceOptions & 0x80U) == 0x80U); - m_control->setEncrypted((serviceOptions & 0x40U) == 0x40U); - m_control->setPriority((serviceOptions & 0x07U)); - ::memcpy(imbe, data + 5U, RAW_IMBE_LENGTH_BYTES); // IMBE + m_rsBuffer[0U] = data[1U]; // LCO + m_control->setLCO(data[1U]); + m_rsBuffer[1U] = data[2U]; // MFId + m_control->setMFId(data[2U]); + + m_rsBuffer[2U] = data[3U]; + if (m_control->isStandardMFId()) { + uint8_t serviceOptions = (uint8_t)(data[3U]); // Service Options + m_control->setEmergency((serviceOptions & 0x80U) == 0x80U); + m_control->setEncrypted((serviceOptions & 0x40U) == 0x40U); + m_control->setPriority((serviceOptions & 0x07U)); + } + + ::memcpy(imbe, data + 5U, RAW_IMBE_LENGTH_BYTES); // IMBE } break; case DFSIFrameType::LDU1_VOICE4: { - uint32_t dstId = (data[1U] << 16) | (data[2U] << 8) | (data[3U] << 0); - m_control->setDstId(dstId); // Talkgroup Address + m_rsBuffer[3U] = data[1U]; + m_rsBuffer[4U] = data[2U]; + m_rsBuffer[5U] = data[3U]; + if (m_control->isStandardMFId()) { + uint32_t dstId = (data[1U] << 16) | (data[2U] << 8) | (data[3U] << 0); + m_control->setDstId(dstId); // Talkgroup Address + } + ::memcpy(imbe, data + 5U, RAW_IMBE_LENGTH_BYTES); // IMBE } break; case DFSIFrameType::LDU1_VOICE5: { - uint32_t srcId = (data[1U] << 16) | (data[2U] << 8) | (data[3U] << 0); - m_control->setSrcId(srcId); // Source Address + m_rsBuffer[6U] = data[1U]; + m_rsBuffer[7U] = data[2U]; + m_rsBuffer[8U] = data[3U]; + if (m_control->isStandardMFId()) { + uint32_t srcId = (data[1U] << 16) | (data[2U] << 8) | (data[3U] << 0); + m_control->setSrcId(srcId); // Source Address + } + ::memcpy(imbe, data + 5U, RAW_IMBE_LENGTH_BYTES); // IMBE } break; case DFSIFrameType::LDU1_VOICE6: { + m_rsBuffer[9U] = data[1U]; // RS (24,12,13) + m_rsBuffer[10U] = data[2U]; // RS (24,12,13) + m_rsBuffer[11U] = data[3U]; // RS (24,12,13) ::memcpy(imbe, data + 5U, RAW_IMBE_LENGTH_BYTES); // IMBE } break; case DFSIFrameType::LDU1_VOICE7: { + m_rsBuffer[12U] = data[1U]; // RS (24,12,13) + m_rsBuffer[13U] = data[2U]; // RS (24,12,13) + m_rsBuffer[14U] = data[3U]; // RS (24,12,13) ::memcpy(imbe, data + 5U, RAW_IMBE_LENGTH_BYTES); // IMBE } break; case DFSIFrameType::LDU1_VOICE8: { + m_rsBuffer[15U] = data[1U]; // RS (24,12,13) + m_rsBuffer[16U] = data[2U]; // RS (24,12,13) + m_rsBuffer[17U] = data[3U]; // RS (24,12,13) ::memcpy(imbe, data + 5U, RAW_IMBE_LENGTH_BYTES); // IMBE } break; @@ -195,6 +237,22 @@ bool LC::decodeLDU1(const uint8_t* data, uint8_t* imbe) break; } + // by LDU1_VOICE8 we should have all the pertinant RS bytes + if (m_frameType == DFSIFrameType::LDU1_VOICE8) { + ulong64_t rsValue = 0U; + + // combine bytes into ulong64_t (8 byte) value + rsValue = m_rsBuffer[1U]; + rsValue = (rsValue << 8) + m_rsBuffer[2U]; + rsValue = (rsValue << 8) + m_rsBuffer[3U]; + rsValue = (rsValue << 8) + m_rsBuffer[4U]; + rsValue = (rsValue << 8) + m_rsBuffer[5U]; + rsValue = (rsValue << 8) + m_rsBuffer[6U]; + rsValue = (rsValue << 8) + m_rsBuffer[7U]; + rsValue = (rsValue << 8) + m_rsBuffer[8U]; + m_control->setRS(rsValue); + } + return true; } @@ -256,17 +314,31 @@ void LC::encodeLDU1(uint8_t* data, const uint8_t* imbe) uint8_t rs[P25_LDU_LC_FEC_LENGTH_BYTES]; ::memset(rs, 0x00U, P25_LDU_LC_FEC_LENGTH_BYTES); - rs[0U] = m_control->getLCO(); // LCO - rs[1U] = m_control->getMFId(); // MFId - rs[2U] = serviceOptions; // Service Options - uint32_t dstId = m_control->getDstId(); - rs[3U] = (dstId >> 16) & 0xFFU; // Target Address - rs[4U] = (dstId >> 8) & 0xFFU; - rs[5U] = (dstId >> 0) & 0xFFU; - uint32_t srcId = m_control->getSrcId(); - rs[6U] = (srcId >> 16) & 0xFFU; // Source Address - rs[7U] = (srcId >> 8) & 0xFFU; - rs[8U] = (srcId >> 0) & 0xFFU; + if (m_control->isStandardMFId()) { + rs[0U] = m_control->getLCO(); // LCO + rs[1U] = m_control->getMFId(); // MFId + rs[2U] = serviceOptions; // Service Options + uint32_t dstId = m_control->getDstId(); + rs[3U] = (dstId >> 16) & 0xFFU; // Target Address + rs[4U] = (dstId >> 8) & 0xFFU; + rs[5U] = (dstId >> 0) & 0xFFU; + uint32_t srcId = m_control->getSrcId(); + rs[6U] = (srcId >> 16) & 0xFFU; // Source Address + rs[7U] = (srcId >> 8) & 0xFFU; + rs[8U] = (srcId >> 0) & 0xFFU; + } else { + rs[0U] = m_control->getLCO(); // LCO + + // split ulong64_t (8 byte) value into bytes + rs[1U] = (uint8_t)((m_control->getRS() >> 56) & 0xFFU); + rs[2U] = (uint8_t)((m_control->getRS() >> 48) & 0xFFU); + rs[3U] = (uint8_t)((m_control->getRS() >> 40) & 0xFFU); + rs[4U] = (uint8_t)((m_control->getRS() >> 32) & 0xFFU); + rs[5U] = (uint8_t)((m_control->getRS() >> 24) & 0xFFU); + rs[6U] = (uint8_t)((m_control->getRS() >> 16) & 0xFFU); + rs[7U] = (uint8_t)((m_control->getRS() >> 8) & 0xFFU); + rs[8U] = (uint8_t)((m_control->getRS() >> 0) & 0xFFU); + } // encode RS (24,12,13) FEC m_rs.encode241213(rs); @@ -368,7 +440,7 @@ bool LC::decodeLDU2(const uint8_t* data, uint8_t* imbe) assert(imbe != nullptr); assert(m_control != nullptr); - m_frameType = data[0U]; // Frame Type + m_frameType = (DFSIFrameType::E)data[0U]; // Frame Type // different frame types mean different things switch (m_frameType) diff --git a/src/common/p25/dfsi/LC.h b/src/common/p25/dfsi/LC.h index ce09d718..26c7786b 100644 --- a/src/common/p25/dfsi/LC.h +++ b/src/common/p25/dfsi/LC.h @@ -7,7 +7,7 @@ * @package DVM / Common Library * @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) * -* Copyright (C) 2022 Bryan Biedenkapp, N2PLL +* Copyright (C) 2022,2024 Bryan Biedenkapp, N2PLL * */ #if !defined(__P25_DFSI__LC_H__) @@ -15,6 +15,7 @@ #include "common/Defines.h" #include "common/p25/data/LowSpeedData.h" +#include "common/p25/dfsi/DFSIDefines.h" #include "common/p25/lc/LC.h" #include "common/edac/RS634717.h" @@ -59,7 +60,7 @@ namespace p25 public: /** Common Data */ /// Frame Type. - __PROPERTY(uint8_t, frameType, FrameType); + __PROPERTY(defines::DFSIFrameType::E, frameType, FrameType); /// RSSI. __PROPERTY(uint8_t, rssi, RSSI); @@ -72,6 +73,8 @@ namespace p25 private: edac::RS634717 m_rs; + uint8_t* m_rsBuffer; + /** Encryption data */ uint8_t* m_mi; diff --git a/src/common/p25/lc/LC.cpp b/src/common/p25/lc/LC.cpp index 3ee52ed9..640e59cf 100644 --- a/src/common/p25/lc/LC.cpp +++ b/src/common/p25/lc/LC.cpp @@ -60,11 +60,11 @@ LC::LC() : m_group(true), m_algId(ALGO_UNENCRYPT), m_kId(0U), + m_rsValue(0U), m_rs(), m_encryptOverride(false), m_tsbkVendorSkip(false), m_callTimer(0U), - m_rsValue(0U), m_mi(nullptr) { m_mi = new uint8_t[MI_LENGTH_BYTES]; @@ -463,6 +463,17 @@ void LC::encodeLDU2(uint8_t* data) #endif } +/// +/// Helper to determine if the MFId is a standard MFId. +/// +/// +bool LC::isStandardMFId() const +{ + if ((m_mfId == MFG_STANDARD) || (m_mfId == MFG_STANDARD_ALT)) + return true; + return false; +} + /** Encryption data */ /// Sets the encryption message indicator. /// @@ -492,6 +503,8 @@ void LC::getMI(uint8_t* mi) const /// void LC::copy(const LC& data) { + m_lco = data.m_lco; + m_protect = data.m_protect; m_mfId = data.m_mfId; @@ -565,15 +578,17 @@ bool LC::decodeLC(const uint8_t* rs) rsValue = (rsValue << 8) + rs[6U]; rsValue = (rsValue << 8) + rs[7U]; rsValue = (rsValue << 8) + rs[8U]; + m_rsValue = rsValue; m_protect = (rs[0U] & 0x80U) == 0x80U; // Protect Flag m_lco = rs[0U] & 0x3FU; // LCO m_mfId = rs[1U]; // Mfg Id. - // non-standard P25 vendor opcodes (these are just detected for passthru) + // non-standard P25 vendor opcodes (these are just detected for passthru, and stored + // as the packed RS value) if ((m_mfId != MFG_STANDARD) && (m_mfId != MFG_STANDARD_ALT)) { - m_rsValue = rsValue; + //Utils::dump(1U, "Decoded P25 Non-Standard RS", rs, P25_LDU_LC_FEC_LENGTH_BYTES); return true; } @@ -640,67 +655,70 @@ void LC::encodeLC(uint8_t* rs) ulong64_t rsValue = 0U; rs[0U] = m_lco; // LCO - // standard P25 reference opcodes - switch (m_lco) { - case LCO::GROUP: - rsValue = m_mfId; - rsValue = (rsValue << 8) + - (m_emergency ? 0x80U : 0x00U) + // Emergency Flag - (m_encrypted ? 0x40U : 0x00U) + // Encrypted Flag - (m_priority & 0x07U); // Priority - rsValue = (rsValue << 24) + m_dstId; // Talkgroup Address - rsValue = (rsValue << 24) + m_srcId; // Source Radio Address - break; - case LCO::GROUP_UPDT: - rs[0U] |= 0x40U; // Implicit Operation - rsValue = m_siteData.channelId(); // Group A - Channel ID - rsValue = (rsValue << 12) + m_grpVchNo; // Group A - Channel Number - rsValue = (rsValue << 16) + m_dstId; // Group A - Talkgroup Address - rsValue = (rsValue << 4) + m_siteData.channelId(); // Group B - Channel ID - rsValue = (rsValue << 12) + m_grpVchNoB; // Group B - Channel Number - rsValue = (rsValue << 16) + m_dstIdB; // Group B - Talkgroup Address - break; - case LCO::PRIVATE: - rsValue = m_mfId; - rsValue = (rsValue << 8) + - (m_emergency ? 0x80U : 0x00U) + // Emergency Flag - (m_encrypted ? 0x40U : 0x00U) + // Encrypted Flag - (m_priority & 0x07U); // Priority - rsValue = (rsValue << 24) + m_dstId; // Target Radio Address - rsValue = (rsValue << 24) + m_srcId; // Source Radio Address - break; - case LCO::TEL_INT_VCH_USER: - rs[0U] |= 0x40U; // Implicit Operation - rsValue = (rsValue << 8) + - (m_emergency ? 0x80U : 0x00U) + // Emergency Flag - (m_encrypted ? 0x40U : 0x00U) + // Encrypted Flag - (m_priority & 0x07U); // Priority - rsValue = (rsValue << 16) + m_callTimer; // Call Timer - rsValue = (rsValue << 24) + m_srcId; // Source/Target Radio Address - break; - case LCO::EXPLICIT_SOURCE_ID: - rsValue = m_netId; // Network ID - rsValue = (rsValue << 12) + (m_sysId & 0xFFFU); // System ID - rsValue = (rsValue << 24) + m_srcId; // Source Radio Address - break; - case LCO::RFSS_STS_BCAST: - rs[0U] |= 0x40U; // Implicit Operation - rsValue = m_siteData.lra(); // Location Registration Area - rsValue = (rsValue << 12) + m_siteData.sysId(); // System ID - rsValue = (rsValue << 8) + m_siteData.rfssId(); // RF Sub-System ID - rsValue = (rsValue << 8) + m_siteData.siteId(); // Site ID - rsValue = (rsValue << 4) + m_siteData.channelId(); // Channel ID - rsValue = (rsValue << 12) + m_siteData.channelNo(); // Channel Number - rsValue = (rsValue << 8) + m_siteData.serviceClass(); // System Service Class - break; - default: - LogError(LOG_P25, "LC::encodeLC(), unknown LC value, mfId = $%02X, lco = $%02X", m_mfId, m_lco); - break; - } + if ((m_mfId == MFG_STANDARD) || (m_mfId == MFG_STANDARD_ALT)) { + // standard P25 reference opcodes + switch (m_lco) { + case LCO::GROUP: + rsValue = m_mfId; + rsValue = (rsValue << 8) + + (m_emergency ? 0x80U : 0x00U) + // Emergency Flag + (m_encrypted ? 0x40U : 0x00U) + // Encrypted Flag + (m_priority & 0x07U); // Priority + rsValue = (rsValue << 24) + m_dstId; // Talkgroup Address + rsValue = (rsValue << 24) + m_srcId; // Source Radio Address + break; + case LCO::GROUP_UPDT: + rs[0U] |= 0x40U; // Implicit Operation + rsValue = m_siteData.channelId(); // Group A - Channel ID + rsValue = (rsValue << 12) + m_grpVchNo; // Group A - Channel Number + rsValue = (rsValue << 16) + m_dstId; // Group A - Talkgroup Address + rsValue = (rsValue << 4) + m_siteData.channelId(); // Group B - Channel ID + rsValue = (rsValue << 12) + m_grpVchNoB; // Group B - Channel Number + rsValue = (rsValue << 16) + m_dstIdB; // Group B - Talkgroup Address + break; + case LCO::PRIVATE: + rsValue = m_mfId; + rsValue = (rsValue << 8) + + (m_emergency ? 0x80U : 0x00U) + // Emergency Flag + (m_encrypted ? 0x40U : 0x00U) + // Encrypted Flag + (m_priority & 0x07U); // Priority + rsValue = (rsValue << 24) + m_dstId; // Target Radio Address + rsValue = (rsValue << 24) + m_srcId; // Source Radio Address + break; + case LCO::TEL_INT_VCH_USER: + rs[0U] |= 0x40U; // Implicit Operation + rsValue = (rsValue << 8) + + (m_emergency ? 0x80U : 0x00U) + // Emergency Flag + (m_encrypted ? 0x40U : 0x00U) + // Encrypted Flag + (m_priority & 0x07U); // Priority + rsValue = (rsValue << 16) + m_callTimer; // Call Timer + rsValue = (rsValue << 24) + m_srcId; // Source/Target Radio Address + break; + case LCO::EXPLICIT_SOURCE_ID: + rsValue = m_netId; // Network ID + rsValue = (rsValue << 12) + (m_sysId & 0xFFFU); // System ID + rsValue = (rsValue << 24) + m_srcId; // Source Radio Address + break; + case LCO::RFSS_STS_BCAST: + rs[0U] |= 0x40U; // Implicit Operation + rsValue = m_siteData.lra(); // Location Registration Area + rsValue = (rsValue << 12) + m_siteData.sysId(); // System ID + rsValue = (rsValue << 8) + m_siteData.rfssId(); // RF Sub-System ID + rsValue = (rsValue << 8) + m_siteData.siteId(); // Site ID + rsValue = (rsValue << 4) + m_siteData.channelId(); // Channel ID + rsValue = (rsValue << 12) + m_siteData.channelNo(); // Channel Number + rsValue = (rsValue << 8) + m_siteData.serviceClass(); // System Service Class + break; + default: + LogError(LOG_P25, "LC::encodeLC(), unknown LC value, mfId = $%02X, lco = $%02X", m_mfId, m_lco); + break; + } + } else { + if (m_rsValue == 0U) { + LogError(LOG_P25, "LC::encodeLC(), zero packed value, mfId = $%02X, lco = $%02X", m_mfId, m_lco); + } - // non-standard P25 vendor opcodes (these are just detected for passthru) - if ((m_mfId != MFG_STANDARD) && (m_mfId != MFG_STANDARD_ALT)) { - rsValue = 0U; + // non-standard P25 vendor opcodes (these are just passed from the packed RS) rsValue = m_rsValue; } @@ -713,6 +731,11 @@ void LC::encodeLC(uint8_t* rs) rs[6U] = (uint8_t)((rsValue >> 16) & 0xFFU); rs[7U] = (uint8_t)((rsValue >> 8) & 0xFFU); rs[8U] = (uint8_t)((rsValue >> 0) & 0xFFU); +/* + if ((m_mfId != MFG_STANDARD) && (m_mfId != MFG_STANDARD_ALT)) { + Utils::dump(1U, "Encoded P25 Non-Standard RS", rs, P25_LDU_LC_FEC_LENGTH_BYTES); + } +*/ } /// diff --git a/src/common/p25/lc/LC.h b/src/common/p25/lc/LC.h index 227357a0..28147b96 100644 --- a/src/common/p25/lc/LC.h +++ b/src/common/p25/lc/LC.h @@ -9,7 +9,7 @@ * @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) * * Copyright (C) 2016 Jonathan Naylor, G4KLX -* Copyright (C) 2017-2023 Bryan Biedenkapp, N2PLL +* Copyright (C) 2017-2024 Bryan Biedenkapp, N2PLL * */ #if !defined(__P25_LC__LC_H__) @@ -66,6 +66,9 @@ namespace p25 /// Encode a logical link data unit 2. void encodeLDU2(uint8_t* data); + /// Helper to determine if the MFId is a standard MFId. + bool isStandardMFId() const; + /** Encryption data */ /// Sets the encryption message indicator. void setMI(const uint8_t* mi); @@ -124,6 +127,10 @@ namespace p25 /// Encryption key ID. __PROPERTY(uint32_t, kId, KId); + /** Packed RS Data */ + /// Packed RS Data. + __PROPERTY(ulong64_t, rsValue, RS); + private: friend class TSBK; friend class TDULC; @@ -133,8 +140,6 @@ namespace p25 uint32_t m_callTimer; - ulong64_t m_rsValue; // used for certain transparent passthru LCOs - /** Encryption data */ uint8_t* m_mi; diff --git a/src/dfsi/network/CallData.cpp b/src/dfsi/network/CallData.cpp index b5298627..3b10c3f9 100644 --- a/src/dfsi/network/CallData.cpp +++ b/src/dfsi/network/CallData.cpp @@ -22,6 +22,13 @@ using namespace p25; using namespace p25::defines; using namespace dfsi; +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/// +/// Initializes a new instance of the VoiceCallData class. +/// VoiceCallData::VoiceCallData() : srcId(0U), dstId(0U), @@ -51,7 +58,11 @@ VoiceCallData::VoiceCallData() : ::memset(netLDU2, 0x00U, 9U * 25U); } -VoiceCallData::~VoiceCallData() { +/// +/// Finalizes a instance of the VoiceCallData class. +/// +VoiceCallData::~VoiceCallData() +{ delete[] mi; delete[] VHDR1; delete[] VHDR2; @@ -59,7 +70,11 @@ VoiceCallData::~VoiceCallData() { delete[] netLDU2; } -void VoiceCallData::resetCallData() { +/// +/// Reset call data to defaults. +/// +void VoiceCallData::resetCallData() +{ srcId = 0U; dstId = 0U; lco = 0U; @@ -84,7 +99,11 @@ void VoiceCallData::resetCallData() { streamId = 0U; } -void VoiceCallData::newStreamId() { +/// +/// Generate a new stream ID for a call. +/// +void VoiceCallData::newStreamId() +{ std::uniform_int_distribution dist(DVM_RAND_MIN, DVM_RAND_MAX); streamId = dist(random); } \ No newline at end of file diff --git a/src/dfsi/network/CallData.h b/src/dfsi/network/CallData.h index bed820bc..df47c8aa 100644 --- a/src/dfsi/network/CallData.h +++ b/src/dfsi/network/CallData.h @@ -37,16 +37,23 @@ // CPP includes #include -namespace network { +namespace network +{ + // --------------------------------------------------------------------------- + // Class Declaration + // Represents an on-going call. + // --------------------------------------------------------------------------- class HOST_SW_API VoiceCallData { - public: + /// Initializes a new instance of the VoiceCallData class. VoiceCallData(); + /// Initializes a new instance of the VoiceCallData class. ~VoiceCallData(); + /// Reset call data to defaults. void resetCallData(); - + /// Generate a new stream ID for a call. void newStreamId(); // Call Data diff --git a/src/dfsi/network/SerialService.cpp b/src/dfsi/network/SerialService.cpp index b8633f0c..a4a1e76e 100644 --- a/src/dfsi/network/SerialService.cpp +++ b/src/dfsi/network/SerialService.cpp @@ -9,6 +9,7 @@ * @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) * * Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL * */ @@ -442,8 +443,8 @@ void SerialService::processP25FromNet(UInt8Array p25Buffer, uint32_t length) control.setSrcId(srcId); control.setDstId(dstId); - LogInfoEx(LOG_NET, P25_LDU1_STR " audio, srcId = %u, dstId = %u, group = %u, emerg = %u, encrypt = %u, prio = %u", - control.getSrcId(), control.getDstId(), control.getGroup(), control.getEmergency(), control.getEncrypted(), control.getPriority()); + LogInfoEx(LOG_NET, P25_LDU1_STR " audio, mfId = $%02X, srcId = %u, dstId = %u, group = %u, emerg = %u, encrypt = %u, prio = %u", + control.getMFId(), control.getSrcId(), control.getDstId(), control.getGroup(), control.getEmergency(), control.getEncrypted(), control.getPriority()); //Utils::dump("P25 LDU1 from net", netLDU1, 9U * 25U); @@ -501,7 +502,7 @@ void SerialService::processP25FromNet(UInt8Array p25Buffer, uint32_t length) count += DFSI_LDU2_VOICE18_FRAME_LENGTH_BYTES; control = lc::LC(*dfsiLC.control()); - LogInfoEx(LOG_SERIAL, P25_LDU2_STR " audio, algo = $%02X, kid = $%04X", control.getAlgId(), control.getKId()); + LogInfoEx(LOG_NET, P25_LDU2_STR " audio, algo = $%02X, kid = $%04X", control.getAlgId(), control.getKId()); //Utils::dump("P25 LDU2 from net", netLDU2, 9U * 25U); @@ -906,9 +907,13 @@ void SerialService::processP25ToNet() // Get LC & LSD data if we're ready for either LDU1 or LDU2 (don't do this every frame to be more efficient) if (m_rxVoiceCallData->n == 9U || m_rxVoiceCallData->n == 18U) { + m_rxVoiceControl->setLCO(m_rxVoiceCallData->lco); + m_rxVoiceControl->setMFId(m_rxVoiceCallData->mfId); + // Create LC m_rxVoiceControl->setSrcId(m_rxVoiceCallData->srcId); m_rxVoiceControl->setDstId(m_rxVoiceCallData->dstId); + // Get service options bool emergency = ((m_rxVoiceCallData->serviceOptions & 0xFFU) & 0x80U) == 0x80U; // Emergency Flag bool encryption = ((m_rxVoiceCallData->serviceOptions & 0xFFU) & 0x40U) == 0x40U; // Encryption Flag @@ -916,13 +921,49 @@ void SerialService::processP25ToNet() m_rxVoiceControl->setEmergency(emergency); m_rxVoiceControl->setEncrypted(encryption); m_rxVoiceControl->setPriority(priority); + // Get more data m_rxVoiceControl->setMI(m_rxVoiceCallData->mi); m_rxVoiceControl->setAlgId(m_rxVoiceCallData->algoId); m_rxVoiceControl->setKId(m_rxVoiceCallData->kId); + // Get LSD m_rxVoiceLsd->setLSD1(m_rxVoiceCallData->lsd1); m_rxVoiceLsd->setLSD2(m_rxVoiceCallData->lsd2); + + // is this an LDU1 frame? + if (m_rxVoiceCallData->n == 9U) { + // is it a non-standard MFId for the LC? + if (!m_rxVoiceControl->isStandardMFId()) { + uint8_t rsBuffer[P25_LDU_LC_FEC_LENGTH_BYTES]; + ::memset(rsBuffer, 0x00U, P25_LDU_LC_FEC_LENGTH_BYTES); + + rsBuffer[0U] = m_rxVoiceCallData->lco; + rsBuffer[1U] = m_rxVoiceCallData->mfId; + rsBuffer[2U] = m_rxVoiceCallData->serviceOptions; + rsBuffer[3U] = 0U; + rsBuffer[4U] = (m_rxVoiceCallData->dstId >> 8) & 0xFFU; + rsBuffer[5U] = (m_rxVoiceCallData->dstId >> 0) & 0xFFU; + rsBuffer[6U] = (m_rxVoiceCallData->srcId >> 16) & 0xFFU; + rsBuffer[7U] = (m_rxVoiceCallData->srcId >> 8) & 0xFFU; + rsBuffer[8U] = (m_rxVoiceCallData->srcId >> 0) & 0xFFU; + + m_rs.decode241213(rsBuffer); + + ulong64_t rsValue = 0U; + + // combine bytes into ulong64_t (8 byte) value + rsValue = rsBuffer[1U]; + rsValue = (rsValue << 8) + rsBuffer[2U]; + rsValue = (rsValue << 8) + rsBuffer[3U]; + rsValue = (rsValue << 8) + rsBuffer[4U]; + rsValue = (rsValue << 8) + rsBuffer[5U]; + rsValue = (rsValue << 8) + rsBuffer[6U]; + rsValue = (rsValue << 8) + rsBuffer[7U]; + rsValue = (rsValue << 8) + rsBuffer[8U]; + m_rxVoiceControl->setRS(rsValue); + } + } } // Send LDU1 if ready @@ -930,7 +971,8 @@ void SerialService::processP25ToNet() // Send (TODO: dynamically set HDU_VALID or DATA_VALID depending on start of call or not) bool ret = m_network->writeP25LDU1(*m_rxVoiceControl, *m_rxVoiceLsd, m_rxVoiceCallData->netLDU1, FrameType::HDU_VALID); // Print - LogInfoEx(LOG_NET, P25_LDU1_STR " audio, srcId = %u, dstId = %u", m_rxVoiceCallData->srcId, m_rxVoiceCallData->dstId); + LogInfoEx(LOG_SERIAL, P25_LDU1_STR " audio, mfId = $%02X, srcId = %u, dstId = %u, group = %u, emerg = %u, encrypt = %u, prio = %u", + m_rxVoiceControl->getMFId(), m_rxVoiceControl->getSrcId(), m_rxVoiceControl->getDstId(), m_rxVoiceControl->getGroup(), m_rxVoiceControl->getEmergency(), m_rxVoiceControl->getEncrypted(), m_rxVoiceControl->getPriority()); // Optional Debug if (ret) { if (m_debug) @@ -1291,17 +1333,31 @@ void SerialService::writeP25Frame(DUID::E duid, dfsi::LC& lc, uint8_t* ldu) switch(duid) { case DUID::LDU1: { - rs[0U] = control.getLCO(); // LCO - rs[1U] = control.getMFId(); // MFId - rs[2U] = serviceOptions; // Service Options - uint32_t dstId = control.getDstId(); - rs[3U] = (dstId >> 16) & 0xFFU; // Target Address - rs[4U] = (dstId >> 8) & 0xFFU; - rs[5U] = (dstId >> 0) & 0xFFU; - uint32_t srcId = control.getSrcId(); - rs[6U] = (srcId >> 16) & 0xFFU; // Source Address - rs[7U] = (srcId >> 8) & 0xFFU; - rs[8U] = (srcId >> 0) & 0xFFU; + if (control.isStandardMFId()) { + rs[0U] = control.getLCO(); // LCO + rs[1U] = control.getMFId(); // MFId + rs[2U] = serviceOptions; // Service Options + uint32_t dstId = control.getDstId(); + rs[3U] = (dstId >> 16) & 0xFFU; // Target Address + rs[4U] = (dstId >> 8) & 0xFFU; + rs[5U] = (dstId >> 0) & 0xFFU; + uint32_t srcId = control.getSrcId(); + rs[6U] = (srcId >> 16) & 0xFFU; // Source Address + rs[7U] = (srcId >> 8) & 0xFFU; + rs[8U] = (srcId >> 0) & 0xFFU; + } else { + rs[0U] = control.getLCO(); // LCO + + // split ulong64_t (8 byte) value into bytes + rs[1U] = (uint8_t)((control.getRS() >> 56) & 0xFFU); + rs[2U] = (uint8_t)((control.getRS() >> 48) & 0xFFU); + rs[3U] = (uint8_t)((control.getRS() >> 40) & 0xFFU); + rs[4U] = (uint8_t)((control.getRS() >> 32) & 0xFFU); + rs[5U] = (uint8_t)((control.getRS() >> 24) & 0xFFU); + rs[6U] = (uint8_t)((control.getRS() >> 16) & 0xFFU); + rs[7U] = (uint8_t)((control.getRS() >> 8) & 0xFFU); + rs[8U] = (uint8_t)((control.getRS() >> 0) & 0xFFU); + } // encode RS (24,12,13) FEC m_rs.encode241213(rs); @@ -1355,7 +1411,7 @@ void SerialService::writeP25Frame(DUID::E duid, dfsi::LC& lc, uint8_t* ldu) m_sequences[control.getDstId()] = ++sequence; // Log - LogInfoEx(LOG_SERIAL, "CALL START: %svoice call from %u to TG %u", (control.getAlgId() != ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); + LogInfoEx(LOG_NET, "CALL START: %svoice call from %u to TG %u", (control.getAlgId() != ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); ActivityLog("network %svoice transmission call from %u to TG %u", (control.getAlgId() != ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); } else { @@ -1366,7 +1422,7 @@ void SerialService::writeP25Frame(DUID::E duid, dfsi::LC& lc, uint8_t* ldu) m_lastHeard[control.getDstId()] = now; m_sequences[control.getDstId()] = ++sequence; - LogInfoEx(LOG_SERIAL, "LATE CALL START: %svoice call from %u to TG %u", (control.getAlgId() != ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); + LogInfoEx(LOG_NET, "LATE CALL START: %svoice call from %u to TG %u", (control.getAlgId() != ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); ActivityLog("network %svoice transmission late entry from %u to TG %u", (control.getAlgId() != ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); } } @@ -1376,7 +1432,7 @@ void SerialService::writeP25Frame(DUID::E duid, dfsi::LC& lc, uint8_t* ldu) // Stop endOfStream(); // Log - LogInfoEx(LOG_SERIAL, "CALL END: %svoice call from %u to TG %u", (control.getAlgId() != ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); + LogInfoEx(LOG_NET, "CALL END: %svoice call from %u to TG %u", (control.getAlgId() != ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); // Clear our counters m_sequences[control.getDstId()] = RTP_END_OF_CALL_SEQ; } @@ -1397,18 +1453,24 @@ void SerialService::writeP25Frame(DUID::E duid, dfsi::LC& lc, uint8_t* ldu) { // Set frametype voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE1 : DFSIFrameType::LDU2_VOICE10); + // Create the new frame objects MotStartVoiceFrame svf = MotStartVoiceFrame(); + // Set values appropriately svf.startOfStream->setStartStop(StartStopFlag::START); svf.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + // Set frame type svf.fullRateVoice->setFrameType(voice.getFrameType()); + // Set source flag & ICW flag svf.fullRateVoice->setSource(m_diu ? SourceFlag::DIU : SourceFlag::QUANTAR); svf.setICW(m_diu ? ICWFlag::DIU : ICWFlag::QUANTAR); + // Copy data ::memcpy(svf.fullRateVoice->imbeData, ldu + 10U, RAW_IMBE_LENGTH_BYTES); + // Encode buffer = new uint8_t[svf.LENGTH]; ::memset(buffer, 0x00U, svf.LENGTH); @@ -1428,16 +1490,18 @@ void SerialService::writeP25Frame(DUID::E duid, dfsi::LC& lc, uint8_t* ldu) { voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE3 : DFSIFrameType::LDU2_VOICE12); ::memcpy(voice.imbeData, ldu + 55U, RAW_IMBE_LENGTH_BYTES); + // Create the additional data array voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + // Copy additional data if (voice.getFrameType() == DFSIFrameType::LDU1_VOICE3) { - voice.additionalData[0U] = control.getLCO(); - voice.additionalData[1U] = control.getMFId(); - voice.additionalData[2U] = serviceOptions; + voice.additionalData[0U] = control.getLCO(); // LCO + voice.additionalData[1U] = rs[1U]; // MFId + voice.additionalData[2U] = rs[2U]; // Service Options } else { - voice.additionalData[0U] = mi[0U]; + voice.additionalData[0U] = mi[0U]; // MI voice.additionalData[1U] = mi[1U]; voice.additionalData[2U] = mi[2U]; } @@ -1447,21 +1511,23 @@ void SerialService::writeP25Frame(DUID::E duid, dfsi::LC& lc, uint8_t* ldu) { voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE4 : DFSIFrameType::LDU2_VOICE13); ::memcpy(voice.imbeData, ldu + 80U, RAW_IMBE_LENGTH_BYTES); + // Create the additional data array voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + // We set the additional data based on LDU1/2 switch (duid) { case DUID::LDU1: { - // Destination address (3 bytes) - __SET_UINT16(control.getDstId(), voice.additionalData, 0U); + voice.additionalData[0U] = rs[3U]; // Destination ID + voice.additionalData[1U] = rs[4U]; + voice.additionalData[2U] = rs[5U]; } break; case DUID::LDU2: { - // Message Indicator - voice.additionalData[0U] = mi[3U]; + voice.additionalData[0U] = mi[3U]; // MI voice.additionalData[1U] = mi[4U]; voice.additionalData[2U] = mi[5U]; } @@ -1482,14 +1548,14 @@ void SerialService::writeP25Frame(DUID::E duid, dfsi::LC& lc, uint8_t* ldu) switch (duid) { case DUID::LDU1: { - // Source address (3 bytes) - __SET_UINT16(control.getSrcId(), voice.additionalData, 0U); + voice.additionalData[0U] = rs[6U]; // Source ID + voice.additionalData[1U] = rs[7U]; + voice.additionalData[2U] = rs[8U]; } break; case DUID::LDU2: { - // Message Indicator - voice.additionalData[0U] = mi[6U]; + voice.additionalData[0U] = mi[6U]; // MI voice.additionalData[1U] = mi[7U]; voice.additionalData[2U] = mi[8U]; } diff --git a/src/host/p25/packet/Voice.cpp b/src/host/p25/packet/Voice.cpp index 9a3a3933..2dbdee10 100644 --- a/src/host/p25/packet/Voice.cpp +++ b/src/host/p25/packet/Voice.cpp @@ -199,6 +199,7 @@ bool Voice::process(uint8_t* data, uint32_t len) bool alreadyDecoded = false; FrameType::E frameType = FrameType::DATA_UNIT; + ulong64_t rsValue = 0U; if (m_p25->m_rfState == RS_RF_LISTENING) { lc::LC lc = lc::LC(); bool ret = lc.decodeLDU1(data + 2U); @@ -206,6 +207,8 @@ bool Voice::process(uint8_t* data, uint32_t len) return false; } + rsValue = lc.getRS(); + uint32_t srcId = lc.getSrcId(); uint32_t dstId = lc.getDstId(); bool group = lc.getGroup(); @@ -644,6 +647,8 @@ bool Voice::process(uint8_t* data, uint32_t len) } } + rsValue = m_rfLC.getRS(); + alreadyDecoded = false; if (m_p25->m_enableControl) { @@ -660,6 +665,7 @@ bool Voice::process(uint8_t* data, uint32_t len) m_vocLDU1Count++; if (m_vocLDU1Count > VOC_LDU1_COUNT) { m_vocLDU1Count = 0U; + m_rfLC.setMFId(MFG_STANDARD); m_rfLC.setLCO(LCO::RFSS_STS_BCAST); } } @@ -671,6 +677,13 @@ bool Voice::process(uint8_t* data, uint32_t len) m_p25->m_nid.encode(data + 2U, DUID::LDU1); // generate LDU1 Data + if (!m_rfLC.isStandardMFId()) { + if (m_debug) { + LogDebug(LOG_RF, "P25, LDU1 LC, non-standard payload, lco = $%02X, mfId = $%02X", m_rfLC.getLCO(), m_rfLC.getMFId()); + } + m_rfLC.setRS(rsValue); + } + m_rfLC.encodeLDU1(data + 2U); // generate Low Speed Data @@ -724,8 +737,8 @@ bool Voice::process(uint8_t* data, uint32_t len) } if (m_verbose) { - LogMessage(LOG_RF, P25_LDU1_STR ", audio, srcId = %u, dstId = %u, group = %u, emerg = %u, encrypt = %u, prio = %u, errs = %u/1233 (%.1f%%)", - m_rfLC.getSrcId(), m_rfLC.getDstId(), m_rfLC.getGroup(), m_rfLC.getEmergency(), m_rfLC.getEncrypted(), m_rfLC.getPriority(), errors, float(errors) / 12.33F); + LogMessage(LOG_RF, P25_LDU1_STR ", audio, mfId = $%02X srcId = %u, dstId = %u, group = %u, emerg = %u, encrypt = %u, prio = %u, errs = %u/1233 (%.1f%%)", + m_rfLC.getMFId(), m_rfLC.getSrcId(), m_rfLC.getDstId(), m_rfLC.getGroup(), m_rfLC.getEmergency(), m_rfLC.getEncrypted(), m_rfLC.getPriority(), errors, float(errors) / 12.33F); } return true; @@ -1554,7 +1567,7 @@ void Voice::writeNet_LDU1() // ensure our dstId are sane from the last LDU1 if (m_netLastLDU1.getDstId() != 0U) { - if (dstId != m_netLastLDU1.getDstId()) { + if (dstId != m_netLastLDU1.getDstId() && control.isStandardMFId()) { if (m_verbose) { LogMessage(LOG_NET, P25_LDU1_STR ", dstId = %u doesn't match last LDU1 dstId = %u, fixing", dstId, m_netLastLDU1.getDstId()); @@ -1565,7 +1578,7 @@ void Voice::writeNet_LDU1() // ensure our srcId are sane from the last LDU1 if (m_netLastLDU1.getSrcId() != 0U) { - if (srcId != m_netLastLDU1.getSrcId()) { + if (srcId != m_netLastLDU1.getSrcId() && control.isStandardMFId()) { if (m_verbose) { LogMessage(LOG_NET, P25_LDU1_STR ", srcId = %u doesn't match last LDU1 srcId = %u, fixing", srcId, m_netLastLDU1.getSrcId()); @@ -1590,6 +1603,7 @@ void Voice::writeNet_LDU1() m_netLC.setEmergency(control.getEmergency()); m_netLC.setEncrypted(control.getEncrypted()); m_netLC.setPriority(control.getPriority()); + ulong64_t rsValue = control.getRS(); m_rfLC = lc::LC(); m_rfLC.setLCO(control.getLCO()); @@ -1823,6 +1837,7 @@ void Voice::writeNet_LDU1() m_vocLDU1Count++; if (m_vocLDU1Count > VOC_LDU1_COUNT) { m_vocLDU1Count = 0U; + m_netLC.setMFId(MFG_STANDARD); m_netLC.setLCO(LCO::RFSS_STS_BCAST); } } @@ -1839,6 +1854,13 @@ void Voice::writeNet_LDU1() m_p25->m_nid.encode(buffer + 2U, DUID::LDU1); // Generate LDU1 data + if (!m_netLC.isStandardMFId()) { + if (m_debug) { + LogDebug(LOG_NET, "P25, LDU1 LC, non-standard payload, lco = $%02X, mfId = $%02X", m_netLC.getLCO(), m_netLC.getMFId()); + } + m_netLC.setRS(rsValue); + } + m_netLC.encodeLDU1(buffer + 2U); // Add the Audio @@ -1866,8 +1888,8 @@ void Voice::writeNet_LDU1() m_p25->addFrame(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U, true); if (m_verbose) { - LogMessage(LOG_NET, P25_LDU1_STR " audio, srcId = %u, dstId = %u, group = %u, emerg = %u, encrypt = %u, prio = %u, sysId = $%03X, netId = $%05X", - m_netLC.getSrcId(), m_netLC.getDstId(), m_netLC.getGroup(), m_netLC.getEmergency(), m_netLC.getEncrypted(), m_netLC.getPriority(), + LogMessage(LOG_NET, P25_LDU1_STR " audio, mfId = $%02X, srcId = %u, dstId = %u, group = %u, emerg = %u, encrypt = %u, prio = %u, sysId = $%03X, netId = $%05X", + m_netLC.getMFId(), m_netLC.getSrcId(), m_netLC.getDstId(), m_netLC.getGroup(), m_netLC.getEmergency(), m_netLC.getEncrypted(), m_netLC.getPriority(), sysId, netId); }