From 617c889d1ae9694056e45325d8f7d0b4125be879 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Mon, 22 Jul 2024 11:38:48 -0400 Subject: [PATCH] Merge DFSI host changes into master (#63) * begin implementing support for the V.24 modem as a regular modem dvmhost can utilize; * add Tx support (maybe); * theoretical TSBKs via V.24?; * set start of stream correctly; * ensure stream frame type is set correctly; send start of stream and end of stream correctly for TSBK (hopefully); * fix variable naming; ensure voice frames aren't repeated back to the V.24 source; * document configuration changes; * add framework for eventual V.24 UDP support; * correct issue not handling regular modem commands; * fix up issue writing V.24 data to the modem buffers; * initial working V24 modem implementation * update commenting/documentation; * reorganize configuration parameters and documentation; * whoops forgot comment; * ensure TSDU MBF is disabled for DFSI modems; tag DFSI parameters in log output proper for clarity; ensure P25 fixed mode is forced for DFSI modems; * fixup V.24 UDP port, properly fake certain modem responses, fix bad RTP transmissions; add STT_NON_IMBE_NO_JITTER message type for ignoring message jitter; * fix data offsets; * ensure DFSI's internal call timeout is settable; * bump version numbers; * use LOG_MODEM not LOG_SERIAL; * correct issue with PING/PONG resetting pktSeq incorrectly; add metric calculation for determining RTT for a PING/PONG; * use milliseconds instead of seconds for delta clocking on the FNE and host for networking; correct several issues with incorrect packet sequences being sent for some control packets; correct issue with tracked packet sequence possibly being incorrectly reset; --------- Co-authored-by: W3AXL <29879554+W3AXL@users.noreply.github.com> --- configs/config.example.yml | 19 + src/common/Defines.h | 13 +- src/common/network/BaseNetwork.cpp | 16 +- src/common/p25/dfsi/frames/FrameDefines.h | 3 +- src/common/p25/dfsi/frames/Frames.h | 1 + src/common/p25/dfsi/frames/MotTSBKFrame.cpp | 108 ++ src/common/p25/dfsi/frames/MotTSBKFrame.h | 95 + src/fne/network/FNENetwork.cpp | 55 +- src/fne/network/FNENetwork.h | 4 +- src/host/CMakeLists.txt | 2 + src/host/Host.Config.cpp | 42 +- src/host/Host.cpp | 27 + src/host/Host.h | 3 + src/host/modem/ModemV24.cpp | 1526 +++++++++++++++++ src/host/modem/ModemV24.h | 365 ++++ .../modem/port/specialized/V24UDPPort.cpp | 639 +++++++ src/host/modem/port/specialized/V24UDPPort.h | 229 +++ src/host/network/Network.cpp | 26 + src/host/p25/Control.cpp | 12 + src/host/p25/Control.h | 1 + src/host/p25/packet/Voice.cpp | 6 +- 21 files changed, 3153 insertions(+), 39 deletions(-) create mode 100644 src/common/p25/dfsi/frames/MotTSBKFrame.cpp create mode 100644 src/common/p25/dfsi/frames/MotTSBKFrame.h create mode 100644 src/host/modem/ModemV24.cpp create mode 100644 src/host/modem/ModemV24.h create mode 100644 src/host/modem/port/specialized/V24UDPPort.cpp create mode 100644 src/host/modem/port/specialized/V24UDPPort.h diff --git a/configs/config.example.yml b/configs/config.example.yml index 155c67c4..3185ad52 100644 --- a/configs/config.example.yml +++ b/configs/config.example.yml @@ -455,7 +455,13 @@ system: # protocol: # Modem port type. + # null - Null Modem (Loopback for testing) + # uart - Serial Modem type: "null" # Valid values are "null", and "uart" + # Modem interface mode. + # air - Standard air interface modem (Hotspot or Repeater) + # dfsi - TIA-102 DFSI interface modem + mode: "air" # Valid values are "air", and "dfsi" uart: # UART/RS232 serial port device. @@ -549,6 +555,19 @@ system: rssiCoarse: 127 # Valid values between 0 and 255 rssiFine: 127 # Valid values between 0 and 255 + # + # V.24 Modem Configuration + # + dfsi: + # RT/RT flag enabled (0x02) or disabled (0x04) + rtrt: true + # Use the DIU source flag (0x00) instead of the quantar source flag (0x02) + diu: true + # Jitter buffer length in ms + jitter: 200 + # Timer which will reset local/remote call flags if frames aren't received longer than this time in ms + callTimeout: 200 + # Sets received the signal offset from DC. rxDCOffset: 0 # Valid values between -128 and 128 # Sets transmitted the signal offset from DC. diff --git a/src/common/Defines.h b/src/common/Defines.h index 51467070..da996110 100644 --- a/src/common/Defines.h +++ b/src/common/Defines.h @@ -108,8 +108,8 @@ typedef unsigned long long ulong64_t; #define __EXE_NAME__ "" #define VERSION_MAJOR "04" -#define VERSION_MINOR "02" -#define VERSION_REV "C" +#define VERSION_MINOR "04" +#define VERSION_REV "D" #define __NETVER__ "DVM_R" VERSION_MAJOR VERSION_REV VERSION_MINOR #define __VER__ VERSION_MAJOR "." VERSION_MINOR VERSION_REV " (R" VERSION_MAJOR VERSION_REV VERSION_MINOR " " __GIT_VER__ ")" @@ -139,9 +139,12 @@ typedef unsigned long long ulong64_t; #define DEFAULT_CONF_FILE "config.yml" #define DEFAULT_LOCK_FILE "/tmp/dvm.lock" -#define NULL_PORT "null" -#define UART_PORT "uart" -#define PTY_PORT "pty" +#define NULL_PORT "null" +#define UART_PORT "uart" +#define PTY_PORT "pty" + +#define MODEM_MODE_AIR "air" +#define MODEM_MODE_DFSI "dfsi" const uint32_t REMOTE_MODEM_PORT = 3334; const uint32_t TRAFFIC_DEFAULT_PORT = 62031; diff --git a/src/common/network/BaseNetwork.cpp b/src/common/network/BaseNetwork.cpp index c4d79791..d0600043 100644 --- a/src/common/network/BaseNetwork.cpp +++ b/src/common/network/BaseNetwork.cpp @@ -102,7 +102,7 @@ bool BaseNetwork::writeGrantReq(const uint8_t mode, const uint32_t srcId, const buffer[20U] = mode; // DVM Mode State - return writeMaster({ NET_FUNC::GRANT_REQ, NET_SUBFUNC::NOP }, buffer, MSG_HDR_SIZE, 0U, 0U); + return writeMaster({ NET_FUNC::GRANT_REQ, NET_SUBFUNC::NOP }, buffer, MSG_HDR_SIZE, RTP_END_OF_CALL_SEQ, 0U); } /* Writes the local activity log to the network. */ @@ -123,7 +123,7 @@ bool BaseNetwork::writeActLog(const char* message) ::strcpy(buffer + 11U, message); return writeMaster({ NET_FUNC::TRANSFER, NET_SUBFUNC::TRANSFER_SUBFUNC_ACTIVITY }, (uint8_t*)buffer, (uint32_t)len + 12U, - 0U, 0U, false, m_useAlternatePortForDiagnostics); + RTP_END_OF_CALL_SEQ, 0U, false, m_useAlternatePortForDiagnostics); } /* Writes the local diagnostics log to the network. */ @@ -144,7 +144,7 @@ bool BaseNetwork::writeDiagLog(const char* message) ::strcpy(buffer + 11U, message); return writeMaster({ NET_FUNC::TRANSFER, NET_SUBFUNC::TRANSFER_SUBFUNC_DIAG }, (uint8_t*)buffer, (uint32_t)len + 12U, - 0U, 0U, false, m_useAlternatePortForDiagnostics); + RTP_END_OF_CALL_SEQ, 0U, false, m_useAlternatePortForDiagnostics); } /* Writes the local status to the network. */ @@ -184,7 +184,7 @@ bool BaseNetwork::announceGroupAffiliation(uint32_t srcId, uint32_t dstId) __SET_UINT16(srcId, buffer, 0U); __SET_UINT16(dstId, buffer, 3U); - return writeMaster({ NET_FUNC::ANNOUNCE, NET_SUBFUNC::ANNC_SUBFUNC_GRP_AFFIL }, buffer, MSG_ANNC_GRP_AFFIL, 0U, 0U); + return writeMaster({ NET_FUNC::ANNOUNCE, NET_SUBFUNC::ANNC_SUBFUNC_GRP_AFFIL }, buffer, MSG_ANNC_GRP_AFFIL, RTP_END_OF_CALL_SEQ, 0U); } /* Writes a unit registration to the network. */ @@ -198,7 +198,7 @@ bool BaseNetwork::announceUnitRegistration(uint32_t srcId) __SET_UINT16(srcId, buffer, 0U); - return writeMaster({ NET_FUNC::ANNOUNCE, NET_SUBFUNC::ANNC_SUBFUNC_UNIT_REG }, buffer, MSG_ANNC_UNIT_REG, 0U, 0U); + return writeMaster({ NET_FUNC::ANNOUNCE, NET_SUBFUNC::ANNC_SUBFUNC_UNIT_REG }, buffer, MSG_ANNC_UNIT_REG, RTP_END_OF_CALL_SEQ, 0U); } /* Writes a unit deregistration to the network. */ @@ -212,7 +212,7 @@ bool BaseNetwork::announceUnitDeregistration(uint32_t srcId) __SET_UINT16(srcId, buffer, 0U); - return writeMaster({ NET_FUNC::ANNOUNCE, NET_SUBFUNC::ANNC_SUBFUNC_UNIT_DEREG }, buffer, MSG_ANNC_UNIT_REG, 0U, 0U); + return writeMaster({ NET_FUNC::ANNOUNCE, NET_SUBFUNC::ANNC_SUBFUNC_UNIT_DEREG }, buffer, MSG_ANNC_UNIT_REG, RTP_END_OF_CALL_SEQ, 0U); } /* Writes a complete update of the peer affiliation list to the network. */ @@ -235,7 +235,7 @@ bool BaseNetwork::announceAffiliationUpdate(const std::unordered_map peers) offs += 4U; } - return writeMaster({ NET_FUNC::ANNOUNCE, NET_SUBFUNC::ANNC_SUBFUNC_SITE_VC }, buffer, 4U + (peers.size() * 4U), 0U, 0U); + return writeMaster({ NET_FUNC::ANNOUNCE, NET_SUBFUNC::ANNC_SUBFUNC_SITE_VC }, buffer, 4U + (peers.size() * 4U), RTP_END_OF_CALL_SEQ, 0U); } /* Resets the DMR ring buffer for the given slot. */ diff --git a/src/common/p25/dfsi/frames/FrameDefines.h b/src/common/p25/dfsi/frames/FrameDefines.h index b78991b8..4798cab8 100644 --- a/src/common/p25/dfsi/frames/FrameDefines.h +++ b/src/common/p25/dfsi/frames/FrameDefines.h @@ -107,7 +107,8 @@ namespace p25 namespace StreamTypeFlag { /** @brief V.24 Data Stream Type */ enum E : uint8_t { - VOICE = 0x0BU //! Voice + VOICE = 0x0BU, //! Voice + TSBK = 0x0FU //! TSBK }; } diff --git a/src/common/p25/dfsi/frames/Frames.h b/src/common/p25/dfsi/frames/Frames.h index 803c981e..0732a1d4 100644 --- a/src/common/p25/dfsi/frames/Frames.h +++ b/src/common/p25/dfsi/frames/Frames.h @@ -25,6 +25,7 @@ #include "common/p25/dfsi/frames/MotStartVoiceFrame.h" #include "common/p25/dfsi/frames/MotVoiceHeader1.h" #include "common/p25/dfsi/frames/MotVoiceHeader2.h" +#include "common/p25/dfsi/frames/MotTSBKFrame.h" // FSC #include "common/p25/dfsi/frames/fsc/FSCMessage.h" diff --git a/src/common/p25/dfsi/frames/MotTSBKFrame.cpp b/src/common/p25/dfsi/frames/MotTSBKFrame.cpp new file mode 100644 index 00000000..03b91e60 --- /dev/null +++ b/src/common/p25/dfsi/frames/MotTSBKFrame.cpp @@ -0,0 +1,108 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Common Library + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * + */ +#include "common/p25/P25Defines.h" +#include "common/p25/dfsi/frames/MotTSBKFrame.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/Utils.h" +#include "common/Log.h" + +#include +#include + +using namespace p25; +using namespace p25::defines; +using namespace p25::dfsi; +using namespace p25::dfsi::defines; +using namespace p25::dfsi::frames; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/* Initializes a instance of the MotTSBKFrame class. */ + +MotTSBKFrame::MotTSBKFrame() : + startOfStream(nullptr), + tsbkData(nullptr) +{ + tsbkData = new uint8_t[P25_TSBK_LENGTH_BYTES]; + ::memset(tsbkData, 0x00U, P25_TSBK_LENGTH_BYTES); + + startOfStream = new MotStartOfStream(); +} + +/* Initializes a instance of the MotTSBKFrame class. */ + +MotTSBKFrame::MotTSBKFrame(uint8_t* data) : + startOfStream(nullptr), + tsbkData(nullptr) +{ + tsbkData = new uint8_t[P25_TSBK_LENGTH_BYTES]; + ::memset(tsbkData, 0x00U, P25_TSBK_LENGTH_BYTES); + + decode(data); +} + +/* Finalizes a instance of the MotTSBKFrame class. */ + +MotTSBKFrame::~MotTSBKFrame() +{ + if (startOfStream != nullptr) + delete startOfStream; + if (tsbkData != nullptr) + delete[] tsbkData; +} + +/* Decode a TSBK frame. */ + +bool MotTSBKFrame::decode(const uint8_t* data) +{ + assert(data != nullptr); + + // create a new start of stream + if (startOfStream != nullptr) + delete startOfStream; + startOfStream = new MotStartOfStream(); + + // create a buffer to decode the start record skipping the 10th byte (adjMM) + uint8_t startBuffer[MotStartOfStream::LENGTH]; + ::memset(startBuffer, 0x00U, MotStartOfStream::LENGTH); + ::memcpy(startBuffer + 1U, data, 4U); + + // decode start of stream + startOfStream->decode(startBuffer); + + ::memcpy(tsbkData, data + 8U, P25_TSBK_LENGTH_BYTES); + + return true; +} + +/* Encode a TSBK frame. */ + +void MotTSBKFrame::encode(uint8_t* data) +{ + assert(data != nullptr); + assert(startOfStream != nullptr); + + // encode start of stream - scope is intentional + { + uint8_t buffer[MotStartOfStream::LENGTH]; + startOfStream->encode(buffer); + + // copy to data array (skipping first and last bytes) + ::memcpy(data + 1U, buffer + 1U, 4U); + } + + // encode TSBK - scope is intentional + { + data[0U] = DFSIFrameType::TSBK; + ::memcpy(data + 8U, tsbkData, P25_TSBK_LENGTH_BYTES); + } +} diff --git a/src/common/p25/dfsi/frames/MotTSBKFrame.h b/src/common/p25/dfsi/frames/MotTSBKFrame.h new file mode 100644 index 00000000..15db4872 --- /dev/null +++ b/src/common/p25/dfsi/frames/MotTSBKFrame.h @@ -0,0 +1,95 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Common Library + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * + */ +/** + * @file MotTSBKFrame.h + * @ingroup dfsi_frames + * @file MotTSBKFrame.cpp + * @ingroup dfsi_frames + */ +#if !defined(__MOT_TSBK_FRAME_H__) +#define __MOT_TSBK_FRAME_H__ + +#include "Defines.h" +#include "common/Defines.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "common/p25/dfsi/frames/FrameDefines.h" +#include "common/p25/dfsi/frames/MotStartOfStream.h" +#include "common/p25/dfsi/frames/MotFullRateVoice.h" + +namespace p25 +{ + namespace dfsi + { + namespace frames + { + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Implements a P25 Motorola TSBK frame. + * \code{.unparsed} + * Byte 0 1 2 3 + * Bit 7 6 5 4 3 2 1 0 7 6 5 4 3 2 1 0 7 6 5 4 3 2 1 0 7 6 5 4 3 2 1 0 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Encoded Motorola Start of Stream | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Reserved ? | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | TSBK | + * + + + * | | + * + + + * | | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Unknown ? | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * \endcode + * @ingroup dfsi_frames + */ + class HOST_SW_API MotTSBKFrame { + public: + static const uint8_t LENGTH = 24; + + /** + * @brief Initializes a copy instance of the MotTSBKFrame class. + */ + MotTSBKFrame(); + /** + * @brief Initializes a copy instance of the MotTSBKFrame class. + * @param data Buffer to containing MotTSBKFrame to decode. + */ + MotTSBKFrame(uint8_t* data); + /** + * @brief Finalizes a instance of the MotTSBKFrame class. + */ + ~MotTSBKFrame(); + + /** + * @brief Decode a TSBK frame. + * @param[in] data Buffer to containing MotTSBKFrame to decode. + */ + bool decode(const uint8_t* data); + /** + * @brief Encode a TSBK frame. + * @param[out] data Buffer to encode a MotTSBKFrame. + */ + void encode(uint8_t* data); + + public: + MotStartOfStream* startOfStream; // ?? - this should probably be private with getters/setters + uint8_t* tsbkData; // ?? - this should probably be private with getters/setters + }; + } // namespace frames + } // namespace dfsi +} // namespace p25 + +#endif // __MOT_TSBK_FRAME_H__ \ No newline at end of file diff --git a/src/fne/network/FNENetwork.cpp b/src/fne/network/FNENetwork.cpp index 5cc5ab40..d377e606 100644 --- a/src/fne/network/FNENetwork.cpp +++ b/src/fne/network/FNENetwork.cpp @@ -256,7 +256,7 @@ void FNENetwork::clock(uint32_t ms) return; } - uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); if (m_forceListUpdate) { for (auto peer : m_peers) { @@ -274,7 +274,7 @@ void FNENetwork::clock(uint32_t ms) FNEPeerConnection* connection = peer.second; if (connection != nullptr) { if (connection->connected()) { - uint64_t dt = connection->lastPing() + (m_host->m_pingTime * m_host->m_maxMissedPings); + uint64_t dt = connection->lastPing() + ((m_host->m_pingTime * 1000) * m_host->m_maxMissedPings); if (dt < now) { LogInfoEx(LOG_NET, "PEER %u (%s) timed out, dt = %u, now = %u", id, connection->identity().c_str(), dt, now); @@ -389,7 +389,7 @@ void* FNENetwork::threadedNetworkRx(void* arg) if (req != nullptr) { ::pthread_detach(req->thread); - uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); FNENetwork* network = static_cast(req->obj); if (network == nullptr) { @@ -414,8 +414,12 @@ void* FNENetwork::threadedNetworkRx(void* arg) if (connection != nullptr) { if (pktSeq == RTP_END_OF_CALL_SEQ) { - connection->pktLastSeq(pktSeq); - connection->pktNextSeq(0U); + // only reset packet sequences if we're a PROTOCOL or RPTC function + if ((req->fneHeader.getFunction() == NET_FUNC::PROTOCOL) || + (req->fneHeader.getFunction() == NET_FUNC::RPTC)) { + connection->pktLastSeq(pktSeq); + connection->pktNextSeq(0U); + } } else { if ((connection->currStreamId() == streamId) && (pktSeq != connection->pktNextSeq()) && (pktSeq != (RTP_END_OF_CALL_SEQ - 1U))) { LogWarning(LOG_NET, "PEER %u (%s) stream %u out-of-sequence; %u != %u", peerId, connection->identity().c_str(), @@ -847,19 +851,33 @@ void* FNENetwork::threadedNetworkRx(void* arg) connection->pingsReceived(pingsRx); connection->lastPing(now); - connection->pktLastSeq(connection->pktLastSeq() + 1); // does this peer need an ACL update? - uint64_t dt = connection->lastACLUpdate() + network->m_updateLookupTime; + uint64_t dt = connection->lastACLUpdate() + (network->m_updateLookupTime * 1000); if (dt < now) { LogInfoEx(LOG_NET, "PEER %u (%s) updating ACL list, dt = %u, now = %u", peerId, connection->identity().c_str(), dt, now); - network->peerACLUpdate(peerId); - connection->lastACLUpdate(now); + if (connection->pktLastSeq() == RTP_END_OF_CALL_SEQ) { + network->peerACLUpdate(peerId); + connection->lastACLUpdate(now); + } } + uint8_t payload[8U]; + ::memset(payload, 0x00U, 8U); + + // split ulong64_t (8 byte) value into bytes + payload[0U] = (uint8_t)((now >> 56) & 0xFFU); + payload[1U] = (uint8_t)((now >> 48) & 0xFFU); + payload[2U] = (uint8_t)((now >> 40) & 0xFFU); + payload[3U] = (uint8_t)((now >> 32) & 0xFFU); + payload[4U] = (uint8_t)((now >> 24) & 0xFFU); + payload[5U] = (uint8_t)((now >> 16) & 0xFFU); + payload[6U] = (uint8_t)((now >> 8) & 0xFFU); + payload[7U] = (uint8_t)((now >> 0) & 0xFFU); + network->m_peers[peerId] = connection; - network->writePeerCommand(peerId, { NET_FUNC::PONG, NET_SUBFUNC::NOP }); + network->writePeerCommand(peerId, { NET_FUNC::PONG, NET_SUBFUNC::NOP }, payload, 8U); if (network->m_reportPeerPing) { LogInfoEx(LOG_NET, "PEER %u (%s) ping, pingsReceived = %u, lastPing = %u", peerId, connection->identity().c_str(), @@ -1351,10 +1369,15 @@ void* FNENetwork::threadedACLUpdate(void* arg) std::string peerIdentity = network->resolvePeerIdentity(req->peerId); LogInfoEx(LOG_NET, "PEER %u (%s) sending ACL list updates", req->peerId, peerIdentity.c_str()); - network->writeWhitelistRIDs(req->peerId); - network->writeBlacklistRIDs(req->peerId); - network->writeTGIDs(req->peerId); - network->writeDeactiveTGIDs(req->peerId); + FNEPeerConnection* connection = network->m_peers[req->peerId]; + if (connection != nullptr) { + network->writeWhitelistRIDs(req->peerId); + network->writeBlacklistRIDs(req->peerId); + network->writeTGIDs(req->peerId); + + connection->pktLastSeq(RTP_END_OF_CALL_SEQ - 1U); + network->writeDeactiveTGIDs(req->peerId); + } delete req; } @@ -1366,7 +1389,7 @@ void* FNENetwork::threadedACLUpdate(void* arg) void FNENetwork::writeWhitelistRIDs(uint32_t peerId) { - uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); // send radio ID white/black lists std::vector ridWhitelist; @@ -1439,7 +1462,7 @@ void FNENetwork::writeWhitelistRIDs(uint32_t peerId) void FNENetwork::writeBlacklistRIDs(uint32_t peerId) { - uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); // send radio ID blacklist std::vector ridBlacklist; diff --git a/src/fne/network/FNENetwork.h b/src/fne/network/FNENetwork.h index 10706255..07be1db8 100644 --- a/src/fne/network/FNENetwork.h +++ b/src/fne/network/FNENetwork.h @@ -116,7 +116,7 @@ namespace network m_lastACLUpdate(0U), m_isExternalPeer(false), m_config(), - m_pktLastSeq(0U), + m_pktLastSeq(RTP_END_OF_CALL_SEQ), m_pktNextSeq(1U) { /* stub */ @@ -143,7 +143,7 @@ namespace network m_lastACLUpdate(0U), m_isExternalPeer(false), m_config(), - m_pktLastSeq(0U), + m_pktLastSeq(RTP_END_OF_CALL_SEQ), m_pktNextSeq(1U) { assert(id > 0U); diff --git a/src/host/CMakeLists.txt b/src/host/CMakeLists.txt index 156799ba..b98c582e 100644 --- a/src/host/CMakeLists.txt +++ b/src/host/CMakeLists.txt @@ -46,6 +46,8 @@ file(GLOB dvmhost_SRC "src/host/modem/*.cpp" "src/host/modem/port/*.h" "src/host/modem/port/*.cpp" + "src/host/modem/port/specialized/*.h" + "src/host/modem/port/specialized/*.cpp" "src/host/network/*.h" "src/host/network/*.cpp" "src/remote/RESTClient.cpp" diff --git a/src/host/Host.Config.cpp b/src/host/Host.Config.cpp index b9061b66..74e6f913 100644 --- a/src/host/Host.Config.cpp +++ b/src/host/Host.Config.cpp @@ -16,6 +16,7 @@ #include "modem/port/UARTPort.h" #include "modem/port/PseudoPTYPort.h" #include "modem/port/UDPPort.h" +#include "modem/port/specialized/V24UDPPort.h" #include "Host.h" #include "HostMain.h" @@ -395,6 +396,7 @@ bool Host::createModem() yaml::Node modemProtocol = modemConf["protocol"]; std::string portType = modemProtocol["type"].as("null"); + std::string modemMode = modemProtocol["mode"].as("air"); yaml::Node uartProtocol = modemProtocol["uart"]; std::string uartPort = uartProtocol["port"].as(); uint32_t uartSpeed = uartProtocol["speed"].as(115200); @@ -449,6 +451,14 @@ bool Host::createModem() uint16_t p25FifoLength = (uint16_t)modemConf["p25FifoLength"].as(P25_TX_BUFFER_LEN); uint16_t nxdnFifoLength = (uint16_t)modemConf["nxdnFifoLength"].as(NXDN_TX_BUFFER_LEN); + yaml::Node dfsiParams = modemConf["dfsi"]; + + bool rtrt = dfsiParams["rtrt"].as(true); + bool diu = dfsiParams["diu"].as(true); + uint16_t jitter = dfsiParams["jitter"].as(200U); + bool useFSCForUDP = dfsiParams["useFSC"].as(false); + uint16_t dfsiCallTimeout = dfsiParams["callTimeout"].as(200U); + // clamp fifo sizes if (dmrFifoLength < DMR_TX_BUFFER_LEN) { LogWarning(LOG_HOST, "DMR FIFO size must be greater then %u bytes, defaulting to %u bytes!", DMR_TX_BUFFER_LEN, DMR_TX_BUFFER_LEN); @@ -492,6 +502,7 @@ bool Host::createModem() LogInfo("Modem Parameters"); LogInfo(" Port Type: %s", portType.c_str()); + LogInfo(" Interface Mode: %s", modemMode.c_str()); port::IModemPort* modemPort = nullptr; std::transform(portType.begin(), portType.end(), portType.begin(), ::tolower); @@ -551,16 +562,33 @@ bool Host::createModem() return false; } + std::transform(modemMode.begin(), modemMode.end(), modemMode.begin(), ::tolower); + if (modemMode == MODEM_MODE_DFSI) { + m_isModemDFSI = true; + LogInfo(" DFSI RT/RT: %s", rtrt ? "yes" : "no"); + LogInfo(" DFSI DIU Flag: %s", diu ? "yes" : "no"); + LogInfo(" DFSI Jitter Size: %u ms", jitter); + if (g_remoteModemMode) { + LogInfo(" DFSI Use FSC: %s", useFSCForUDP ? "yes" : "no"); + } + } + if (g_remoteModemMode) { if (portType == UART_PORT || portType == PTY_PORT) { m_modemRemotePort = new port::UDPPort(g_remoteAddress, g_remotePort); m_modemRemote = true; ignoreModemConfigArea = true; - } else { delete modemPort; - modemPort = new port::UDPPort(g_remoteAddress, g_remotePort); + if (modemMode == MODEM_MODE_DFSI) { + yaml::Node networkConf = m_conf["network"]; + uint32_t id = networkConf["id"].as(1000U); + modemPort = new port::specialized::V24UDPPort(id, g_remoteAddress, g_remotePort, g_remotePort, useFSCForUDP, debug); + m_udpDSFIRemotePort = modemPort; + } else { + modemPort = new port::UDPPort(g_remoteAddress, g_remotePort); + } m_modemRemote = false; } @@ -614,8 +642,14 @@ bool Host::createModem() LogInfo(" Debug: yes"); } - m_modem = new Modem(modemPort, m_duplex, rxInvert, txInvert, pttInvert, dcBlocker, cosLockout, fdmaPreamble, dmrRxDelay, p25CorrCount, - m_dmrQueueSizeBytes, m_p25QueueSizeBytes, m_nxdnQueueSizeBytes, disableOFlowReset, ignoreModemConfigArea, dumpModemStatus, trace, debug); + if (m_isModemDFSI) { + m_modem = new ModemV24(modemPort, m_duplex, m_p25QueueSizeBytes, m_p25QueueSizeBytes, rtrt, diu, jitter, + dumpModemStatus, trace, debug); + ((ModemV24*)m_modem)->setCallTimeout(dfsiCallTimeout); + } else { + m_modem = new Modem(modemPort, m_duplex, rxInvert, txInvert, pttInvert, dcBlocker, cosLockout, fdmaPreamble, dmrRxDelay, p25CorrCount, + m_dmrQueueSizeBytes, m_p25QueueSizeBytes, m_nxdnQueueSizeBytes, disableOFlowReset, ignoreModemConfigArea, dumpModemStatus, trace, debug); + } if (!m_modemRemote) { m_modem->setModeParams(m_dmrEnabled, m_p25Enabled, m_nxdnEnabled); m_modem->setLevels(rxLevel, cwIdTXLevel, dmrTXLevel, p25TXLevel, nxdnTXLevel); diff --git a/src/host/Host.cpp b/src/host/Host.cpp index 0740153f..29af1582 100644 --- a/src/host/Host.cpp +++ b/src/host/Host.cpp @@ -16,6 +16,7 @@ #include "common/StopWatch.h" #include "common/Thread.h" #include "common/Utils.h" +#include "modem/port/specialized/V24UDPPort.h" #include "remote/RESTClient.h" #include "host/Host.h" #include "ActivityLog.h" @@ -59,6 +60,8 @@ Host::Host(const std::string& confFile) : m_conf(), m_modem(nullptr), m_modemRemote(false), + m_isModemDFSI(false), + m_udpDSFIRemotePort(nullptr), m_network(nullptr), m_modemRemotePort(nullptr), m_state(STATE_IDLE), @@ -585,6 +588,16 @@ int Host::run() g_killed = true; } + if (m_isModemDFSI && m_dmrEnabled) { + ::LogError(LOG_HOST, "Cannot use V.24/DFSI modem with DMR protocol!"); + g_killed = true; + } + + if (m_isModemDFSI && m_nxdnEnabled) { + ::LogError(LOG_HOST, "Cannot use V.24/DFSI modem with NXDN protocol!"); + g_killed = true; + } + // P25 CC checks if (m_dmrEnabled && m_p25CtrlChannel) { ::LogError(LOG_HOST, "Cannot have DMR enabled when using dedicated P25 control!"); @@ -681,6 +694,11 @@ int Host::run() setState(STATE_P25); } + if (m_isModemDFSI) { + m_fixedMode = true; + setState(STATE_P25); + } + if (m_nxdnCtrlChannel) { m_fixedMode = true; setState(STATE_NXDN); @@ -738,6 +756,8 @@ int Host::run() } stopWatch.start(); + } else { + return EXIT_SUCCESS; } bool hasTxShutdown = false; @@ -906,6 +926,13 @@ int Host::run() } } + if (m_udpDSFIRemotePort != nullptr) { + m_mainLoopStage = 11U; // intentional magic number + modem::port::specialized::V24UDPPort* udpPort = dynamic_cast(m_udpDSFIRemotePort); + + udpPort->clock(ms); + } + // ------------------------------------------------------ // -- Timer Clocking -- // ------------------------------------------------------ diff --git a/src/host/Host.h b/src/host/Host.h index 4a26e7a3..f1bc9100 100644 --- a/src/host/Host.h +++ b/src/host/Host.h @@ -40,6 +40,7 @@ #include "network/Network.h" #include "network/RESTAPI.h" #include "modem/Modem.h" +#include "modem/ModemV24.h" #include #include @@ -92,6 +93,8 @@ private: modem::Modem* m_modem; bool m_modemRemote; + bool m_isModemDFSI; + modem::port::IModemPort* m_udpDSFIRemotePort; network::Network* m_network; modem::port::IModemPort* m_modemRemotePort; diff --git a/src/host/modem/ModemV24.cpp b/src/host/modem/ModemV24.cpp new file mode 100644 index 00000000..2aacf868 --- /dev/null +++ b/src/host/modem/ModemV24.cpp @@ -0,0 +1,1526 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Modem Host Software + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * Copyright (C) 2024 Patrick McDonnell, W3AXL + * + */ +#include "Defines.h" +#include "common/p25/P25Defines.h" +#include "common/p25/data/LowSpeedData.h" +#include "common/p25/dfsi/LC.h" +#include "common/p25/dfsi/frames/Frames.h" +#include "common/p25/lc/LC.h" +#include "common/p25/lc/tsbk/TSBKFactory.h" +#include "common/p25/NID.h" +#include "common/p25/P25Utils.h" +#include "common/p25/Sync.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "modem/ModemV24.h" + +using namespace modem; +using namespace p25; +using namespace p25::defines; +using namespace p25::dfsi::defines; +using namespace p25::dfsi::frames; + +#include + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/* Initializes a new instance of the ModemV24 class. */ + +ModemV24::ModemV24(port::IModemPort* port, bool duplex, uint32_t p25QueueSize, uint32_t p25TxQueueSize, + bool rtrt, bool diu, uint16_t jitter, bool dumpModemStatus, bool trace, bool debug) : + Modem(port, duplex, false, false, false, false, false, 80U, 7U, 8U, 1U, p25QueueSize, 1U, + false, false, dumpModemStatus, trace, debug), + m_rtrt(rtrt), + m_diu(diu), + m_audio(), + m_nid(nullptr), + m_txP25Queue(p25TxQueueSize, "TX P25 Queue"), + m_txCall(), + m_rxCall(), + m_txCallInProgress(false), + m_rxCallInProgress(false), + m_txLastFrameTime(0U), + m_rxLastFrameTime(0U), + m_callTimeout(200U), + m_jitter(jitter), + m_lastP25Tx(0U), + m_rs() +{ + // Init m_call + m_txCall = new DFSICallData(); + m_rxCall = new DFSICallData(); +} + +/* Finalizes a instance of the Modem class. */ + +ModemV24::~ModemV24() +{ + delete m_nid; + delete m_txCall; + delete m_rxCall; +} + +/* Sets the call timeout. */ + +void ModemV24::setCallTimeout(uint16_t timeout) +{ + m_callTimeout = timeout; +} + +/* Sets the P25 NAC. */ + +void ModemV24::setP25NAC(uint32_t nac) +{ + Modem::setP25NAC(nac); + m_nid = new NID(nac); +} + +/* Opens connection to the air interface modem. */ + +bool ModemV24::open() +{ + LogMessage(LOG_MODEM, "Initializing modem"); + m_gotModemStatus = false; + + bool ret = m_port->open(); + if (!ret) + return false; + + ret = getFirmwareVersion(); + if (!ret) { + m_port->close(); + return false; + } else { + // Stopping the inactivity timer here when a firmware version has been + // successfuly read prevents the death spiral of "no reply from modem..." + m_inactivityTimer.stop(); + } + + m_rspOffset = 0U; + m_rspState = RESP_START; + + // do we have an open port handler? + if (m_openPortHandler) { + ret = m_openPortHandler(this); + if (!ret) + return false; + + m_error = false; + return true; + } + + m_statusTimer.start(); + + m_error = false; + + LogMessage(LOG_MODEM, "Modem Ready [Direct Mode]"); + return true; +} + +/* Updates the timer by the passed number of milliseconds. */ + +void ModemV24::clock(uint32_t ms) +{ + // poll the modem status + m_statusTimer.clock(ms); + if (m_statusTimer.hasExpired()) { + getStatus(); + m_statusTimer.start(); + } + + m_inactivityTimer.clock(ms); + if (m_inactivityTimer.hasExpired()) { + LogError(LOG_MODEM, "No reply from the modem for some time, resetting it"); + reset(); + } + + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + bool forceModemReset = false; + RESP_TYPE_DVM type = getResponse(); + + // do we have a custom response handler? + if (m_rspHandler != nullptr) { + // execute custom response handler + if (m_rspHandler(this, ms, type, m_rspDoubleLength, m_buffer, m_length)) { + // all logic handled by handler -- return + return; + } + } + + if (type == RTM_TIMEOUT) { + // Nothing to do + } + else if (type == RTM_ERROR) { + // Nothing to do + } + else { + // type == RTM_OK + uint8_t cmdOffset = 2U; + if (m_rspDoubleLength) { + cmdOffset = 3U; + } + + switch (m_buffer[cmdOffset]) { + /** Project 25 */ + case CMD_P25_DATA: + { + if (m_p25Enabled) { + std::lock_guard lock(m_p25ReadLock); + + // convert data from V.24/DFSI formatting to TIA-102 air formatting + convertToAir(m_buffer + (cmdOffset + 1U), m_length - (cmdOffset + 1U)); + } + } + break; + + case CMD_P25_LOST: + { + if (m_p25Enabled) { + std::lock_guard lock(m_p25ReadLock); + + if (m_rspDoubleLength) { + LogError(LOG_MODEM, "CMD_P25_LOST double length?; len = %u", m_length); + break; + } + + uint8_t data = 1U; + m_rxP25Queue.addData(&data, 1U); + + data = TAG_LOST; + m_rxP25Queue.addData(&data, 1U); + } + } + break; + + /** General */ + case CMD_GET_STATUS: + { + m_isHotspot = (m_buffer[3U] & 0x01U) == 0x01U; + + // override hotspot flag if we're forcing hotspot + if (m_forceHotspot) { + m_isHotspot = m_forceHotspot; + } + + bool dmrEnable = (m_buffer[3U] & 0x02U) == 0x02U; + bool p25Enable = (m_buffer[3U] & 0x08U) == 0x08U; + bool nxdnEnable = (m_buffer[3U] & 0x10U) == 0x10U; + + m_modemState = (DVM_STATE)m_buffer[4U]; + + m_tx = (m_buffer[5U] & 0x01U) == 0x01U; + + bool adcOverflow = (m_buffer[5U] & 0x02U) == 0x02U; + if (adcOverflow) { + //LogError(LOG_MODEM, "ADC levels have overflowed"); + m_adcOverFlowCount++; + + if (m_adcOverFlowCount >= MAX_ADC_OVERFLOW / 2U) { + LogWarning(LOG_MODEM, "ADC overflow count > %u!", MAX_ADC_OVERFLOW / 2U); + } + + if (!m_disableOFlowReset) { + if (m_adcOverFlowCount > MAX_ADC_OVERFLOW) { + LogError(LOG_MODEM, "ADC overflow count > %u, resetting modem", MAX_ADC_OVERFLOW); + forceModemReset = true; + } + } + else { + m_adcOverFlowCount = 0U; + } + } + else { + if (m_adcOverFlowCount != 0U) { + m_adcOverFlowCount--; + } + } + + bool rxOverflow = (m_buffer[5U] & 0x04U) == 0x04U; + if (rxOverflow) + LogError(LOG_MODEM, "RX buffer has overflowed"); + + bool txOverflow = (m_buffer[5U] & 0x08U) == 0x08U; + if (txOverflow) + LogError(LOG_MODEM, "TX buffer has overflowed"); + + m_lockout = (m_buffer[5U] & 0x10U) == 0x10U; + + bool dacOverflow = (m_buffer[5U] & 0x20U) == 0x20U; + if (dacOverflow) { + //LogError(LOG_MODEM, "DAC levels have overflowed"); + m_dacOverFlowCount++; + + if (m_dacOverFlowCount > MAX_DAC_OVERFLOW / 2U) { + LogWarning(LOG_MODEM, "DAC overflow count > %u!", MAX_DAC_OVERFLOW / 2U); + } + + if (!m_disableOFlowReset) { + if (m_dacOverFlowCount > MAX_DAC_OVERFLOW) { + LogError(LOG_MODEM, "DAC overflow count > %u, resetting modem", MAX_DAC_OVERFLOW); + forceModemReset = true; + } + } + else { + m_dacOverFlowCount = 0U; + } + } + else { + if (m_dacOverFlowCount != 0U) { + m_dacOverFlowCount--; + } + } + + m_cd = (m_buffer[5U] & 0x40U) == 0x40U; + + // spaces from the modem are returned in "logical" frame count, not raw byte size + m_dmrSpace1 = 0U; + m_dmrSpace2 = 0U; + m_p25Space = m_buffer[10U] * (P25DEF::P25_LDU_FRAME_LENGTH_BYTES);//(P25DEF::P25_PDU_FRAME_LENGTH_BYTES); + m_nxdnSpace = 0U; + + if (m_dumpModemStatus) { + LogDebug(LOG_MODEM, "ModemV24::clock(), CMD_GET_STATUS, isHotspot = %u, dmr = %u / %u, p25 = %u / %u, nxdn = %u / %u, modemState = %u, tx = %u, adcOverflow = %u, rxOverflow = %u, txOverflow = %u, dacOverflow = %u, dmrSpace1 = %u, dmrSpace2 = %u, p25Space = %u, nxdnSpace = %u", + m_isHotspot, dmrEnable, m_dmrEnabled, p25Enable, m_p25Enabled, nxdnEnable, m_nxdnEnabled, m_modemState, m_tx, adcOverflow, rxOverflow, txOverflow, dacOverflow, m_dmrSpace1, m_dmrSpace2, m_p25Space, m_nxdnSpace); + LogDebug(LOG_MODEM, "ModemV24::clock(), CMD_GET_STATUS, rxDMRData1 size = %u, len = %u, free = %u; rxDMRData2 size = %u, len = %u, free = %u, rxP25Data size = %u, len = %u, free = %u, rxNXDNData size = %u, len = %u, free = %u", + m_rxDMRQueue1.length(), m_rxDMRQueue1.dataSize(), m_rxDMRQueue1.freeSpace(), m_rxDMRQueue2.length(), m_rxDMRQueue2.dataSize(), m_rxDMRQueue2.freeSpace(), + m_rxP25Queue.length(), m_rxP25Queue.dataSize(), m_rxP25Queue.freeSpace(), m_rxNXDNQueue.length(), m_rxNXDNQueue.dataSize(), m_rxNXDNQueue.freeSpace()); + } + + m_gotModemStatus = true; + m_inactivityTimer.start(); + } + break; + + case CMD_GET_VERSION: + case CMD_ACK: + break; + + case CMD_NAK: + { + LogWarning(LOG_MODEM, "NAK, command = 0x%02X (%s), reason = %u (%s)", m_buffer[3U], cmdToString(m_buffer[3U]).c_str(), m_buffer[4U], rsnToString(m_buffer[4U]).c_str()); + switch (m_buffer[4U]) { + case RSN_RINGBUFF_FULL: + { + switch (m_buffer[3U]) { + case CMD_DMR_DATA1: + LogWarning(LOG_MODEM, "NAK, %s, dmrSpace1 = %u", rsnToString(m_buffer[4U]).c_str(), m_dmrSpace1); + break; + case CMD_DMR_DATA2: + LogWarning(LOG_MODEM, "NAK, %s, dmrSpace2 = %u", rsnToString(m_buffer[4U]).c_str(), m_dmrSpace2); + break; + + case CMD_P25_DATA: + LogWarning(LOG_MODEM, "NAK, %s, p25Space = %u", rsnToString(m_buffer[4U]).c_str(), m_p25Space); + break; + + case CMD_NXDN_DATA: + LogWarning(LOG_MODEM, "NAK, %s, nxdnSpace = %u", rsnToString(m_buffer[4U]).c_str(), m_nxdnSpace); + break; + } + } + break; + } + } + break; + + case CMD_DEBUG1: + case CMD_DEBUG2: + case CMD_DEBUG3: + case CMD_DEBUG4: + case CMD_DEBUG5: + case CMD_DEBUG_DUMP: + printDebug(m_buffer, m_length); + break; + + default: + LogWarning(LOG_MODEM, "Unknown message, type = %02X", m_buffer[2U]); + Utils::dump("Buffer dump", m_buffer, m_length); + if (m_rspState != RESP_START) + m_rspState = RESP_START; + break; + } + } + + // force a modem reset because of a error condition + if (forceModemReset) { + forceModemReset = false; + reset(); + } + + // write anything waiting to the serial port + int len = writeSerial(); + if (m_debug && len > 0) { + LogDebug(LOG_MODEM, "Wrote %u-byte message to the serial V24 device", len); + } else if (len < 0) { + LogError(LOG_MODEM, "Failed to write to serial port!"); + } + + // clear an RX call in progress flag if we're longer than our timeout value + now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + if (m_rxCallInProgress && (now - m_rxLastFrameTime > m_callTimeout)) { + m_rxCallInProgress = false; + m_rxCall->resetCallData(); + LogWarning(LOG_MODEM, "No call data received from V24 for %u ms, resetting RX call", (now - m_rxLastFrameTime)); + } +} + +/* Closes connection to the air interface modem. */ + +void ModemV24::close() +{ + LogDebug(LOG_MODEM, "Closing the modem"); + m_port->close(); + + m_gotModemStatus = false; + + // do we have a close port handler? + if (m_closePortHandler != nullptr) { + m_closePortHandler(this); + } +} + +/* Writes raw data to the air interface modem. */ + +int ModemV24::write(const uint8_t* data, uint32_t length) +{ + assert(data != nullptr); + + uint8_t modemCommand = CMD_GET_VERSION; + if (data[0U] == DVM_SHORT_FRAME_START) { + modemCommand = data[2U]; + } else if (data[0U] == DVM_LONG_FRAME_START) { + modemCommand = data[3U]; + } + + if (modemCommand == CMD_P25_DATA) { + uint8_t buffer[length]; + ::memset(buffer, 0x00U, length); + ::memcpy(buffer, data + 2U, length); + + convertFromAir(buffer, length); + return length; + } else { + return Modem::write(data, length); + } +} + +// --------------------------------------------------------------------------- +// Private Class Members +// --------------------------------------------------------------------------- + +/* Helper to write data from the P25 Tx queue to the serial interface. */ + +int ModemV24::writeSerial() +{ + /* + * Serial TX ringbuffer format: + * + * | 0x01 | 0x02 | 0x03 | 0x04 | 0x05 | 0x06 | 0x07 | 0x08 | 0x09 | 0x0A | 0x0B | 0x0C | ... | + * | Length | Tag | int64_t timestamp in ms | data | + */ + + // check empty + if (m_txP25Queue.isEmpty()) + return 0U; + + // get length + uint8_t length[2U]; + ::memset(length, 0x00U, 2U); + m_txP25Queue.peek(length, 2U); + + // convert length byets to int + uint16_t len = 0U; + len = (length[0U] << 8) + length[1U]; + + // this ensures we never get in a situation where we have length & type bytes stuck in the queue by themselves + if (m_txP25Queue.dataSize() == 2U && len > m_txP25Queue.dataSize()) { + m_txP25Queue.get(length, 2U); // ensure we pop bytes off + return 0U; + } + + // get current timestamp + int64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + // peek the timestamp to see if we should wait + if (m_txP25Queue.dataSize() >= 11U) { + uint8_t lengthTagTs[11U]; + ::memset(lengthTagTs, 0x00U, 11U); + m_txP25Queue.peek(lengthTagTs, 11U); + + // get the timestamp + int64_t ts; + assert(sizeof ts == 8); + ::memcpy(&ts, lengthTagTs + 3U, 8U); + + // if it's not time to send, return + if (ts > now) { + return 0U; + } + } + + // check if we have enough data to get everything - len + 2U (length bytes) + 1U (tag) + 8U (timestamp) + if (m_txP25Queue.dataSize() >= len + 11U) { + // Get the length, tag and timestamp + uint8_t lengthTagTs[11U]; + m_txP25Queue.get(lengthTagTs, 11U); + + // Get the actual data + uint8_t buffer[len]; + m_txP25Queue.get(buffer, len); + + // Sanity check on data tag + uint8_t tag = lengthTagTs[2U]; + if (tag != TAG_DATA) { + LogError(LOG_MODEM, "Got unexpected data tag from TX P25 ringbuffer! %02X", tag); + return 0U; + } + + // we already checked the timestamp above, so we just get the data and write it + return m_port->write(buffer, len); + } + + return 0U; +} + +/* Helper to store converted Rx frames. */ + +void ModemV24::storeConvertedRx(const uint8_t* buffer, uint32_t length) +{ + // store converted frame into the Rx modem queue + uint8_t storedLen[2U]; + storedLen[0U] = 0x00U; + storedLen[1U] = length; + m_rxP25Queue.addData(storedLen, 2U); + + //Utils::dump("Storing converted RX data", buffer, length); + + m_rxP25Queue.addData(buffer, length); +} + +/* Helper to generate a P25 TDU packet. */ + +void ModemV24::create_TDU(uint8_t* buffer) +{ + assert(buffer != nullptr); + + uint8_t data[P25_TDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(data + 2U, 0x00U, P25_TDU_FRAME_LENGTH_BYTES); + + // Generate Sync + Sync::addP25Sync(data + 2U); + + // Generate NID + m_nid->encode(data + 2U, DUID::TDU); + + // Add busy bits + P25Utils::addStatusBits(data + 2U, P25_TDU_FRAME_LENGTH_BITS, false); + + buffer[0U] = modem::TAG_EOT; + buffer[1U] = 0x01U; + ::memcpy(buffer, data, P25_TDU_FRAME_LENGTH_BYTES + 2U); +} + +/* Internal helper to convert from V.24/DFSI to TIA-102 air interface. */ + +void ModemV24::convertToAir(const uint8_t *data, uint32_t length) +{ + assert(data != nullptr); + assert(length > 0U); + + uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES + 2U); + + // get the DFSI data (skip the 0x00 padded byte at the start) + uint8_t dfsiData[length - 1U]; + ::memset(dfsiData, 0x00U, length - 1U); + ::memcpy(dfsiData, data + 1U, length - 1U); + + if (m_debug) + Utils::dump("V24 RX data from board", dfsiData, length - 1U); + + DFSIFrameType::E frameType = (DFSIFrameType::E)dfsiData[0U]; + m_rxLastFrameTime = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + // Switch based on DFSI frame type + switch (frameType) { + case DFSIFrameType::MOT_START_STOP: + { + MotStartOfStream start = MotStartOfStream(dfsiData); + if (start.getStartStop() == StartStopFlag::START) { + m_rxCall->resetCallData(); + m_rxCallInProgress = true; + if (m_debug) { + LogDebug(LOG_MODEM, "V24 RX, ICW START, RT = $%02X, Type = $%02X", dfsiData[2U], dfsiData[4U]); + } + } else { + if (m_rxCallInProgress) { + m_rxCall->resetCallData(); + m_rxCallInProgress = false; + if (m_debug) { + LogDebug(LOG_MODEM, "V24 RX, ICW STOP, RT = $%02X, Type = $%02X", dfsiData[2U], dfsiData[4U]); + } + // generate a TDU + create_TDU(buffer); + storeConvertedRx(buffer, P25_TDU_FRAME_LENGTH_BYTES + 2U); + } + } + } + break; + + case DFSIFrameType::MOT_VHDR_1: + { + MotVoiceHeader1 vhdr1 = MotVoiceHeader1(dfsiData); + + // copy to call data VHDR1 + ::memset(m_rxCall->VHDR1, 0x00U, MotVoiceHeader1::HCW_LENGTH); + ::memcpy(m_rxCall->VHDR1, vhdr1.header, MotVoiceHeader1::HCW_LENGTH); + } + break; + case DFSIFrameType::MOT_VHDR_2: + { + MotVoiceHeader2 vhdr2 = MotVoiceHeader2(dfsiData); + + // copy to call data VHDR2 + ::memset(m_rxCall->VHDR2, 0x00U, MotVoiceHeader2::HCW_LENGTH); + ::memcpy(m_rxCall->VHDR2, vhdr2.header, MotVoiceHeader2::HCW_LENGTH); + + // buffer for raw VHDR data + uint8_t raw[DFSI_VHDR_RAW_LEN]; + ::memset(raw, 0x00U, DFSI_VHDR_RAW_LEN); + + ::memcpy(raw, m_rxCall->VHDR1, 8U); + ::memcpy(raw + 8U, m_rxCall->VHDR1 + 9U, 8U); + ::memcpy(raw + 16U, m_rxCall->VHDR1 + 18U, 2U); + + ::memcpy(raw + 18U, m_rxCall->VHDR2, 8U); + ::memcpy(raw + 26U, m_rxCall->VHDR2 + 9U, 8U); + ::memcpy(raw + 34U, m_rxCall->VHDR2 + 18U, 2U); + + // buffer for decoded VHDR data + uint8_t vhdr[DFSI_VHDR_LEN]; + + uint offset = 0U; + for (uint32_t i = 0; i < DFSI_VHDR_RAW_LEN; i++, offset += 6) + Utils::hex2Bin(raw[i], vhdr, offset); + + // try to decode the RS data + try { + bool ret = m_rs.decode362017(vhdr); + if (!ret) { + LogError(LOG_MODEM, "V.24/DFSI traffic failed to decode RS (36,20,17) FEC"); + } else { + // late entry? + if (!m_rxCallInProgress) { + m_rxCallInProgress = true; + m_rxCall->resetCallData(); + if (m_debug) + LogDebug(LOG_MODEM, "V24 RX VHDR late entry, resetting call data"); + } + + ::memcpy(m_rxCall->MI, vhdr, MI_LENGTH_BYTES); + + m_rxCall->mfId = vhdr[9U]; + m_rxCall->algoId = vhdr[10U]; + m_rxCall->kId = __GET_UINT16B(vhdr, 11U); + m_rxCall->dstId = __GET_UINT16B(vhdr, 13U); + + if (m_debug) { + LogDebug(LOG_MODEM, "P25, VHDR algId = $%02X, kId = $%04X, dstId = $%04X", m_rxCall->algoId, m_rxCall->kId, m_rxCall->dstId); + } + + // generate a HDU + lc::LC lc = lc::LC(); + lc.setDstId(m_rxCall->dstId); + lc.setAlgId(m_rxCall->algoId); + lc.setKId(m_rxCall->kId); + lc.setMI(m_rxCall->MI); + + // Generate Sync + Sync::addP25Sync(buffer + 2U); + + // Generate NID + m_nid->encode(buffer + 2U, DUID::HDU); + + // Generate HDU + lc.encodeHDU(buffer + 2U); + + // Add busy bits + P25Utils::addStatusBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, true); + + buffer[0U] = modem::TAG_DATA; + buffer[1U] = 0x01U; + storeConvertedRx(buffer, P25_HDU_FRAME_LENGTH_BYTES + 2U); + } + } + catch (...) { + LogError(LOG_MODEM, "V.24/DFSI RX traffic got exception while trying to decode RS data for VHDR"); + } + } + break; + + // VOICE1/10 create a start voice frame + case DFSIFrameType::LDU1_VOICE1: + { + MotStartVoiceFrame svf = MotStartVoiceFrame(dfsiData); + ::memcpy(m_rxCall->netLDU1 + 10U, svf.fullRateVoice->imbeData, RAW_IMBE_LENGTH_BYTES); + m_rxCall->n++; + } + break; + case DFSIFrameType::LDU2_VOICE10: + { + MotStartVoiceFrame svf = MotStartVoiceFrame(dfsiData); + ::memcpy(m_rxCall->netLDU2 + 10U, svf.fullRateVoice->imbeData, RAW_IMBE_LENGTH_BYTES); + m_rxCall->n++; + } + break; + + case DFSIFrameType::TSBK: + { + MotTSBKFrame tf = MotTSBKFrame(dfsiData); + lc::tsbk::OSP_TSBK_RAW tsbk = lc::tsbk::OSP_TSBK_RAW(); + if (!tsbk.decode(tf.tsbkData, true)) { + LogError(LOG_MODEM, "V.24/DFSI traffic failed to decode TSBK FEC"); + } else { + uint8_t buffer[P25_TSDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = modem::TAG_DATA; + buffer[1U] = 0x00U; + + // Generate Sync + Sync::addP25Sync(buffer + 2U); + + // Generate NID + m_nid->encode(buffer + 2U, DUID::TSDU); + + // Regenerate TSDU Data + tsbk.setLastBlock(true); // always set last block -- this a Single Block TSDU + tsbk.encode(buffer + 2U); + + // Add busy bits + P25Utils::addStatusBits(buffer + 2U, P25_TSDU_FRAME_LENGTH_BYTES, false, true); + P25Utils::addTrunkSlotStatusBits(buffer + 2U, P25_TSDU_FRAME_LENGTH_BYTES); + + // Set first busy bits to 1,1 + P25Utils::setStatusBits(buffer + 2U, P25_SS0_START, true, true); + + storeConvertedRx(buffer, P25_TSDU_FRAME_LENGTH_BYTES + 2U); + } + } + break; + + // The remaining LDUs all create full rate voice frames so we do that here + default: + { + MotFullRateVoice voice = MotFullRateVoice(dfsiData); + switch (frameType) { + case DFSIFrameType::LDU1_VOICE2: + { + ::memcpy(m_rxCall->netLDU1 + 26U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU1_VOICE3: + { + ::memcpy(m_rxCall->netLDU1 + 55U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->lco = voice.additionalData[0U]; + m_rxCall->mfId = voice.additionalData[1U]; + m_rxCall->serviceOptions = voice.additionalData[2U]; + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC3 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU1_VOICE4: + { + ::memcpy(m_rxCall->netLDU1 + 80U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->dstId = __GET_UINT16(voice.additionalData, 0U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC4 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU1_VOICE5: + { + ::memcpy(m_rxCall->netLDU1 + 105U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->srcId = __GET_UINT16(voice.additionalData, 0U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC5 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU1_VOICE6: + { + ::memcpy(m_rxCall->netLDU1 + 130U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU1_VOICE7: + { + ::memcpy(m_rxCall->netLDU1 + 155U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU1_VOICE8: + { + ::memcpy(m_rxCall->netLDU1 + 180U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU1_VOICE9: + { + ::memcpy(m_rxCall->netLDU1 + 204U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->lsd1 = voice.additionalData[0U]; + m_rxCall->lsd2 = voice.additionalData[1U]; + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC9 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU2_VOICE11: + { + ::memcpy(m_rxCall->netLDU2 + 26U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU2_VOICE12: + { + ::memcpy(m_rxCall->netLDU2 + 55U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + ::memcpy(m_rxCall->MI, voice.additionalData, 3U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC12 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU2_VOICE13: + { + ::memcpy(m_rxCall->netLDU2 + 80U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + ::memcpy(m_rxCall->MI + 3U, voice.additionalData, 3U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC13 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU2_VOICE14: + { + ::memcpy(m_rxCall->netLDU2 + 105U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + ::memcpy(m_rxCall->MI + 6U, voice.additionalData, 3U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC14 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU2_VOICE15: + { + ::memcpy(m_rxCall->netLDU2 + 130U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->algoId = voice.additionalData[0U]; + m_rxCall->kId = __GET_UINT16B(voice.additionalData, 1U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC15 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU2_VOICE16: + { + ::memcpy(m_rxCall->netLDU2 + 155U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU2_VOICE17: + { + ::memcpy(m_rxCall->netLDU2 + 180U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU2_VOICE18: + { + ::memcpy(m_rxCall->netLDU2 + 204U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->lsd1 = voice.additionalData[0U]; + m_rxCall->lsd2 = voice.additionalData[1U]; + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC18 traffic missing metadata"); + } + } + break; + default: + break; + } + + // increment our voice frame counter + m_rxCall->n++; + } + break; + } + + // encode LDU1 if ready + if (m_rxCall->n == 9U) { + lc::LC lc = lc::LC(); + lc.setLCO(m_rxCall->lco); + lc.setMFId(m_rxCall->mfId); + + if (lc.isStandardMFId()) { + lc.setSrcId(m_rxCall->srcId); + lc.setDstId(m_rxCall->dstId); + } else { + uint8_t rsBuffer[P25_LDU_LC_FEC_LENGTH_BYTES]; + ::memset(rsBuffer, 0x00U, P25_LDU_LC_FEC_LENGTH_BYTES); + + rsBuffer[0U] = m_rxCall->lco; + rsBuffer[1U] = m_rxCall->mfId; + rsBuffer[2U] = m_rxCall->serviceOptions; + rsBuffer[3U] = (m_rxCall->dstId >> 16) & 0xFFU; + rsBuffer[4U] = (m_rxCall->dstId >> 8) & 0xFFU; + rsBuffer[5U] = (m_rxCall->dstId >> 0) & 0xFFU; + rsBuffer[6U] = (m_rxCall->srcId >> 16) & 0xFFU; + rsBuffer[7U] = (m_rxCall->srcId >> 8) & 0xFFU; + rsBuffer[8U] = (m_rxCall->srcId >> 0) & 0xFFU; + + // combine bytes into ulong64_t (8 byte) value + ulong64_t rsValue = 0U; + 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]; + + lc.setRS(rsValue); + } + + bool emergency = ((m_rxCall->serviceOptions & 0xFFU) & 0x80U) == 0x80U; // Emergency Flag + bool encryption = ((m_rxCall->serviceOptions & 0xFFU) & 0x40U) == 0x40U; // Encryption Flag + uint8_t priority = ((m_rxCall->serviceOptions & 0xFFU) & 0x07U); // Priority + lc.setEmergency(emergency); + lc.setEncrypted(encryption); + lc.setPriority(priority); + + data::LowSpeedData lsd = data::LowSpeedData(); + lsd.setLSD1(m_rxCall->lsd1); + lsd.setLSD2(m_rxCall->lsd2); + + // generate Sync + Sync::addP25Sync(buffer + 2U); + + // generate NID + m_nid->encode(buffer + 2U, DUID::LDU1); + + // generate LDU1 Data + lc.encodeLDU1(buffer + 2U); + + // generate Low Speed Data + lsd.process(buffer + 2U); + + // generate audio + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 10U, 0U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 26U, 1U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 55U, 2U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 80U, 3U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 105U, 4U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 130U, 5U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 155U, 6U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 180U, 7U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 204U, 8U); + + // add busy bits + P25Utils::addStatusBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, true); + + buffer[0U] = modem::TAG_DATA; + buffer[1U] = 0x01U; + storeConvertedRx(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + } + + // encode LDU2 if ready + if (m_rxCall->n == 18U) { + lc::LC lc = lc::LC(); + lc.setMI(m_rxCall->MI); + lc.setAlgId(m_rxCall->algoId); + lc.setKId(m_rxCall->kId); + + data::LowSpeedData lsd = data::LowSpeedData(); + lsd.setLSD1(m_rxCall->lsd1); + lsd.setLSD2(m_rxCall->lsd2); + + // generate Sync + Sync::addP25Sync(buffer + 2U); + + // generate NID + m_nid->encode(buffer + 2U, DUID::LDU2); + + // generate LDU2 data + lc.encodeLDU2(buffer + 2U); + + // generate Low Speed Data + lsd.process(buffer + 2U); + + // generate audio + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 10U, 0U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 26U, 1U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 55U, 2U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 80U, 3U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 105U, 4U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 130U, 5U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 155U, 6U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 180U, 7U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 204U, 8U); + + // add busy bits + P25Utils::addStatusBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, true); + + buffer[0U] = modem::TAG_DATA; + buffer[1U] = 0x01U; + storeConvertedRx(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + m_rxCall->n = 0; + } +} + +/* Helper to add a V.24 data frame to the P25 TX queue with the proper timestamp and formatting */ + +void ModemV24::queueP25Frame(uint8_t* data, uint16_t len, SERIAL_TX_TYPE msgType) +{ + assert(data != nullptr); + assert(len > 0U); + + if (m_debug) + LogDebug(LOG_MODEM, "ModemV24::queueP25Frame() msgType = $%02X", msgType); + if (m_trace) + Utils::dump(1U, "ModemV24::queueP25Frame() data", data, len); + + // get current time in ms + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + // timestamp for this message (in ms) + uint64_t msgTime = 0U; + + // if this is our first message, timestamp is just now + the jitter buffer offset in ms + if (m_lastP25Tx == 0U) { + msgTime = now + m_jitter; + + // if the message type requests no jitter delay -- just set the message time to now + if (msgType == STT_NON_IMBE_NO_JITTER) + msgTime = now; + } + // if we had a message before this, calculate the new timestamp dynamically + else { + // if the last message occurred longer than our jitter buffer delay, we restart the sequence and calculate the same as above + if ((int64_t)(now - m_lastP25Tx) > m_jitter) { + msgTime = now + m_jitter; + } + // otherwise, we time out messages as required by the message type + else { + if (msgType == STT_IMBE) { + // IMBEs must go out at 20ms intervals + msgTime = m_lastP25Tx + 20U; + } else { + // Otherwise we don't care, we use 5ms since that's the theoretical minimum time a 9600 baud message can take + msgTime = m_lastP25Tx + 5U; + } + } + } + + len += 4U; + + // convert 16-bit length to 2 bytes + uint8_t length[2U]; + if (len > 255U) + length[0U] = (len >> 8U) & 0xFFU; + else + length[0U] = 0x00U; + length[1U] = len & 0xFFU; + + m_txP25Queue.addData(length, 2U); + + // add the data tag + uint8_t tag = TAG_DATA; + m_txP25Queue.addData(&tag, 1U); + + // convert 64-bit timestamp to 8 bytes and add + uint8_t tsBytes[8U]; + assert(sizeof msgTime == 8U); + ::memcpy(tsBytes, &msgTime, 8U); + m_txP25Queue.addData(tsBytes, 8U); + + // add the DVM start byte, length byte, CMD byte, and padding 0 + uint8_t header[4U]; + header[0U] = DVM_SHORT_FRAME_START; + header[1U] = len & 0xFFU; + header[2U] = CMD_P25_DATA; + header[3U] = 0x00U; + m_txP25Queue.addData(header, 4U); + + // add the data + m_txP25Queue.addData(data, len - 4U); + + // update the last message time + m_lastP25Tx = msgTime; +} + +/* Send a start of stream sequence (HDU, etc) to the connected serial V.24 device */ + +void ModemV24::startOfStream(const p25::lc::LC& control) +{ + m_txCallInProgress = true; + + MotStartOfStream start = MotStartOfStream(); + start.setStartStop(StartStopFlag::START); + start.setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + + // create buffer for bytes and encode + uint8_t startBuf[start.LENGTH]; + ::memset(startBuf, 0x00U, start.LENGTH); + start.encode(startBuf); + + if (m_trace) + Utils::dump(1U, "ModemV24::startOfStream() MotStartOfStream", startBuf, MotStartOfStream::LENGTH); + + queueP25Frame(startBuf, MotStartOfStream::LENGTH, STT_NON_IMBE); + + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + control.getMI(mi); + + uint8_t vhdr[DFSI_VHDR_LEN]; + ::memset(vhdr, 0x00U, DFSI_VHDR_LEN); + + ::memcpy(vhdr, mi, MI_LENGTH_BYTES); + + vhdr[9U] = control.getMFId(); + vhdr[10U] = control.getAlgId(); + __SET_UINT16B(control.getKId(), vhdr, 11U); + __SET_UINT16B(control.getDstId(), vhdr, 13U); + + // perform RS encoding + m_rs.encode362017(vhdr); + + // convert the binary bytes to hex bytes + uint8_t raw[DFSI_VHDR_RAW_LEN]; + uint32_t offset = 0; + for (uint8_t i = 0; i < DFSI_VHDR_RAW_LEN; i++, offset += 6) { + raw[i] = Utils::bin2Hex(vhdr, offset); + } + + // prepare VHDR1 + MotVoiceHeader1 vhdr1 = MotVoiceHeader1(); + vhdr1.startOfStream->setStartStop(StartStopFlag::START); + vhdr1.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + vhdr1.setICW(m_diu ? ICWFlag::DIU : ICWFlag::QUANTAR); + + ::memcpy(vhdr1.header, raw, 8U); + ::memcpy(vhdr1.header + 9U, raw + 8U, 8U); + ::memcpy(vhdr1.header + 18U, raw + 16U, 2U); + + // encode VHDR1 and send + uint8_t vhdr1Buf[vhdr1.LENGTH]; + ::memset(vhdr1Buf, 0x00U, vhdr1.LENGTH); + vhdr1.encode(vhdr1Buf); + + if (m_trace) + Utils::dump(1U, "ModemV24::startOfStream() MotVoiceHeader1", vhdr1Buf, MotVoiceHeader1::LENGTH); + + queueP25Frame(vhdr1Buf, MotVoiceHeader1::LENGTH, STT_NON_IMBE); + + // prepare VHDR2 + MotVoiceHeader2 vhdr2 = MotVoiceHeader2(); + ::memcpy(vhdr2.header, raw + 18U, 8U); + ::memcpy(vhdr2.header + 9U, raw + 26U, 8U); + ::memcpy(vhdr2.header + 18U, raw + 34U, 2U); + + // encode VHDR2 and send + uint8_t vhdr2Buf[vhdr2.LENGTH]; + ::memset(vhdr2Buf, 0x00U, vhdr2.LENGTH); + vhdr2.encode(vhdr2Buf); + + if (m_trace) + Utils::dump(1U, "ModemV24::startOfStream() MotVoiceHeader2", vhdr2Buf, MotVoiceHeader2::LENGTH); + + queueP25Frame(vhdr2Buf, MotVoiceHeader2::LENGTH, STT_NON_IMBE); +} + +/* Send an end of stream sequence (TDU, etc) to the connected serial V.24 device */ + +void ModemV24::endOfStream() +{ + MotStartOfStream end = MotStartOfStream(); + end.setStartStop(StartStopFlag::STOP); + + // create buffer and encode + uint8_t endBuf[MotStartOfStream::LENGTH]; + ::memset(endBuf, 0x00U, MotStartOfStream::LENGTH); + end.encode(endBuf); + + if (m_trace) + Utils::dump(1U, "ModemV24::endOfStream() MotStartOfStream", endBuf, MotStartOfStream::LENGTH); + + queueP25Frame(endBuf, MotStartOfStream::LENGTH, STT_NON_IMBE); + + m_txCallInProgress = false; +} + +/* Internal helper to convert from TIA-102 air interface to V.24/DFSI. */ + +void ModemV24::convertFromAir(uint8_t* data, uint32_t length) +{ + assert(data != nullptr); + assert(length > 0U); + + if (m_trace) + Utils::dump(1U, "ModemV24::convertFromAir() data", data, length); + + uint8_t ldu[9U * 25U]; + ::memset(ldu, 0x00U, 9 * 25U); + + // decode the NID + bool valid = m_nid->decode(data + 2U); + if (!valid) + return; + + DUID::E duid = m_nid->getDUID(); + + // handle individual DUIDs + lc::LC lc = lc::LC(); + data::LowSpeedData lsd = data::LowSpeedData(); + switch (duid) { + case DUID::HDU: + { + bool ret = lc.decodeHDU(data + 2U); + if (!ret) { + LogWarning(LOG_MODEM, P25_HDU_STR ", undecodable LC"); + } + + startOfStream(lc); + } + break; + case DUID::LDU1: + { + bool ret = lc.decodeLDU1(data + 2U); + if (!ret) { + LogWarning(LOG_MODEM, P25_LDU1_STR ", undecodable LC"); + return; + } + + lsd.process(data + 2U); + + // late entry? + if (!m_txCallInProgress) { + startOfStream(lc); + if (m_debug) + LogDebug(LOG_MODEM, "V24 TX VHDR late entry, resetting TX call data"); + } + + // generate audio + m_audio.decode(data + 2U, ldu + 10U, 0U); + m_audio.decode(data + 2U, ldu + 26U, 1U); + m_audio.decode(data + 2U, ldu + 55U, 2U); + m_audio.decode(data + 2U, ldu + 80U, 3U); + m_audio.decode(data + 2U, ldu + 105U, 4U); + m_audio.decode(data + 2U, ldu + 130U, 5U); + m_audio.decode(data + 2U, ldu + 155U, 6U); + m_audio.decode(data + 2U, ldu + 180U, 7U); + m_audio.decode(data + 2U, ldu + 204U, 8U); + } + break; + case DUID::LDU2: + { + bool ret = lc.decodeLDU2(data + 2U); + if (!ret) { + LogWarning(LOG_MODEM, P25_LDU2_STR ", undecodable LC"); + return; + } + + lsd.process(data + 2U); + + // generate audio + m_audio.decode(data + 2U, ldu + 10U, 0U); + m_audio.decode(data + 2U, ldu + 26U, 1U); + m_audio.decode(data + 2U, ldu + 55U, 2U); + m_audio.decode(data + 2U, ldu + 80U, 3U); + m_audio.decode(data + 2U, ldu + 105U, 4U); + m_audio.decode(data + 2U, ldu + 130U, 5U); + m_audio.decode(data + 2U, ldu + 155U, 6U); + m_audio.decode(data + 2U, ldu + 180U, 7U); + m_audio.decode(data + 2U, ldu + 204U, 8U); + } + break; + + case DUID::TDU: + case DUID::TDULC: + if (m_txCallInProgress) + endOfStream(); + break; + + case DUID::PDU: + break; + + case DUID::TSDU: + { + lc::tsbk::OSP_TSBK_RAW tsbk = lc::tsbk::OSP_TSBK_RAW(); + if (!tsbk.decode(data + 2U)) { + LogWarning(LOG_MODEM, P25_TSDU_STR ", undecodable LC"); + return; + } + + MotStartOfStream startOfStream = MotStartOfStream(); + startOfStream.setStartStop(StartStopFlag::START); + startOfStream.setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + startOfStream.setStreamType(StreamTypeFlag::TSBK); + + // create buffer and encode + uint8_t startBuf[MotStartOfStream::LENGTH]; + ::memset(startBuf, 0x00U, MotStartOfStream::LENGTH); + startOfStream.encode(startBuf); + + queueP25Frame(startBuf, MotStartOfStream::LENGTH, STT_NON_IMBE_NO_JITTER); + + MotTSBKFrame tf = MotTSBKFrame(); + tf.startOfStream->setStartStop(StartStopFlag::START); + tf.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + tf.startOfStream->setStreamType(StreamTypeFlag::TSBK); + delete[] tf.tsbkData; + + tf.tsbkData = new uint8_t[P25_TSBK_LENGTH_BYTES]; + ::memset(tf.tsbkData, 0x00U, P25_TSBK_LENGTH_BYTES); + ::memcpy(tf.tsbkData, tsbk.getDecodedRaw(), P25_TSBK_LENGTH_BYTES); + + // create buffer and encode + uint8_t tsbkBuf[MotTSBKFrame::LENGTH]; + ::memset(tsbkBuf, 0x00U, MotTSBKFrame::LENGTH); + tf.encode(tsbkBuf); + + if (m_trace) + Utils::dump(1U, "ModemV24::convertFromAir() MotTSBKFrame", tsbkBuf, MotTSBKFrame::LENGTH); + + queueP25Frame(tsbkBuf, MotTSBKFrame::LENGTH, STT_NON_IMBE_NO_JITTER); + + MotStartOfStream endOfStream = MotStartOfStream(); + endOfStream.setStartStop(StartStopFlag::STOP); + endOfStream.setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + endOfStream.setStreamType(StreamTypeFlag::TSBK); + + // create buffer and encode + uint8_t endBuf[MotStartOfStream::LENGTH]; + ::memset(endBuf, 0x00U, MotStartOfStream::LENGTH); + endOfStream.encode(endBuf); + + queueP25Frame(endBuf, MotStartOfStream::LENGTH, STT_NON_IMBE_NO_JITTER); + queueP25Frame(endBuf, MotStartOfStream::LENGTH, STT_NON_IMBE_NO_JITTER); + } + break; + + default: + break; + } + + if (duid == DUID::LDU1 || duid == DUID::LDU2) { + uint8_t rs[P25_LDU_LC_FEC_LENGTH_BYTES]; + ::memset(rs, 0x00U, P25_LDU_LC_FEC_LENGTH_BYTES); + + if (duid == DUID::LDU1) { + if (lc.isStandardMFId()) { + uint8_t serviceOptions = + (lc.getEmergency() ? 0x80U : 0x00U) + + (lc.getEncrypted() ? 0x40U : 0x00U) + + (lc.getPriority() & 0x07U); + + rs[0U] = lc.getLCO(); // LCO + rs[1U] = lc.getMFId(); // MFId + rs[2U] = serviceOptions; // Service Options + uint32_t dstId = lc.getDstId(); + rs[3U] = (dstId >> 16) & 0xFFU; // Target Address + rs[4U] = (dstId >> 8) & 0xFFU; + rs[5U] = (dstId >> 0) & 0xFFU; + uint32_t srcId = lc.getSrcId(); + rs[6U] = (srcId >> 16) & 0xFFU; // Source Address + rs[7U] = (srcId >> 8) & 0xFFU; + rs[8U] = (srcId >> 0) & 0xFFU; + } else { + rs[0U] = lc.getLCO(); // LCO + + // split ulong64_t (8 byte) value into bytes + rs[1U] = (uint8_t)((lc.getRS() >> 56) & 0xFFU); + rs[2U] = (uint8_t)((lc.getRS() >> 48) & 0xFFU); + rs[3U] = (uint8_t)((lc.getRS() >> 40) & 0xFFU); + rs[4U] = (uint8_t)((lc.getRS() >> 32) & 0xFFU); + rs[5U] = (uint8_t)((lc.getRS() >> 24) & 0xFFU); + rs[6U] = (uint8_t)((lc.getRS() >> 16) & 0xFFU); + rs[7U] = (uint8_t)((lc.getRS() >> 8) & 0xFFU); + rs[8U] = (uint8_t)((lc.getRS() >> 0) & 0xFFU); + } + + // encode RS (24,12,13) FEC + m_rs.encode241213(rs); + } else { + // generate MI data + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + lc.getMI(mi); + + for (uint32_t i = 0; i < MI_LENGTH_BYTES; i++) + rs[i] = mi[i]; // Message Indicator + + rs[9U] = lc.getAlgId(); // Algorithm ID + rs[10U] = (lc.getKId() >> 8) & 0xFFU; // Key ID + rs[11U] = (lc.getKId() >> 0) & 0xFFU; // ... + + // encode RS (24,16,9) FEC + m_rs.encode24169(rs); + } + + for (int n = 0; n < 9; n++) { + uint8_t* buffer = nullptr; + uint16_t bufferSize = 0; + MotFullRateVoice voice = MotFullRateVoice(); + + switch (n) { + case 0: // VOICE1/10 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE1 : DFSIFrameType::LDU2_VOICE10); + + MotStartVoiceFrame svf = MotStartVoiceFrame(); + svf.startOfStream->setStartStop(StartStopFlag::START); + svf.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + svf.fullRateVoice->setFrameType(voice.getFrameType()); + svf.fullRateVoice->setSource(m_diu ? SourceFlag::DIU : SourceFlag::QUANTAR); + svf.setICW(m_diu ? ICWFlag::DIU : ICWFlag::QUANTAR); + + ::memcpy(svf.fullRateVoice->imbeData, ldu + 10U, RAW_IMBE_LENGTH_BYTES); + + buffer = new uint8_t[MotStartVoiceFrame::LENGTH]; + ::memset(buffer, 0x00U, MotStartVoiceFrame::LENGTH); + svf.encode(buffer); + bufferSize = MotStartVoiceFrame::LENGTH; + } + break; + case 1: // VOICE2/11 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE2 : DFSIFrameType::LDU2_VOICE11); + voice.setSource(m_diu ? SourceFlag::DIU : SourceFlag::QUANTAR); + ::memcpy(voice.imbeData, ldu + 26U, RAW_IMBE_LENGTH_BYTES); + } + break; + case 2: // VOICE3/12 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE3 : DFSIFrameType::LDU2_VOICE12); + ::memcpy(voice.imbeData, ldu + 55U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + // copy additional data + voice.additionalData[0U] = rs[0U]; + voice.additionalData[1U] = rs[1U]; + voice.additionalData[2U] = rs[2U]; + } + break; + case 3: // VOICE4/13 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE4 : DFSIFrameType::LDU2_VOICE13); + ::memcpy(voice.imbeData, ldu + 80U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + // copy additional data + voice.additionalData[0U] = rs[3U]; + voice.additionalData[1U] = rs[4U]; + voice.additionalData[2U] = rs[5U]; + } + break; + case 4: // VOICE5/14 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE5 : DFSIFrameType::LDU2_VOICE14); + ::memcpy(voice.imbeData, ldu + 105U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = rs[6U]; + voice.additionalData[1U] = rs[7U]; + voice.additionalData[2U] = rs[8U]; + } + break; + case 5: // VOICE6/15 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE6 : DFSIFrameType::LDU2_VOICE15); + ::memcpy(voice.imbeData, ldu + 130U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = rs[9U]; + voice.additionalData[1U] = rs[10U]; + voice.additionalData[2U] = rs[11U]; + } + break; + case 6: // VOICE7/16 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE7 : DFSIFrameType::LDU2_VOICE16); + ::memcpy(voice.imbeData, ldu + 155U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = rs[12U]; + voice.additionalData[1U] = rs[13U]; + voice.additionalData[2U] = rs[14U]; + } + break; + case 7: // VOICE8/17 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE8 : DFSIFrameType::LDU2_VOICE17); + ::memcpy(voice.imbeData, ldu + 180U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = rs[15U]; + voice.additionalData[1U] = rs[16U]; + voice.additionalData[2U] = rs[17U]; + } + break; + case 8: // VOICE9/18 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE9 : DFSIFrameType::LDU2_VOICE18); + ::memcpy(voice.imbeData, ldu + 204U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = lsd.getLSD1(); + voice.additionalData[1U] = lsd.getLSD2(); + } + break; + } + + // For n=0 (VHDR1/10) case we create the buffer in the switch, for all other frame types we do that here + if (n != 0) { + buffer = new uint8_t[voice.size()]; + ::memset(buffer, 0x00U, voice.size()); + voice.encode(buffer); + bufferSize = voice.size(); + } + + if (buffer != nullptr) { + if (m_trace) { + Utils::dump("ModemV24::convertFromAir() Encoded V.24 Voice Frame Data", buffer, bufferSize); + } + + queueP25Frame(buffer, bufferSize, STT_IMBE); + delete[] buffer; + } + } + } +} diff --git a/src/host/modem/ModemV24.h b/src/host/modem/ModemV24.h new file mode 100644 index 00000000..4477f6f0 --- /dev/null +++ b/src/host/modem/ModemV24.h @@ -0,0 +1,365 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Modem Host Software + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * + */ +/** + * @file ModemV24.h + * @ingroup modem + * @file ModemV24.cpp + * @ingroup modem + */ +#if !defined(__MODEM_V24_H__) +#define __MODEM_V24_H__ + +#include "Defines.h" +#include "common/edac/RS634717.h" +#include "common/p25/dfsi/frames/MotVoiceHeader1.h" +#include "common/p25/dfsi/frames/MotVoiceHeader2.h" +#include "common/p25/lc/LC.h" +#include "common/p25/Audio.h" +#include "common/p25/NID.h" +#include "modem/Modem.h" + +namespace modem +{ + // --------------------------------------------------------------------------- + // Constants + // --------------------------------------------------------------------------- + + /** + * @addtogroup modem + * @{ + */ + + /** + * @brief DFSI serial tx flags used to determine proper jitter handling of data in ringbuffer. + * @ingroup modem + */ + enum SERIAL_TX_TYPE { + STT_NO_DATA, //! No Data + STT_NON_IMBE, //! Non-IMBE Data/Signalling Frame + STT_NON_IMBE_NO_JITTER, //! Non-IMBE Data/Signalling Frame with Jitter Disabled + STT_IMBE //! IMBE Voice Frame + }; + + /** @} */ + + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Represents DFSI call data. + * @ingroup modem + */ + class HOST_SW_API DFSICallData { + public: + auto operator=(DFSICallData&) -> DFSICallData& = delete; + auto operator=(DFSICallData&&) -> DFSICallData& = delete; + DFSICallData(DFSICallData&) = delete; + + /** + * @brief Initializes a new instance of the DFSICallData class. + */ + DFSICallData() : + srcId(0U), + dstId(0U), + lco(0U), + mfId(P25DEF::MFG_STANDARD), + serviceOptions(0U), + lsd1(0U), + lsd2(0U), + MI(nullptr), + algoId(P25DEF::ALGO_UNENCRYPT), + kId(0U), + VHDR1(nullptr), + VHDR2(nullptr), + netLDU1(nullptr), + netLDU2(nullptr) + { + MI = new uint8_t[P25DEF::MI_LENGTH_BYTES]; + VHDR1 = new uint8_t[p25::dfsi::frames::MotVoiceHeader1::HCW_LENGTH]; + VHDR2 = new uint8_t[p25::dfsi::frames::MotVoiceHeader2::HCW_LENGTH]; + + netLDU1 = new uint8_t[9U * 25U]; + netLDU2 = new uint8_t[9U * 25U]; + + ::memset(netLDU1, 0x00U, 9U * 25U); + ::memset(netLDU2, 0x00U, 9U * 25U); + + resetCallData(); + } + /** + * @brief Finalizes a instance of the DFSICallData class. + */ + ~DFSICallData() + { + if (MI != nullptr) + delete[] MI; + if (VHDR1 != nullptr) + delete[] VHDR1; + if (VHDR2 != nullptr) + delete[] VHDR2; + if (netLDU1 != nullptr) + delete[] netLDU1; + if (netLDU2 != nullptr) + delete[] netLDU2; + } + + /** + * @brief Helper to reset the call data associated with this connection. + */ + void resetCallData() + { + srcId = 0U; + dstId = 0U; + + lco = 0U; + mfId = P25DEF::MFG_STANDARD; + serviceOptions = 0U; + + lsd1 = 0U; + lsd2 = 0U; + + if (MI != nullptr) + ::memset(MI, 0x00U, P25DEF::MI_LENGTH_BYTES); + algoId = P25DEF::ALGO_UNENCRYPT; + kId = 0U; + + if (VHDR1 != nullptr) + ::memset(VHDR1, 0x00U, p25::dfsi::frames::MotVoiceHeader1::HCW_LENGTH); + if (VHDR2 != nullptr) + ::memset(VHDR2, 0x00U, p25::dfsi::frames::MotVoiceHeader2::HCW_LENGTH); + + if (netLDU1 != nullptr) + ::memset(netLDU1, 0x00U, 9U * 25U); + if (netLDU2 != nullptr) + ::memset(netLDU2, 0x00U, 9U * 25U); + + n = 0U; + seqNo = 0U; + } + + public: + /** @name Call Data */ + /** + * @brief Source Radio ID. + */ + uint32_t srcId; + /** + * @brief Destination ID. + */ + uint32_t dstId; + + /** + * @brief Link Control Opcode. + */ + uint8_t lco; + /** + * @brief Manufacturer ID. + */ + uint8_t mfId; + /** + * @brief Call Service Options. + */ + uint8_t serviceOptions; + + /** + * @brief Low Speed Data 1. + */ + uint8_t lsd1; + /** + * @brief Low Speed Data 2. + */ + uint8_t lsd2; + + /** + * @brief Encryption Message Indicator. + */ + uint8_t* MI; + /** + * @brief Encryption Algorithm ID. + */ + uint8_t algoId; + /** + * @brief Encryption Key ID. + */ + uint32_t kId; + + /** + * @brief Voice Header 1. + */ + uint8_t* VHDR1; + /** + * @brief Voice Header 2. + */ + uint8_t* VHDR2; + + /** + * @brief Sequence Number. + */ + uint32_t seqNo; + /** + * @brief + */ + uint8_t n; + + /** + * @brief LDU1 Buffer. + */ + uint8_t* netLDU1; + /** + * @brief LDU2 Buffer. + */ + uint8_t* netLDU2; + /** @} */ + }; + + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Implements the core interface to the V.24 modem hardware. + * @ingroup modem + */ + class HOST_SW_API ModemV24 : public Modem { + public: + /** + * @brief Initializes a new instance of the ModemV24 class. + * @param port Port the air interface modem is connected to. + * @param duplex Flag indicating the modem is operating in duplex mode. + * @param p25QueueSize Modem P25 Rx frame buffer queue size (bytes). + * @param p25TxQueueSize Modem P25 Tx frame buffer queue size (bytes). + * @param rtrt Flag indicating whether or not RT/RT is enabled. + * @param diu Flag indicating whether or not V.24 communications are to a DIU. + * @param jitter + * @param dumpModemStatus Flag indicating whether the modem status is dumped to the log. + * @param trace Flag indicating whether air interface modem trace is enabled. + * @param debug Flag indicating whether air interface modem debug is enabled. + */ + ModemV24(port::IModemPort* port, bool duplex, uint32_t p25QueueSize, uint32_t p25TxQueueSize, + bool rtrt, bool diu, uint16_t jitter, bool dumpModemStatus, bool trace, bool debug); + /** + * @brief Finalizes a instance of the ModemV24 class. + */ + ~ModemV24(); + + /** + * @brief Sets the call timeout. + * @param timeout Timeout. + */ + void setCallTimeout(uint16_t timeout); + /** + * @brief Sets the P25 NAC. + * @param nac NAC. + */ + void setP25NAC(uint32_t nac) override; + + /** + * @brief Opens connection to the air interface modem. + * @returns bool True, if connection to modem is made, otherwise false. + */ + bool open() override; + + /** + * @brief Updates the modem by the passed number of milliseconds. + * @param ms Number of milliseconds. + */ + void clock(uint32_t ms) override; + + /** + * @brief Closes connection to the air interface modem. + */ + void close() override; + + /** + * @brief Writes raw data to the air interface modem. + * @param data Data to write to modem. + * @param length Length of data to write. + * @returns int Actual length of data written. + */ + int write(const uint8_t* data, uint32_t length) override; + + private: + bool m_rtrt; + bool m_diu; + + p25::Audio m_audio; + + p25::NID* m_nid; + + RingBuffer m_txP25Queue; + + DFSICallData* m_txCall; + DFSICallData* m_rxCall; + bool m_txCallInProgress; + bool m_rxCallInProgress; + uint64_t m_txLastFrameTime; + uint64_t m_rxLastFrameTime; + + uint16_t m_callTimeout; + + uint16_t m_jitter; + uint64_t m_lastP25Tx; + + edac::RS634717 m_rs; + + /** + * @brief Helper to write data from the P25 Tx queue to the serial interface. + * @return int Actual number of bytes written to the serial interface. + */ + int writeSerial(); + + /** + * @brief Helper to store converted Rx frames. + * @param buffer Buffer containing converted Rx frame. + * @param length Length of buffer. + */ + void storeConvertedRx(const uint8_t* buffer, uint32_t length); + /** + * @brief Helper to generate a P25 TDU packet. + * @param buffer Buffer to create TDU. + */ + void create_TDU(uint8_t* buffer); + + /** + * @brief Internal helper to convert from V.24/DFSI to TIA-102 air interface. + * @param data Buffer containing data to convert. + * @param length Length of buffer. + */ + void convertToAir(const uint8_t *data, uint32_t length); + + /** + * @brief Helper to add a V.24 data frame to the P25 Tx queue with the proper timestamp and formatting. + * @param data Buffer containing V.24 data frame to send. + * @param len Length of buffer. + * @param msgType Type of message to send (used for proper jitter clocking). + */ + void queueP25Frame(uint8_t* data, uint16_t length, SERIAL_TX_TYPE msgType); + + /** + * @brief Send a start of stream sequence (HDU, etc) to the connected serial V24 device. + * @param[in] control Instance of p25::lc::LC containing link control data. + */ + void startOfStream(const p25::lc::LC& control); + /** + * @brief Send an end of stream sequence (TDU, etc) to the connected serial V24 device. + */ + void endOfStream(); + + /** + * @brief Internal helper to convert from TIA-102 air interface to V.24/DFSI. + * @param data Buffer containing data to convert. + * @param length Length of buffer. + */ + void convertFromAir(uint8_t* data, uint32_t length); + }; +} // namespace modem + +#endif // __MODEM_V24_H__ diff --git a/src/host/modem/port/specialized/V24UDPPort.cpp b/src/host/modem/port/specialized/V24UDPPort.cpp new file mode 100644 index 00000000..ad269bda --- /dev/null +++ b/src/host/modem/port/specialized/V24UDPPort.cpp @@ -0,0 +1,639 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Modem Host Software + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * + */ +#include "Defines.h" +#include "common/network/RTPHeader.h" +#include "common/p25/dfsi/frames/Frames.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "modem/port/specialized/V24UDPPort.h" +#include "modem/Modem.h" + +using namespace p25::dfsi::defines; +using namespace p25::dfsi::frames; +using namespace p25::dfsi::frames::fsc; +using namespace modem::port; +using namespace modem::port::specialized; +using namespace network; +using namespace network::frame; +using namespace network::udp; + +#include +#include + +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +#define RTP_END_OF_CALL_SEQ 65535 + +const uint32_t BUFFER_LENGTH = 2000U; + +const char* V24_UDP_HARDWARE = "V.24 UDP Modem Controller"; +const uint8_t V24_UDP_PROTOCOL_VERSION = 4U; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/* Initializes a new instance of the V24UDPPort class. */ + +V24UDPPort::V24UDPPort(uint32_t peerId, const std::string& address, uint16_t modemPort, uint16_t controlPort, bool useFSC, bool debug) : + m_socket(nullptr), + m_localPort(modemPort), + m_controlSocket(nullptr), + m_ctrlFrameQueue(nullptr), + m_address(address), + m_addr(), + m_controlAddr(), + m_addrLen(0U), + m_ctrlAddrLen(0U), + m_buffer(2000U, "UDP Port Ring Buffer"), + m_reqConnectionTimer(1000U, 30U), + m_heartbeatTimer(1000U, 5U), + m_reqConnectionToPeer(true), + m_establishedConnection(false), + m_random(), + m_peerId(peerId), + m_streamId(0U), + m_timestamp(INVALID_TS), + m_pktSeq(0U), + m_modemState(STATE_P25), + m_tx(false), + m_debug(debug) +{ + assert(peerId > 0U); + assert(!address.empty()); + assert(modemPort > 0U); + assert(controlPort > 0U); + + if (controlPort > 0U && useFSC) { + m_controlSocket = new Socket(controlPort); + m_ctrlFrameQueue = new RawFrameQueue(m_controlSocket, m_debug); + + if (udp::Socket::lookup(address, controlPort, m_controlAddr, m_ctrlAddrLen) != 0) + m_ctrlAddrLen = 0U; + + if (m_ctrlAddrLen > 0U) { + std::string ctrlAddrStr = udp::Socket::address(m_controlAddr); + LogWarning(LOG_HOST, "SECURITY: Remote modem expects V.24 control channel IP address; %s for remote modem control", ctrlAddrStr.c_str()); + } + } else { + createVCPort(modemPort); + } + + std::random_device rd; + std::mt19937 mt(rd()); + m_random = mt; + + m_streamId = createStreamId(); +} + +/* Finalizes a instance of the V24UDPPort class. */ + +V24UDPPort::~V24UDPPort() +{ + if (m_ctrlFrameQueue != nullptr) + delete m_ctrlFrameQueue; + if (m_controlSocket != nullptr) + delete m_controlSocket; + + if (m_socket != nullptr) + delete m_socket; +} + +/* Updates the timer by the passed number of milliseconds. */ + +void V24UDPPort::clock(uint32_t ms) +{ + // if we have a FSC control socket + if (m_controlSocket != nullptr) { + if (m_reqConnectionToPeer) { + if (!m_reqConnectionTimer.isRunning()) { + // make initial request + writeConnect(); + m_reqConnectionTimer.start(); + } else { + m_reqConnectionTimer.clock(ms); + if (m_reqConnectionTimer.isRunning() && m_reqConnectionTimer.hasExpired()) { + // make another request + writeConnect(); + } + } + } + + if (m_establishedConnection) { + m_heartbeatTimer.clock(ms); + if (m_heartbeatTimer.isRunning() && m_heartbeatTimer.hasExpired()) { + writeHeartbeat(); + m_heartbeatTimer.start(); + } + } + + processCtrlNetwork(); + } + + // if we have a RTP voice socket + if (m_socket != nullptr) { + uint8_t data[BUFFER_LENGTH]; + ::memset(data, 0x00U, BUFFER_LENGTH); + + sockaddr_storage addr; + uint32_t addrLen; + int ret = m_socket->read(data, BUFFER_LENGTH, addr, addrLen); + if (ret != 0) { + // An error occurred on the socket + if (ret < 0) + return; + + // Add new data to the ring buffer + if (ret > 0) { + RTPHeader rtpHeader = RTPHeader(); + rtpHeader.decode(data); + + // ensure payload type is correct + if (rtpHeader.getPayloadType() != DFSI_RTP_PAYLOAD_TYPE) + { + LogError(LOG_MODEM, "Invalid RTP header received from network"); + return; + } + + // copy message + uint32_t messageLength = ret - RTP_HEADER_LENGTH_BYTES; + uint8_t message[messageLength]; + ::memset(message, 0x00U, messageLength); + + ::memcpy(message, data + RTP_HEADER_LENGTH_BYTES, messageLength); + + if (udp::Socket::match(addr, m_addr)) { + uint8_t reply[messageLength + 4U]; + + reply[0U] = DVM_SHORT_FRAME_START; + reply[1U] = messageLength & 0xFFU; + reply[2U] = CMD_P25_DATA; + + reply[3U] = 0x00U; + + ::memcpy(reply + 4U, message, messageLength); + + m_buffer.addData(reply, messageLength + 4U); + } + else { + std::string addrStr = udp::Socket::address(addr); + LogWarning(LOG_HOST, "SECURITY: Remote modem mode encountered invalid IP address; %s", addrStr.c_str()); + } + } + } + } +} + +/* Resets the RTP packet sequence and stream ID. */ + +void V24UDPPort::reset() +{ + m_pktSeq = 0U; + m_timestamp = INVALID_TS; + m_streamId = createStreamId(); +} + +/* Opens a connection to the port. */ + +bool V24UDPPort::open() +{ + if (m_addrLen == 0U && m_ctrlAddrLen == 0U) { + LogError(LOG_NET, "Unable to resolve the address of the modem"); + return false; + } + + if (m_controlSocket != nullptr) { + return m_controlSocket->open(m_controlAddr); + } else { + if (m_socket != nullptr) { + return m_socket->open(m_addr); + } + } + + return false; +} + +/* Reads data from the port. */ + +int V24UDPPort::read(uint8_t* buffer, uint32_t length) +{ + assert(buffer != nullptr); + assert(length > 0U); + + // Get required data from the ring buffer + uint32_t avail = m_buffer.dataSize(); + if (avail < length) + length = avail; + + if (length > 0U) + m_buffer.get(buffer, length); + + return int(length); +} + +/* Writes data to the port. */ + +int V24UDPPort::write(const uint8_t* buffer, uint32_t length) +{ + assert(buffer != nullptr); + assert(length > 0U); + + switch (buffer[2U]) { + case CMD_GET_VERSION: + getVersion(); + return int(length); + case CMD_GET_STATUS: + getStatus(); + return int(length); + case CMD_SET_CONFIG: + case CMD_SET_MODE: + writeAck(buffer[2U]); + return int(length); + case CMD_P25_DATA: + { + if (m_socket != nullptr) { + uint32_t messageLen = 0U; + uint8_t* message = generateMessage(buffer + 3U, length - 3U, m_streamId, m_peerId, m_pktSeq, &messageLen); + + bool written = m_socket->write(message, messageLen, m_addr, m_addrLen); + if (written) + return length; + } else { + writeNAK(CMD_P25_DATA, RSN_INVALID_REQUEST); + return int(length); + } + } + break; + case CMD_FLSH_READ: + writeNAK(CMD_FLSH_READ, RSN_NO_INTERNAL_FLASH); + return int(length); + default: + return int(length); + } + + return -1; +} + +/* Closes the connection to the port. */ + +void V24UDPPort::close() +{ + if (m_controlSocket != nullptr) + m_controlSocket->close(); + if (m_socket != nullptr) + m_socket->close(); +} + +// --------------------------------------------------------------------------- +// Private Class Members +// --------------------------------------------------------------------------- + +/* Process FSC control frames from the network. */ + +void V24UDPPort::processCtrlNetwork() +{ + sockaddr_storage address; + uint32_t addrLen; + int length = 0U; + + // read message + UInt8Array buffer = m_ctrlFrameQueue->read(length, address, addrLen); + if (length > 0) { + if (m_debug) + Utils::dump(1U, "FSC Control Network Message", buffer.get(), length); + + V24PacketRequest* req = new V24PacketRequest(); + req->address = address; + req->addrLen = addrLen; + + req->length = length; + req->buffer = new uint8_t[length]; + ::memcpy(req->buffer, buffer.get(), length); + + if (!Thread::runAsThread(this, threadedCtrlNetworkRx, req)) { + delete[] req->buffer; + delete req; + return; + } + } +} + +/* Process a data frames from the network. */ + +void* V24UDPPort::threadedCtrlNetworkRx(void* arg) +{ + V24PacketRequest* req = (V24PacketRequest*)arg; + if (req != nullptr) { + ::pthread_detach(req->thread); + + V24UDPPort* network = static_cast(req->obj); + if (network == nullptr) { + delete req; + return nullptr; + } + + if (req->length > 0) { + if (network->m_reqConnectionToPeer) { + // FSC_CONNECT response -- is ... strange + if (req->buffer[0] == 1U) { + network->m_reqConnectionToPeer = false; + network->m_reqConnectionTimer.stop(); + network->m_establishedConnection = true; + + FSCConnectResponse resp = FSCConnectResponse(req->buffer); + uint16_t vcBasePort = resp.getVCBasePort(); + + network->m_localPort = vcBasePort; + network->createVCPort(vcBasePort); + network->m_heartbeatTimer.start(); + + uint8_t buffer[FSCConnectResponse::LENGTH]; + ::memset(buffer, 0x00U, FSCConnectResponse::LENGTH); + + resp.setVCBasePort(network->m_localPort); + resp.encode(buffer); + + network->m_ctrlFrameQueue->write(buffer, FSCConnectResponse::LENGTH, network->m_controlAddr, network->m_ctrlAddrLen); + } + } + else + { + std::unique_ptr message = FSCMessage::createMessage(req->buffer); + if (message != nullptr) { + switch (message->getMessageId()) + { + case FSCMessageType::FSC_ACK: + { + FSCACK* ackMessage = static_cast(message.get()); + switch (ackMessage->getResponseCode()) + { + case FSCAckResponseCode::CONTROL_NAK: + case FSCAckResponseCode::CONTROL_NAK_CONNECTED: + case FSCAckResponseCode::CONTROL_NAK_M_UNSUPP: + case FSCAckResponseCode::CONTROL_NAK_V_UNSUPP: + case FSCAckResponseCode::CONTROL_NAK_F_UNSUPP: + case FSCAckResponseCode::CONTROL_NAK_PARMS: + case FSCAckResponseCode::CONTROL_NAK_BUSY: + LogError(LOG_MODEM, "V.24 UDP, ACK, ackMessageId = $%02X, ackResponseCode = $%02X", ackMessage->getAckMessageId(), ackMessage->getResponseCode()); + break; + + case FSCAckResponseCode::CONTROL_ACK: + { + if (ackMessage->getAckMessageId() == FSCMessageType::FSC_DISCONNECT) { + network->m_reqConnectionTimer.stop(); + network->m_reqConnectionToPeer = false; + network->m_establishedConnection = false; + network->m_heartbeatTimer.stop(); + } + } + break; + + default: + LogError(LOG_MODEM, "V.24 UDP, unknown ACK opcode, ackMessageId = $%02X", ackMessage->getAckMessageId()); + break; + } + } + break; + + case FSCMessageType::FSC_CONNECT: + { + network->createVCPort(network->m_localPort); + network->m_heartbeatTimer.start(); + + uint8_t buffer[FSCConnectResponse::LENGTH]; + ::memset(buffer, 0x00U, FSCConnectResponse::LENGTH); + + FSCConnectResponse resp = FSCConnectResponse(req->buffer); + resp.setVCBasePort(network->m_localPort); + resp.encode(buffer); + + network->m_ctrlFrameQueue->write(buffer, FSCConnectResponse::LENGTH, network->m_controlAddr, network->m_ctrlAddrLen); + } + break; + + case FSCMessageType::FSC_DISCONNECT: + { + network->m_reqConnectionTimer.stop(); + network->m_reqConnectionToPeer = false; + network->m_establishedConnection = false; + network->m_heartbeatTimer.stop(); + } + break; + + case FSCMessageType::FSC_HEARTBEAT: + { + if (network->m_establishedConnection) { + network->writeHeartbeat(); + } + } + break; + + default: + break; + } + } + } + } + + if (req->buffer != nullptr) + delete[] req->buffer; + delete req; + } + + return nullptr; +} + +/* Internal helper to setup the voice channel port. */ + +void V24UDPPort::createVCPort(uint16_t port) +{ + m_socket = new Socket(port); + + if (udp::Socket::lookup(m_address, port, m_addr, m_addrLen) != 0) + m_addrLen = 0U; + + if (m_addrLen > 0U) { + std::string addrStr = udp::Socket::address(m_addr); + LogWarning(LOG_HOST, "SECURITY: Remote modem expects V.24 voice channel IP address; %s for remote modem control", addrStr.c_str()); + } +} + +/* Internal helper to write a FSC connect packet. */ + +void V24UDPPort::writeConnect() +{ + FSCConnect connect = FSCConnect(); + connect.setFSHeartbeatPeriod(5U); // hardcoded? + connect.setHostHeartbeatPeriod(5U); // hardcoded? + connect.setVCBasePort(m_localPort); + connect.setVCSSRC(m_peerId); + + uint8_t buffer[FSCConnect::LENGTH]; + ::memset(buffer, 0x00U, FSCConnect::LENGTH); + + connect.encode(buffer); + + m_ctrlFrameQueue->write(buffer, FSCConnect::LENGTH, m_controlAddr, m_ctrlAddrLen); +} + +/* Internal helper to write a FSC heartbeat packet. */ + +void V24UDPPort::writeHeartbeat() +{ + uint8_t buffer[FSCHeartbeat::LENGTH]; + ::memset(buffer, 0x00U, FSCHeartbeat::LENGTH); + + FSCHeartbeat hb = FSCHeartbeat(); + hb.encode(buffer); + + m_ctrlFrameQueue->write(buffer, FSCHeartbeat::LENGTH, m_controlAddr, m_ctrlAddrLen); +} + +/* Generate RTP message. */ + +uint8_t* V24UDPPort::generateMessage(const uint8_t* message, uint32_t length, uint32_t streamId, + uint32_t ssrc, uint16_t rtpSeq, uint32_t* outBufferLen) +{ + assert(message != nullptr); + assert(length > 0U); + + uint32_t timestamp = m_timestamp; + if (timestamp != INVALID_TS) { + timestamp += (RTP_GENERIC_CLOCK_RATE / 133); + if (m_debug) + LogDebug(LOG_NET, "V24UDPPort::generateMessage() RTP streamId = %u, previous TS = %u, TS = %u, rtpSeq = %u", streamId, m_timestamp, timestamp, rtpSeq); + m_timestamp = timestamp; + } + + uint32_t bufferLen = RTP_HEADER_LENGTH_BYTES + length; + uint8_t* buffer = new uint8_t[bufferLen]; + ::memset(buffer, 0x00U, bufferLen); + + RTPHeader header = RTPHeader(); + header.setExtension(false); + + header.setPayloadType(DFSI_RTP_PAYLOAD_TYPE); + header.setTimestamp(timestamp); + header.setSequence(rtpSeq); + header.setSSRC(ssrc); + + header.encode(buffer); + + if (streamId != 0U && timestamp == INVALID_TS && rtpSeq != RTP_END_OF_CALL_SEQ) { + if (m_debug) + LogDebug(LOG_NET, "V24UDPPort::generateMessage() RTP streamId = %u, initial TS = %u, rtpSeq = %u", streamId, header.getTimestamp(), rtpSeq); + m_timestamp = header.getTimestamp(); + } + + if (streamId != 0U && rtpSeq == RTP_END_OF_CALL_SEQ) { + m_timestamp = INVALID_TS; + if (m_debug) + LogDebug(LOG_NET, "V24UDPPort::generateMessage() RTP streamId = %u, rtpSeq = %u", streamId, rtpSeq); + } + + ::memcpy(buffer + RTP_HEADER_LENGTH_BYTES, message, length); + + if (m_debug) + Utils::dump(1U, "V24UDPPort::generateMessage() Buffered Message", buffer, bufferLen); + + if (outBufferLen != nullptr) { + *outBufferLen = bufferLen; + } + + return buffer; +} + +/* Helper to return a faked modem version. */ + +void V24UDPPort::getVersion() +{ + uint8_t reply[200U]; + + reply[0U] = DVM_SHORT_FRAME_START; + reply[1U] = 0U; + reply[2U] = CMD_GET_VERSION; + + reply[3U] = V24_UDP_PROTOCOL_VERSION; + reply[4U] = 15U; + + // Reserve 16 bytes for the UDID + ::memset(reply + 5U, 0x00U, 16U); + + uint8_t count = 21U; + for (uint8_t i = 0U; V24_UDP_HARDWARE[i] != 0x00U; i++, count++) + reply[count] = V24_UDP_HARDWARE[i]; + + reply[1U] = count; + + m_buffer.addData(reply, count); +} + +/* Helper to return a faked modem status. */ + +void V24UDPPort::getStatus() +{ + uint8_t reply[15U]; + + // Send all sorts of interesting internal values + reply[0U] = DVM_SHORT_FRAME_START; + reply[1U] = 12U; + reply[2U] = CMD_GET_STATUS; + + reply[3U] = 0x00U; + reply[3U] |= 0x08U; // P25 enable flag + + reply[4U] = uint8_t(m_modemState); + + reply[5U] = m_tx ? 0x01U : 0x00U; + + reply[6U] = 0U; + + reply[7U] = 0U; + reply[8U] = 0U; + + reply[9U] = 0U; + + reply[10U] = 5U; // always report 5 P25 frames worth of space + + reply[11U] = 0U; + + m_buffer.addData(reply, 12U); +} + +/* Helper to write a faked modem acknowledge. */ + +void V24UDPPort::writeAck(uint8_t type) +{ + uint8_t reply[4U]; + + reply[0U] = DVM_SHORT_FRAME_START; + reply[1U] = 4U; + reply[2U] = CMD_ACK; + reply[3U] = type; + + m_buffer.addData(reply, 4U); +} + +/* Helper to write a faked modem negative acknowledge. */ + +void V24UDPPort::writeNAK(uint8_t opcode, uint8_t err) +{ + uint8_t reply[5U]; + + reply[0U] = DVM_SHORT_FRAME_START; + reply[1U] = 5U; + reply[2U] = CMD_NAK; + reply[3U] = opcode; + reply[4U] = err; + + m_buffer.addData(reply, 5U); +} \ No newline at end of file diff --git a/src/host/modem/port/specialized/V24UDPPort.h b/src/host/modem/port/specialized/V24UDPPort.h new file mode 100644 index 00000000..ba114168 --- /dev/null +++ b/src/host/modem/port/specialized/V24UDPPort.h @@ -0,0 +1,229 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Modem Host Software + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * @package DVM / Modem Host Software + * @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) + * @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) + * + * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * + */ +/** + * @file V24UDPPort.h + * @ingroup port + * @file V24UDPPort.cpp + * @ingroup port + */ +#if !defined(__V24_UDP_PORT_H__) +#define __V24_UDP_PORT_H__ + +#include "Defines.h" +#include "common/network/udp/Socket.h" +#include "common/network/RawFrameQueue.h" +#include "common/network/RTPHeader.h" +#include "common/RingBuffer.h" +#include "common/Timer.h" +#include "common/Thread.h" +#include "modem/port/IModemPort.h" + +#include +#include + +namespace modem +{ + namespace port + { + namespace specialized + { + // --------------------------------------------------------------------------- + // Structure Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Represents the data required for a V.24 network packet handler thread. + * @ingroup fne_network + */ + struct V24PacketRequest : thread_t { + sockaddr_storage address; //! IP Address and Port. + uint32_t addrLen; //! + network::frame::RTPHeader rtpHeader; //! RTP Header + int length = 0U; //! Length of raw data buffer + uint8_t *buffer; //! Raw data buffer + }; + + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief This class implements low-level routines to communicate over UDP for V.24. + * @ingroup port + */ + class HOST_SW_API V24UDPPort : public IModemPort { + public: + /** + * @brief Initializes a new instance of the V24UDPPort class. + * @param peerId Unique ID of this modem on the network. + * @param address Hostname/IP address to connect to. + * @param modemPort Port number. + * @param controlPort Control Port number. + * @param useFSC Flag indicating whether or not FSC handshakes are used to setup communications. + * @param debug Flag indicating whether network debug is enabled. + */ + V24UDPPort(uint32_t peerId, const std::string& modemAddress, uint16_t modemPort, uint16_t controlPort = 0U, bool useFSC = false, bool debug = false); + /** + * @brief Finalizes a instance of the V24UDPPort class. + */ + ~V24UDPPort() override; + + /** + * @brief Updates the timer by the passed number of milliseconds. + * @param ms Number of milliseconds. + */ + void clock(uint32_t ms); + + /** + * @brief Resets the RTP packet sequence and stream ID. + */ + void reset(); + + /** + * @brief Opens a connection to the serial port. + * @returns bool True, if connection is opened, otherwise false. + */ + bool open() override; + + /** + * @brief Reads data from the serial port. + * @param[out] buffer Buffer to read data from the port to. + * @param length Length of data to read from the port. + * @returns int Actual length of data read from serial port. + */ + int read(uint8_t* buffer, uint32_t length) override; + /** + * @brief Writes data to the serial port. + * @param[in] buffer Buffer containing data to write to port. + * @param length Length of data to write to port. + * @returns int Actual length of data written to the port. + */ + int write(const uint8_t* buffer, uint32_t length) override; + + /** + * @brief Closes the connection to the serial port. + */ + void close() override; + + private: + network::udp::Socket* m_socket; + uint16_t m_localPort; + + network::udp::Socket* m_controlSocket; + network::RawFrameQueue* m_ctrlFrameQueue; + + std::string m_address; + sockaddr_storage m_addr; + sockaddr_storage m_controlAddr; + uint32_t m_addrLen; + uint32_t m_ctrlAddrLen; + + RingBuffer m_buffer; + + Timer m_reqConnectionTimer; + Timer m_heartbeatTimer; + + bool m_reqConnectionToPeer; + bool m_establishedConnection; + + std::mt19937 m_random; + + uint32_t m_peerId; + + uint32_t m_streamId; + uint32_t m_timestamp; + uint16_t m_pktSeq; + + uint8_t m_modemState; + bool m_tx; + + bool m_debug; + + /** + * @brief Process FSC control frames from the network. + */ + void processCtrlNetwork(); + + /** + * @brief Entry point to process a given network packet. + * @param arg Instance of the NetPacketRequest structure. + * @returns void* (Ignore) + */ + static void* threadedCtrlNetworkRx(void* arg); + + /** + * @brief Internal helper to setup the voice channel port. + * @param port Port number. + */ + void createVCPort(uint16_t port); + /** + * @brief Internal helper to write a FSC connect packet. + */ + void writeConnect(); + /** + * @brief Internal helper to write a FSC heartbeat packet. + */ + void writeHeartbeat(); + + /** + * @brief Helper to update the RTP packet sequence. + * @param reset Flag indicating the current RTP packet sequence value should be reset. + * @returns uint16_t RTP packet sequence. + */ + uint16_t pktSeq(bool reset = false); + + /** + * @brief Generates a new stream ID. + * @returns uint32_t New stream ID. + */ + uint32_t createStreamId() { std::uniform_int_distribution dist(0x00000001, 0xfffffffe); return dist(m_random); } + + /** + * @brief Generate RTP message. + * @param[in] message Message buffer to frame and queue. + * @param length Length of message. + * @param streamId Message stream ID. + * @param ssrc RTP SSRC ID. + * @param rtpSeq RTP Sequence. + * @param[out] outBufferLen Length of buffer generated. + * @returns uint8_t* Buffer containing RTP message. + */ + uint8_t* generateMessage(const uint8_t* message, uint32_t length, uint32_t streamId, + uint32_t ssrc, uint16_t rtpSeq, uint32_t* outBufferLen); + + /** + * @brief Helper to return a faked modem version. + */ + void getVersion(); + /** + * @brief Helper to return a faked modem status. + */ + void getStatus(); + /** + * @brief Helper to write a faked modem acknowledge. + * @param type + */ + void writeAck(uint8_t type); + /** + * @brief Helper to write a faked modem negative acknowledge. + * @param opcode + * @param err + */ + void writeNAK(uint8_t opcode, uint8_t err); + }; + } // namespace specialized + } // namespace port +} // namespace modem + +#endif // __V24_UDP_PORT_H__ diff --git a/src/host/network/Network.cpp b/src/host/network/Network.cpp index 3ca151f3..08ddd4dc 100644 --- a/src/host/network/Network.cpp +++ b/src/host/network/Network.cpp @@ -22,6 +22,12 @@ using namespace network; #include #include +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +#define MAX_SERVER_DIFF 250 // maximum difference in time between a server timestamp and local timestamp in milliseconds + // --------------------------------------------------------------------------- // Public Class Members // --------------------------------------------------------------------------- @@ -193,6 +199,8 @@ void Network::clock(uint32_t ms) return; } + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + // roll the RTP timestamp if no call is in progress if ((m_status == NET_STAT_RUNNING) && (m_rxDMRStreamId[0U] == 0U && m_rxDMRStreamId[1U] == 0U) && @@ -580,6 +588,24 @@ void Network::clock(uint32_t ms) break; case NET_FUNC::PONG: // Master Ping Response m_timeoutTimer.start(); + if (length >= 14) { + ulong64_t serverNow = 0U; + + // combine bytes into ulong64_t (8 byte) value + serverNow = buffer[6U]; + serverNow = (serverNow << 8) + buffer[7U]; + serverNow = (serverNow << 8) + buffer[8U]; + serverNow = (serverNow << 8) + buffer[9U]; + serverNow = (serverNow << 8) + buffer[10U]; + serverNow = (serverNow << 8) + buffer[11U]; + serverNow = (serverNow << 8) + buffer[12U]; + serverNow = (serverNow << 8) + buffer[13U]; + + // check the ping RTT and report any over the maximum defined time + uint64_t dt = now - serverNow; + if (dt > MAX_SERVER_DIFF) + LogWarning(LOG_NET, "PEER %u pong, time delay greater than %ums, now = %u, server = %u, dt = %u", m_peerId, MAX_SERVER_DIFF, now, serverNow, dt); + } break; default: Utils::dump("unknown opcode from the master", buffer.get(), length); diff --git a/src/host/p25/Control.cpp b/src/host/p25/Control.cpp index 0d31b869..3976bcfb 100644 --- a/src/host/p25/Control.cpp +++ b/src/host/p25/Control.cpp @@ -17,6 +17,7 @@ #include "common/Log.h" #include "common/Utils.h" #include "p25/Control.h" +#include "modem/ModemV24.h" #include "remote/RESTClient.h" #include "ActivityLog.h" #include "HostMain.h" @@ -55,6 +56,7 @@ Control::Control(bool authoritative, uint32_t nac, uint32_t callHang, uint32_t q m_txNAC(nac), m_timeout(timeout), m_modem(modem), + m_isModemDFSI(false), m_network(network), m_inhibitUnauth(false), m_legacyGroupGrnt(true), @@ -126,6 +128,11 @@ Control::Control(bool authoritative, uint32_t nac, uint32_t callHang, uint32_t q assert(idenTable != nullptr); assert(rssiMapper != nullptr); + // bryanb: this is a hacky check to see if the modem is a ModemV24 or not... + modem::ModemV24* modemV24 = dynamic_cast(modem); + if (modemV24 != nullptr) + m_isModemDFSI = true; + m_interval.start(); acl::AccessControl::init(m_ridLookup, m_tidLookup); @@ -282,6 +289,8 @@ void Control::setOptions(yaml::Node& conf, bool supervisor, const std::string cw m_ackTSBKRequests = control["ackRequests"].as(true); m_control->m_ctrlTSDUMBF = !control["disableTSDUMBF"].as(false); + if (m_isModemDFSI) + m_control->m_ctrlTSDUMBF = false; // force single block TSDUs for DFSI mode m_control->m_ctrlTimeDateAnn = control["enableTimeDateAnn"].as(false); m_control->m_redundantImmediate = control["redundantImmediate"].as(true); m_control->m_redundantGrant = control["redundantGrantTransmit"].as(false); @@ -1631,6 +1640,9 @@ void Control::writeRF_Preamble(uint32_t preambleCount, bool force) if (!m_duplex && !force) return; + if (m_isModemDFSI) + return; + if (preambleCount == 0) { preambleCount = m_tduPreambleCount; } diff --git a/src/host/p25/Control.h b/src/host/p25/Control.h index 00c5e2f4..c7cb817e 100644 --- a/src/host/p25/Control.h +++ b/src/host/p25/Control.h @@ -283,6 +283,7 @@ namespace p25 uint32_t m_timeout; modem::Modem* m_modem; + bool m_isModemDFSI; network::Network* m_network; bool m_inhibitUnauth; diff --git a/src/host/p25/packet/Voice.cpp b/src/host/p25/packet/Voice.cpp index 7ef6eec9..c3d017e0 100644 --- a/src/host/p25/packet/Voice.cpp +++ b/src/host/p25/packet/Voice.cpp @@ -513,7 +513,7 @@ bool Voice::process(uint8_t* data, uint32_t len) writeNetwork(buffer, DUID::HDU); - if (m_p25->m_duplex) { + if (m_p25->m_duplex && !m_p25->m_isModemDFSI) { buffer[0U] = modem::TAG_DATA; buffer[1U] = 0x00U; @@ -735,7 +735,7 @@ bool Voice::process(uint8_t* data, uint32_t len) writeNetwork(data + 2U, DUID::LDU1, frameType); - if (m_p25->m_duplex) { + if (m_p25->m_duplex && !m_p25->m_isModemDFSI) { data[0U] = modem::TAG_DATA; data[1U] = 0x00U; @@ -852,7 +852,7 @@ bool Voice::process(uint8_t* data, uint32_t len) writeNetwork(data + 2U, DUID::LDU2); - if (m_p25->m_duplex) { + if (m_p25->m_duplex && !m_p25->m_isModemDFSI) { data[0U] = modem::TAG_DATA; data[1U] = 0x00U;