Digital_Voice_Modem_Project
/
dvmhost
mirror of https://github.com/DVMProject/dvmhost.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

implement support to capture and properly send HDU encryption data with the first LDU1 across the network;

Browse Source
3.0-rcon_maint 2023-03-12
Bryan Biedenkapp 3 years ago
parent 8e7a39a24f
commit 97403b5324
7 changed files with 137 additions and 46 deletions
Show Stats Download Patch File Download Diff File

71
network/BaseNetwork.cpp
Unescape View File

@ -186,9 +186,10 @@ bool BaseNetwork::readDMR(dmr::data::Data& data)
/// <param name="control"></param>
/// <param name="lsd"></param>
/// <param name="duid"></param>
/// <param name="frameType"></param>
/// <param name="len"></param>
/// <returns></returns>
uint8_t* BaseNetwork::readP25(bool& ret, p25::lc::LC& control, p25::data::LowSpeedData& lsd, uint8_t& duid, uint32_t& len)
uint8_t* BaseNetwork::readP25(bool& ret, p25::lc::LC& control, p25::data::LowSpeedData& lsd, uint8_t& duid, uint8_t& frameType, uint32_t& len)
{
if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING) {
ret = false;
@ -216,9 +217,40 @@ uint8_t* BaseNetwork::readP25(bool& ret, p25::lc::LC& control, p25::data::LowSpe
uint8_t lsd2 = m_buffer[21U];
duid = m_buffer[22U];
frameType = p25::P25_FT_DATA_UNIT;
if (m_debug) {
LogDebug(LOG_NET, "P25, lco = $%02X, MFId = $%02X, srcId = %u, dstId = %u, len = %u", lco, MFId, srcId, dstId, length);
LogDebug(LOG_NET, "P25, duid = $%02X, lco = $%02X, MFId = $%02X, srcId = %u, dstId = %u, len = %u", duid, lco, MFId, srcId, dstId, length);
}
// is this a LDU1, is this the first of a call?
if (duid == p25::P25_DUID_LDU1) {
frameType = m_buffer[180U];
if (m_debug) {
LogDebug(LOG_NET, "P25, frameType = $%02X", frameType);
}
if (frameType == p25::P25_FT_HDU_VALID) {
uint8_t algId = m_buffer[181U];
uint32_t kid = (m_buffer[182U] << 8) | (m_buffer[183U] << 0);
// copy MI data
uint8_t mi[p25::P25_MI_LENGTH_BYTES];
::memset(mi, 0x00U, p25::P25_MI_LENGTH_BYTES);
for (uint8_t i = 0; i < p25::P25_MI_LENGTH_BYTES; i++) {
mi[i] = m_buffer[184U + i];
}
if (m_debug) {
LogDebug(LOG_NET, "P25, HDU algId = $%02X, kId = $%02X", algId, kid);
}
control.setAlgId(algId);
control.setKId(kid);
control.setMI(mi);
}
}
control.setLCO(lco);
@ -345,8 +377,9 @@ bool BaseNetwork::writeDMR(const dmr::data::Data& data)
/// <param name="control"></param>
/// <param name="lsd"></param>
/// <param name="data"></param>
/// <param name="frameType"></param>
/// <returns></returns>
bool BaseNetwork::writeP25LDU1(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data)
bool BaseNetwork::writeP25LDU1(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data, uint8_t frameType)
{
if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING)
return false;
@ -357,7 +390,7 @@ bool BaseNetwork::writeP25LDU1(const p25::lc::LC& control, const p25::data::LowS
m_streamId[0] = m_p25StreamId;
return writeP25LDU1(m_id, m_p25StreamId, control, lsd, data);
return writeP25LDU1(m_id, m_p25StreamId, control, lsd, data, frameType);
}
/// <summary>
@ -678,9 +711,10 @@ bool BaseNetwork::writeDMR(const uint32_t id, const uint32_t streamId, const dmr
/// <param name="control"></param>
/// <param name="lsd"></param>
/// <param name="data"></param>
/// <param name="frameType"></param>
/// <returns></returns>
bool BaseNetwork::writeP25LDU1(const uint32_t id, const uint32_t streamId, const p25::lc::LC& control, const p25::data::LowSpeedData& lsd,
const uint8_t* data)
const uint8_t* data, uint8_t frameType)
{
if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING)
return false;
@ -713,6 +747,27 @@ bool BaseNetwork::writeP25LDU1(const uint32_t id, const uint32_t streamId, const
buffer[22U] = p25::P25_DUID_LDU1; // DUID
buffer[180U] = frameType; // DVM Frame Type
// is this the first frame of a call?
if (frameType == p25::P25_FT_HDU_VALID) {
buffer[180U] = 0x01U; // First LDU1 Marker
buffer[181U] = control.getAlgId(); // Algorithm ID
uint32_t kid = control.getKId();
buffer[182U] = (kid >> 8) & 0xFFU; // Key ID
buffer[183U] = (kid >> 0) & 0xFFU;
// copy MI data
uint8_t mi[p25::P25_MI_LENGTH_BYTES];
::memset(mi, 0x00U, p25::P25_MI_LENGTH_BYTES);
control.getMI(mi);
for (uint8_t i = 0; i < p25::P25_MI_LENGTH_BYTES; i++) {
buffer[184U + i] = mi[i]; // Message Indicator
}
}
uint32_t count = 24U;
uint8_t imbe[p25::P25_RAW_IMBE_LENGTH_BYTES];
@ -764,9 +819,9 @@ bool BaseNetwork::writeP25LDU1(const uint32_t id, const uint32_t streamId, const
buffer[23U] = count;
if (m_debug)
Utils::dump(1U, "Network Transmitted, P25 LDU1", buffer, (count + PACKET_PAD));
Utils::dump(1U, "Network Transmitted, P25 LDU1", buffer, (count + 15U + PACKET_PAD));
write(buffer, (count + PACKET_PAD));
write(buffer, (count + 15U + PACKET_PAD));
return true;
}
@ -1073,7 +1128,7 @@ bool BaseNetwork::writeNXDN(const uint32_t id, const uint32_t streamId, const nx
buffer[15U] |= lc.getGroup() ? 0x00U : 0x40U; // Group
__SET_UINT32(streamId, buffer, 16U); // Stream ID
__SET_UINT32(streamId, buffer, 16U); // Stream ID
uint32_t count = 24U;

6
network/BaseNetwork.h
Unescape View File

@ -135,14 +135,14 @@ namespace network
/// <summary>Reads DMR frame data from the DMR ring buffer.</summary>
virtual bool readDMR(dmr::data::Data& data);
/// <summary>Reads P25 frame data from the P25 ring buffer.</summary>
virtual uint8_t* readP25(bool& ret, p25::lc::LC& control, p25::data::LowSpeedData& lsd, uint8_t& duid, uint32_t& len);
virtual uint8_t* readP25(bool& ret, p25::lc::LC& control, p25::data::LowSpeedData& lsd, uint8_t& duid, uint8_t& frameType, uint32_t& len);
/// <summary>Reads NXDN frame data from the NXDN ring buffer.</summary>
virtual uint8_t* readNXDN(bool& ret, nxdn::lc::RTCH& lc, uint32_t& len);
/// <summary>Writes DMR frame data to the network.</summary>
virtual bool writeDMR(const dmr::data::Data& data);
/// <summary>Writes P25 LDU1 frame data to the network.</summary>
virtual bool writeP25LDU1(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data);
virtual bool writeP25LDU1(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data, uint8_t frameType);
/// <summary>Writes P25 LDU2 frame data to the network.</summary>
virtual bool writeP25LDU2(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data);
/// <summary>Writes P25 TDU frame data to the network.</summary>
@ -221,7 +221,7 @@ namespace network
/// <summary>Writes DMR frame data to the network.</summary>
bool writeDMR(const uint32_t id, const uint32_t streamId, const dmr::data::Data& data);
/// <summary>Writes P25 LDU1 frame data to the network.</summary>
bool writeP25LDU1(const uint32_t id, const uint32_t streamId, const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data);
bool writeP25LDU1(const uint32_t id, const uint32_t streamId, const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data, uint8_t frameType);
/// <summary>Writes P25 LDU2 frame data to the network.</summary>
bool writeP25LDU2(const uint32_t id, const uint32_t streamId, const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data);
/// <summary>Writes P25 TDU frame data to the network.</summary>

9
network/Network.cpp
Unescape View File

@ -202,9 +202,6 @@ void Network::clock(uint32_t ms)
return;
}
if (m_debug && length > 0)
Utils::dump(1U, "Network Received", m_buffer, length);
if (length > 0) {
if (!UDPSocket::match(m_addr, address)) {
LogError(LOG_NET, "Packet received from an invalid source");
@ -212,6 +209,7 @@ void Network::clock(uint32_t ms)
}
if (::memcmp(m_buffer, TAG_DMR_DATA, 4U) == 0) {
#if defined(ENABLE_DMR)
if (m_enabled && m_dmrEnabled) {
if (m_debug)
Utils::dump(1U, "Network Received, DMR", m_buffer, length);
@ -220,8 +218,10 @@ void Network::clock(uint32_t ms)
m_rxDMRData.addData(&len, 1U);
m_rxDMRData.addData(m_buffer, len);
}
#endif
}
else if (::memcmp(m_buffer, TAG_P25_DATA, 4U) == 0) {
#if defined(ENABLE_P25)
if (m_enabled && m_p25Enabled) {
if (m_debug)
Utils::dump(1U, "Network Received, P25", m_buffer, length);
@ -230,9 +230,10 @@ void Network::clock(uint32_t ms)
m_rxP25Data.addData(&len, 1U);
m_rxP25Data.addData(m_buffer, len);
}
#endif
}
else if (::memcmp(m_buffer, TAG_NXDN_DATA, 4U) == 0) {
#if ENABLE_NXDN_SUPPORT
#if defined(ENABLE_NXDN)
if (m_enabled && m_nxdnEnabled) {
if (m_debug)
Utils::dump(1U, "Network Received, NXDN", m_buffer, length);

11
p25/Control.cpp
Unescape View File

@ -1006,10 +1006,11 @@ void Control::processNetwork()
lc::LC control;
data::LowSpeedData lsd;
uint8_t duid;
uint8_t frameType;
uint32_t length = 100U;
bool ret = false;
uint8_t* data = m_network->readP25(ret, control, lsd, duid, length);
uint8_t* data = m_network->readP25(ret, control, lsd, duid, frameType, length);
if (!ret)
return;
if (length == 0U)
@ -1030,17 +1031,17 @@ void Control::processNetwork()
case P25_DUID_LDU1:
case P25_DUID_LDU2:
if (!m_dedicatedControl)
ret = m_voice->processNetwork(data, length, control, lsd, duid);
ret = m_voice->processNetwork(data, length, control, lsd, duid, frameType);
else {
if (m_voiceOnControl) {
ret = m_voice->processNetwork(data, length, control, lsd, duid);
ret = m_voice->processNetwork(data, length, control, lsd, duid, frameType);
}
}
break;
case P25_DUID_TDU:
case P25_DUID_TDULC:
m_voice->processNetwork(data, length, control, lsd, duid);
m_voice->processNetwork(data, length, control, lsd, duid, frameType);
break;
case P25_DUID_PDU:
@ -1048,7 +1049,7 @@ void Control::processNetwork()
ret = m_data->processNetwork(data, length, control, lsd, duid);
else {
if (m_voiceOnControl) {
ret = m_voice->processNetwork(data, length, control, lsd, duid);
ret = m_voice->processNetwork(data, length, control, lsd, duid, frameType);
}
}
break;

4
p25/P25Defines.h
Unescape View File

@ -211,6 +211,10 @@ namespace p25
const uint32_t DEFAULT_SILENCE_THRESHOLD = 124U;
const uint32_t MAX_P25_VOICE_ERRORS = 1233U;
const uint8_t P25_FT_HDU_VALID = 0x01U;
const uint8_t P25_FT_HDU_LATE_ENTRY = 0x02U;
const uint8_t P25_FT_DATA_UNIT = 0x00U;
// PDU Format Type(s)
const uint8_t PDU_FMT_RSP = 0x03U;
const uint8_t PDU_FMT_UNCONFIRMED = 0x15U;

77
p25/packet/Voice.cpp
Unescape View File

@ -88,6 +88,7 @@ void Voice::resetNet()
m_netLC = lc;
m_netLastLDU1 = lc;
//m_netLastFrameType = P25_FT_DATA_UNIT;
m_netFrames = 0U;
m_netLost = 0U;
@ -208,6 +209,7 @@ bool Voice::process(uint8_t* data, uint32_t len)
bool alreadyDecoded = false;
m_lastDUID = P25_DUID_LDU1;
uint8_t frameType = P25_FT_DATA_UNIT;
if (m_p25->m_rfState == RS_RF_LISTENING) {
// if this is a late entry call, clear states
if (m_rfLastHDU.getDstId() == 0U) {
@ -429,11 +431,14 @@ bool Voice::process(uint8_t* data, uint32_t len)
m_p25->addFrame(buffer, P25_HDU_FRAME_LENGTH_BYTES + 2U);
}
frameType = P25_FT_HDU_VALID;
if (m_verbose) {
LogMessage(LOG_RF, P25_HDU_STR ", dstId = %u, algo = $%02X, kid = $%04X", m_rfLC.getDstId(), m_rfLC.getAlgId(), m_rfLC.getKId());
}
}
else {
frameType = P25_FT_HDU_LATE_ENTRY;
LogWarning(LOG_RF, P25_HDU_STR ", not transmitted; possible late entry, dstId = %u, algo = $%02X, kid = $%04X", m_rfLastHDU.getDstId(), m_rfLastHDU.getAlgId(), m_rfLastHDU.getKId());
}
@ -555,7 +560,7 @@ bool Voice::process(uint8_t* data, uint32_t len)
// add busy bits
P25Utils::addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true);
writeNetwork(data + 2U, P25_DUID_LDU1);
writeNetwork(data + 2U, P25_DUID_LDU1, frameType);
if (m_p25->m_duplex) {
data[0U] = modem::TAG_DATA;
@ -723,8 +728,9 @@ bool Voice::process(uint8_t* data, uint32_t len)
/// <param name="control"></param>
/// <param name="lsd"></param>
/// <param name="duid"></param>
/// <param name="frameType"></param>
/// <returns></returns>
bool Voice::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::LowSpeedData& lsd, uint8_t& duid)
bool Voice::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::LowSpeedData& lsd, uint8_t& duid, uint8_t& frameType)
{
uint32_t count = 0U;
@ -775,6 +781,7 @@ bool Voice::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::L
count += 16U;
m_netLastLDU1 = control;
m_netLastFrameType = frameType;
if (m_p25->m_netState == RS_NET_IDLE) {
// are we interrupting a running CC?
@ -902,6 +909,7 @@ Voice::Voice(Control* p25, network::BaseNetwork* network, bool debug, bool verbo
m_rfLastLDU2(),
m_netLC(),
m_netLastLDU1(),
m_netLastFrameType(P25_FT_DATA_UNIT),
m_rfLSD(),
m_netLSD(),
m_dfsiLC(),
@ -941,7 +949,8 @@ Voice::~Voice()
/// </summary>
/// <param name="data"></param>
/// <param name="duid"></param>
void Voice::writeNetwork(const uint8_t *data, uint8_t duid)
/// <param name="frameType"></param>
void Voice::writeNetwork(const uint8_t *data, uint8_t duid, uint8_t frameType)
{
assert(data != nullptr);
@ -956,7 +965,7 @@ void Voice::writeNetwork(const uint8_t *data, uint8_t duid)
// ignore HDU
break;
case P25_DUID_LDU1:
m_network->writeP25LDU1(m_rfLC, m_rfLSD, data);
m_network->writeP25LDU1(m_rfLC, m_rfLSD, data, frameType);
break;
case P25_DUID_LDU2:
m_network->writeP25LDU2(m_rfLC, m_rfLSD, data);
@ -1082,24 +1091,24 @@ void Voice::writeNet_LDU1()
// ensure our srcId and dstId are sane from the last LDU1
if (m_netLastLDU1.getDstId() != 0U) {
if (dstId != m_netLastLDU1.getDstId()) {
LogWarning(LOG_NET, P25_HDU_STR ", dstId = %u doesn't match last LDU1 dstId = %u, fixing",
LogWarning(LOG_NET, P25_LDU1_STR ", dstId = %u doesn't match last LDU1 dstId = %u, fixing",
m_rfLC.getDstId(), m_rfLastLDU1.getDstId());
dstId = m_netLastLDU1.getDstId();
}
}
else {
LogWarning(LOG_NET, P25_HDU_STR ", last LDU1 LC has bad data, dstId = 0");
LogWarning(LOG_NET, P25_LDU1_STR ", last LDU1 LC has bad data, dstId = 0");
}
if (m_netLastLDU1.getSrcId() != 0U) {
if (srcId != m_netLastLDU1.getSrcId()) {
LogWarning(LOG_NET, P25_HDU_STR ", srcId = %u doesn't match last LDU1 srcId = %u, fixing",
LogWarning(LOG_NET, P25_LDU1_STR ", srcId = %u doesn't match last LDU1 srcId = %u, fixing",
m_rfLC.getSrcId(), m_rfLastLDU1.getSrcId());
srcId = m_netLastLDU1.getSrcId();
}
}
else {
LogWarning(LOG_NET, P25_HDU_STR ", last LDU1 LC has bad data, srcId = 0");
LogWarning(LOG_NET, P25_LDU1_STR ", last LDU1 LC has bad data, srcId = 0");
}
// don't process network frames if this modem isn't authoritative
@ -1172,7 +1181,17 @@ void Voice::writeNet_LDU1()
// if we are idle lets generate HDU data
if (m_p25->m_netState == RS_NET_IDLE) {
uint8_t mi[P25_MI_LENGTH_BYTES];
control.getMI(mi);
::memset(mi, 0x00U, P25_MI_LENGTH_BYTES);
if (m_netLastLDU1.getAlgId() != P25_ALGO_UNENCRYPT && m_netLastLDU1.getKId() != 0) {
m_netLastLDU1.getMI(mi);
control.setAlgId(m_netLastLDU1.getAlgId());
control.setKId(m_netLastLDU1.getKId());
}
else {
control.getMI(mi);
}
if (m_verbose && m_debug) {
Utils::dump(1U, "Network HDU MI", mi, P25_MI_LENGTH_BYTES);
@ -1229,6 +1248,7 @@ void Voice::writeNet_LDU1()
m_netLC = lc::LC();
m_netLastLDU1 = lc::LC();
m_netLastFrameType = P25_FT_DATA_UNIT;
m_p25->m_netState = RS_NET_IDLE;
m_p25->m_netLastDstId = 0U;
@ -1251,29 +1271,38 @@ void Voice::writeNet_LDU1()
m_netLost = 0U;
m_vocLDU1Count = 0U;
LogDebug(LOG_NET, "P25, frameType = $%02X", m_netLastFrameType);
if (!m_p25->m_disableNetworkHDU) {
uint8_t buffer[P25_HDU_FRAME_LENGTH_BYTES + 2U];
::memset(buffer, 0x00U, P25_HDU_FRAME_LENGTH_BYTES + 2U);
if (m_netLastFrameType != P25_FT_HDU_LATE_ENTRY) {
uint8_t buffer[P25_HDU_FRAME_LENGTH_BYTES + 2U];
::memset(buffer, 0x00U, P25_HDU_FRAME_LENGTH_BYTES + 2U);
// Generate Sync
Sync::addP25Sync(buffer + 2U);
// Generate Sync
Sync::addP25Sync(buffer + 2U);
// Generate NID
m_p25->m_nid.encode(buffer + 2U, P25_DUID_HDU);
// Generate NID
m_p25->m_nid.encode(buffer + 2U, P25_DUID_HDU);
// Generate header
m_netLC.encodeHDU(buffer + 2U);
// Generate header
m_netLC.encodeHDU(buffer + 2U);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false, true);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false, true);
buffer[0U] = modem::TAG_DATA;
buffer[1U] = 0x00U;
buffer[0U] = modem::TAG_DATA;
buffer[1U] = 0x00U;
m_p25->addFrame(buffer, P25_HDU_FRAME_LENGTH_BYTES + 2U, true);
m_p25->addFrame(buffer, P25_HDU_FRAME_LENGTH_BYTES + 2U, true);
if (m_verbose) {
LogMessage(LOG_NET, P25_HDU_STR ", dstId = %u, algo = $%02X, kid = $%04X", m_netLC.getDstId(), m_netLC.getAlgId(), m_netLC.getKId());
if (m_verbose) {
LogMessage(LOG_NET, P25_HDU_STR ", dstId = %u, algo = $%02X, kid = $%04X", m_netLC.getDstId(), m_netLC.getAlgId(), m_netLC.getKId());
}
}
else {
if (m_verbose) {
LogMessage(LOG_NET, P25_HDU_STR ", not transmitted; network HDU late entry, dstId = %u, algo = $%02X, kid = $%04X", m_netLC.getDstId(), m_netLC.getAlgId(), m_netLC.getKId());
}
}
}
else {

5
p25/packet/Voice.h
Unescape View File

@ -68,7 +68,7 @@ namespace p25
/// <summary>Process a data frame from the RF interface.</summary>
virtual bool process(uint8_t* data, uint32_t len);
/// <summary>Process a data frame from the network.</summary>
virtual bool processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::LowSpeedData& lsd, uint8_t& duid);
virtual bool processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::LowSpeedData& lsd, uint8_t& duid, uint8_t& frameType);
protected:
friend class packet::Trunk;
@ -93,6 +93,7 @@ namespace p25
lc::LC m_netLC;
lc::LC m_netLastLDU1;
uint8_t m_netLastFrameType;
data::LowSpeedData m_rfLSD;
data::LowSpeedData m_netLSD;
@ -120,7 +121,7 @@ namespace p25
virtual ~Voice();
/// <summary>Write data processed from RF to the network.</summary>
void writeNetwork(const uint8_t* data, uint8_t duid);
void writeNetwork(const uint8_t* data, uint8_t duid, uint8_t frameType = P25_FT_DATA_UNIT);
/// <summary>Helper to write end of voice frame data.</summary>
void writeRF_EndOfVoice();

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.