diff --git a/network/BaseNetwork.cpp b/network/BaseNetwork.cpp index 50f4268c..d8a93e94 100644 --- a/network/BaseNetwork.cpp +++ b/network/BaseNetwork.cpp @@ -186,9 +186,10 @@ bool BaseNetwork::readDMR(dmr::data::Data& data) /// /// /// +/// /// /// -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) /// /// /// +/// /// -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); } /// @@ -678,9 +711,10 @@ bool BaseNetwork::writeDMR(const uint32_t id, const uint32_t streamId, const dmr /// /// /// +/// /// 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; diff --git a/network/BaseNetwork.h b/network/BaseNetwork.h index 4c79e800..7fbcb9e5 100644 --- a/network/BaseNetwork.h +++ b/network/BaseNetwork.h @@ -135,14 +135,14 @@ namespace network /// Reads DMR frame data from the DMR ring buffer. virtual bool readDMR(dmr::data::Data& data); /// Reads P25 frame data from the P25 ring buffer. - 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); /// Reads NXDN frame data from the NXDN ring buffer. virtual uint8_t* readNXDN(bool& ret, nxdn::lc::RTCH& lc, uint32_t& len); /// Writes DMR frame data to the network. virtual bool writeDMR(const dmr::data::Data& data); /// Writes P25 LDU1 frame data to the network. - 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); /// Writes P25 LDU2 frame data to the network. virtual bool writeP25LDU2(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data); /// Writes P25 TDU frame data to the network. @@ -221,7 +221,7 @@ namespace network /// Writes DMR frame data to the network. bool writeDMR(const uint32_t id, const uint32_t streamId, const dmr::data::Data& data); /// Writes P25 LDU1 frame data to the network. - 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); /// Writes P25 LDU2 frame data to the network. bool writeP25LDU2(const uint32_t id, const uint32_t streamId, const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data); /// Writes P25 TDU frame data to the network. diff --git a/network/Network.cpp b/network/Network.cpp index 68c106e8..e1ea2a5b 100644 --- a/network/Network.cpp +++ b/network/Network.cpp @@ -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); diff --git a/p25/Control.cpp b/p25/Control.cpp index 5a963be6..553690fe 100644 --- a/p25/Control.cpp +++ b/p25/Control.cpp @@ -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; diff --git a/p25/P25Defines.h b/p25/P25Defines.h index b341d814..c5aca261 100644 --- a/p25/P25Defines.h +++ b/p25/P25Defines.h @@ -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; diff --git a/p25/packet/Voice.cpp b/p25/packet/Voice.cpp index 748f95e7..7e6dc29f 100644 --- a/p25/packet/Voice.cpp +++ b/p25/packet/Voice.cpp @@ -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) /// /// /// +/// /// -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() /// /// /// -void Voice::writeNetwork(const uint8_t *data, uint8_t duid) +/// +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 { diff --git a/p25/packet/Voice.h b/p25/packet/Voice.h index ef2c8031..3b75401c 100644 --- a/p25/packet/Voice.h +++ b/p25/packet/Voice.h @@ -68,7 +68,7 @@ namespace p25 /// Process a data frame from the RF interface. virtual bool process(uint8_t* data, uint32_t len); /// Process a data frame from the network. - 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(); /// Write data processed from RF to the network. - 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); /// Helper to write end of voice frame data. void writeRF_EndOfVoice();