From 0bbc69d237c4da1eff39b53e0ddbfadf2ce316d4 Mon Sep 17 00:00:00 2001 From: Patrick W3AXL Date: Tue, 18 Jun 2024 11:13:41 -0400 Subject: [PATCH] Basic implementation of dvmdfsi in dvmhost C++ ecosystem (#59) * initial bootstrap of CPP dvmdfsi * more work on serial service, the basics are there now * updated TODOs * more work on dfsi, getting there, just a few more things to implement * rough code finished, totally untested, hope it works * fixes for malloc errors, still not totally working * almost working, P25 voice from FNE is garbled but we're getting there * verified v24 decode/encode is working, still cleaning up serial TX * dvmdfsi ready for beta testing! A few gremlins to find but it works. * added configurable source flag option * fixed diu source flag config entry name * small update to Mot VHDR1 * fixed serial initialization, flags work now, config & code cleanup * log cleanups, added basic call collision logic, fixed kid being truncated to uint8_t * fixed LDU2 MI not getting copied properly * bring add-dvmdfsi up-to-date with master; * Add syslog and some project file cleanup (#58) * initial bootstrap of CPP dvmdfsi * more work on serial service, the basics are there now * updated TODOs * more work on dfsi, getting there, just a few more things to implement * rough code finished, totally untested, hope it works * fixes for malloc errors, still not totally working * almost working, P25 voice from FNE is garbled but we're getting there * verified v24 decode/encode is working, still cleaning up serial TX * dvmdfsi ready for beta testing! A few gremlins to find but it works. * added configurable source flag option * fixed diu source flag config entry name * small update to Mot VHDR1 * fixed serial initialization, flags work now, config & code cleanup * log cleanups, added basic call collision logic, fixed kid being truncated to uint8_t * fixed LDU2 MI not getting copied properly * add support for syslog logging in dvmdfsi; * add useSyslog parameter to log section of configuration file; * project cleanup: split MotRtpFrames.cpp/.h into separate files properly; ensure decode() functions pass the pointer as const to prevent accidental modification of input buffer; move common enums to a separate RtpDefines.h header; reuse MotRtpFrames.h (renamed RtpFrames.h) as a quick way of including all the RTP frames; --------- Co-authored-by: W3AXL <29879554+W3AXL@users.noreply.github.com> --------- Co-authored-by: W3AXL <29879554+W3AXL@users.noreply.github.com> Co-authored-by: Bryan Biedenkapp --- .gitignore | 1 + CMakeLists.txt | 28 +- configs/dfsi-config.example.yml | 99 ++ src/CMakeLists.txt | 8 + src/common/Log.h | 1 + src/dfsi/ActivityLog.cpp | 163 +++ src/dfsi/ActivityLog.h | 32 + src/dfsi/CMakeLists.txt | 32 + src/dfsi/Defines.h | 36 + src/dfsi/Dfsi.cpp | 400 ++++++ src/dfsi/Dfsi.h | 70 + src/dfsi/DfsiMain.cpp | 247 ++++ src/dfsi/DfsiMain.h | 42 + src/dfsi/network/CallData.cpp | 89 ++ src/dfsi/network/CallData.h | 85 ++ src/dfsi/network/DfsiPeerNetwork.cpp | 337 +++++ src/dfsi/network/DfsiPeerNetwork.h | 56 + src/dfsi/network/SerialService.cpp | 1844 ++++++++++++++++++++++++++ src/dfsi/network/SerialService.h | 157 +++ src/dfsi/rtp/MotFullRateVoice.cpp | 211 +++ src/dfsi/rtp/MotFullRateVoice.h | 85 ++ src/dfsi/rtp/MotRtpFrames.cpp | 432 ++++++ src/dfsi/rtp/MotRtpFrames.h | 169 +++ src/dfsi/rtp/MotStartOfStream.cpp | 81 ++ src/dfsi/rtp/MotStartOfStream.h | 67 + src/dfsi/rtp/MotStartVoiceFrame.cpp | 134 ++ src/dfsi/rtp/MotStartVoiceFrame.h | 79 ++ src/dfsi/rtp/MotVoiceHeader1.cpp | 123 ++ src/dfsi/rtp/MotVoiceHeader1.h | 82 ++ src/dfsi/rtp/MotVoiceHeader2.cpp | 92 ++ src/dfsi/rtp/MotVoiceHeader2.h | 79 ++ src/dfsi/rtp/RtpDefines.h | 77 ++ src/dfsi/rtp/RtpFrames.h | 26 + src/fw/hotspot | 2 +- src/fw/modem | 2 +- src/host/modem/port/UARTPort.cpp | 2 +- 36 files changed, 5459 insertions(+), 11 deletions(-) create mode 100644 configs/dfsi-config.example.yml create mode 100644 src/dfsi/ActivityLog.cpp create mode 100644 src/dfsi/ActivityLog.h create mode 100644 src/dfsi/CMakeLists.txt create mode 100644 src/dfsi/Defines.h create mode 100644 src/dfsi/Dfsi.cpp create mode 100644 src/dfsi/Dfsi.h create mode 100644 src/dfsi/DfsiMain.cpp create mode 100644 src/dfsi/DfsiMain.h create mode 100644 src/dfsi/network/CallData.cpp create mode 100644 src/dfsi/network/CallData.h create mode 100644 src/dfsi/network/DfsiPeerNetwork.cpp create mode 100644 src/dfsi/network/DfsiPeerNetwork.h create mode 100644 src/dfsi/network/SerialService.cpp create mode 100644 src/dfsi/network/SerialService.h create mode 100644 src/dfsi/rtp/MotFullRateVoice.cpp create mode 100644 src/dfsi/rtp/MotFullRateVoice.h create mode 100644 src/dfsi/rtp/MotRtpFrames.cpp create mode 100644 src/dfsi/rtp/MotRtpFrames.h create mode 100644 src/dfsi/rtp/MotStartOfStream.cpp create mode 100644 src/dfsi/rtp/MotStartOfStream.h create mode 100644 src/dfsi/rtp/MotStartVoiceFrame.cpp create mode 100644 src/dfsi/rtp/MotStartVoiceFrame.h create mode 100644 src/dfsi/rtp/MotVoiceHeader1.cpp create mode 100644 src/dfsi/rtp/MotVoiceHeader1.h create mode 100644 src/dfsi/rtp/MotVoiceHeader2.cpp create mode 100644 src/dfsi/rtp/MotVoiceHeader2.h create mode 100644 src/dfsi/rtp/RtpDefines.h create mode 100644 src/dfsi/rtp/RtpFrames.h diff --git a/.gitignore b/.gitignore index 7908fa5a..ba10a85a 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ build/ dvmcmd dvmhost +dvmdfsi # Ignore CMake temporary files CMakeLists.txt.user diff --git a/CMakeLists.txt b/CMakeLists.txt index a42356c1..e2c99569 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -230,6 +230,7 @@ endif (ENABLE_TESTS) install(TARGETS dvmhost DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) install(TARGETS dvmcmd DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) install(TARGETS dvmfne DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) +install(TARGETS dvmdfsi DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) if (ENABLE_TUI_SUPPORT AND (NOT DISABLE_MONITOR)) install(TARGETS dvmmon DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) endif (ENABLE_TUI_SUPPORT AND (NOT DISABLE_MONITOR)) @@ -251,12 +252,14 @@ if (NOT TARGET strip) COMMAND arm-linux-gnueabihf-strip -s dvmhost COMMAND arm-linux-gnueabihf-strip -s dvmfne COMMAND arm-linux-gnueabihf-strip -s dvmcmd - COMMAND arm-linux-gnueabihf-strip -s dvmmon) + COMMAND arm-linux-gnueabihf-strip -s dvmmon + COMMAND arm-linux-gnueabihf-strip -s dvmdfsi) else() add_custom_target(strip COMMAND arm-linux-gnueabihf-strip -s dvmhost COMMAND arm-linux-gnueabihf-strip -s dvmfne - COMMAND arm-linux-gnueabihf-strip -s dvmcmd) + COMMAND arm-linux-gnueabihf-strip -s dvmcmd + COMMAND arm-linux-gnueabihf-strip -s dvmdfsi) endif (ENABLE_TUI_SUPPORT AND (NOT DISABLE_MONITOR)) elseif (CROSS_COMPILE_AARCH64) if (ENABLE_TUI_SUPPORT AND (NOT DISABLE_MONITOR)) @@ -264,24 +267,28 @@ if (NOT TARGET strip) COMMAND aarch64-linux-gnu-strip -s dvmhost COMMAND aarch64-linux-gnu-strip -s dvmfne COMMAND aarch64-linux-gnu-strip -s dvmcmd - COMMAND aarch64-linux-gnu-strip -s dvmmon) + COMMAND aarch64-linux-gnu-strip -s dvmmon + COMMAND aarch64-linux-gnu-strip -s dvmdfsi) else() add_custom_target(strip COMMAND aarch64-linux-gnu-strip -s dvmhost COMMAND aarch64-linux-gnu-strip -s dvmfne - COMMAND aarch64-linux-gnu-strip -s dvmcmd) + COMMAND aarch64-linux-gnu-strip -s dvmcmd + COMMAND aarch64-linux-gnu-strip -s dvmdfsi) endif (ENABLE_TUI_SUPPORT AND (NOT DISABLE_MONITOR)) elseif (CROSS_COMPILE_RPI_ARM) if (NOT WITH_RPI_ARM_TOOLS) add_custom_target(strip COMMAND ${CMAKE_CURRENT_BINARY_DIR}/_deps/rpitools-src/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmhost COMMAND ${CMAKE_CURRENT_BINARY_DIR}/_deps/rpitools-src/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmfne - COMMAND ${CMAKE_CURRENT_BINARY_DIR}/_deps/rpitools-src/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmcmd) + COMMAND ${CMAKE_CURRENT_BINARY_DIR}/_deps/rpitools-src/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmcmd + COMMAND ${CMAKE_CURRENT_BINARY_DIR}/_deps/rpitools-src/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmdfsi) else() add_custom_target(strip COMMAND ${RPI_ARM_TOOLS}/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmhost COMMAND ${RPI_ARM_TOOLS}/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmfne - COMMAND ${RPI_ARM_TOOLS}/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmcmd) + COMMAND ${RPI_ARM_TOOLS}/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmcmd + COMMAND ${RPI_ARM_TOOLS}/arm-bcm2708/arm-linux-gnueabihf/bin/arm-linux-gnueabihf-strip -s dvmdfsi) endif () else() if (ENABLE_TUI_SUPPORT AND (NOT DISABLE_MONITOR)) @@ -289,12 +296,14 @@ if (NOT TARGET strip) COMMAND strip -s dvmhost COMMAND strip -s dvmfne COMMAND strip -s dvmcmd - COMMAND strip -s dvmmon) + COMMAND strip -s dvmmon + COMMAND strip -s dvmdfsi) else() add_custom_target(strip COMMAND strip -s dvmhost COMMAND strip -s dvmfne - COMMAND strip -s dvmcmd) + COMMAND strip -s dvmcmd + COMMAND strip -s dvmdfsi) endif (ENABLE_TUI_SUPPORT AND (NOT DISABLE_MONITOR)) endif (CROSS_COMPILE_ARM) endif (NOT TARGET strip) @@ -320,6 +329,7 @@ if (NOT TARGET tarball) COMMAND cp -v dvmcmd ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/bin COMMAND cp -v dvmmon ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/bin COMMAND cp -v dvmfne ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/bin + COMMAND cp -v dvmdfsi ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/bin COMMAND cp ../tools/*.sh ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm COMMAND chmod +x ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/*.sh COMMAND cp -v ../configs/*.yml ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm @@ -348,6 +358,7 @@ if (NOT TARGET tarball) COMMAND cp -v dvmhost ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/bin COMMAND cp -v dvmcmd ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/bin COMMAND cp -v dvmfne ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/bin + COMMAND cp -v dvmdfsi ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/bin COMMAND cp ../tools/*.sh ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm COMMAND chmod +x ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm/*.sh COMMAND cp -v ../configs/*.yml ${CMAKE_INSTALL_PREFIX_TARBALL}/dvm @@ -386,6 +397,7 @@ add_custom_target(old_install COMMAND install -m 755 dvmcmd ${CMAKE_LEGACY_INSTALL_PREFIX}/bin COMMAND install -m 755 dvmmon ${CMAKE_LEGACY_INSTALL_PREFIX}/bin COMMAND install -m 755 dvmfne ${CMAKE_LEGACY_INSTALL_PREFIX}/bin + COMMAND install -m 755 dvmdfsi ${CMAKE_LEGACY_INSTALL_PREFIX}/bin COMMAND install -m 644 ../configs/config.example.yml ${CMAKE_LEGACY_INSTALL_PREFIX}/config.example.yml COMMAND install -m 644 ../configs/fne-config.example.yml ${CMAKE_LEGACY_INSTALL_PREFIX}/fne-config.example.yml COMMAND install -m 644 ../configs/monitor-config.example.yml ${CMAKE_LEGACY_INSTALL_PREFIX}/monitor-config.example.yml diff --git a/configs/dfsi-config.example.yml b/configs/dfsi-config.example.yml new file mode 100644 index 00000000..235a8f2b --- /dev/null +++ b/configs/dfsi-config.example.yml @@ -0,0 +1,99 @@ +# +# Digital Voice Modem - DFSI Software Configuration +# +# @package DVM / DFSI Software +# + +# Flag indicating whether the host will run as a background or foreground task. +daemon: false + +# +# Network Configuration +# +network: + # Textual Name + identity: DFSI + # Network Peer ID + peerId: 9000123 + # Hostname/IP address of FNE master to connect to. + address: 127.0.0.1 + # Port number to connect to. + port: 62031 + # FNE access password. + password: RPT1234 + # Additional debug logging + debug: false + # Encrypted endpoint networking + encrypted: false + # Pre-shared key for encrypted networking (AES-256 32-byte hex keystring) + presharedKey: "000102030405060708090A0B0C0D0E0F000102030405060708090A0B0C0D0E0F" + +# +# DFSI Configuration +# +dfsi: + # Mode + # 1 - UDP DFSI to DVM FNE (TODO: Implement this) + # 2 - Serial DFSI to DVM FNE (Using DVM-V24 adapter board) + # 3 - Serial DFSI to UDP DFSI (TODO: Implement this) + mode: 2 + + # Flag enabling "the" Manufacturer standard of RTP. + theManufacturer: false + + # P25 buffer size in bytes (don't change this unless you have a good reason to) + # Default of 3060 is 10 full LDU1/2s worth of data + p25BufferSize: 3060 + + udp: + # Time in seconds between heartbets to DFSI peers. + heartbeat: 5 + # Flag disabling control connection establishment. + noConnectionEstablishment: false + # Local DFSI RTP Port number. + localRtpPort: 27500 + # Remote RFSS DFSI Hostname/IP address of FNE master to connect to. + remoteDfsiAddress: 127.0.0.2 + # Remote DFSI Control Port number to connect to. + remoteControlPort: 27000 + # Remote DFSI RTP Port number to connect to. + remoteRtpPort: 27500 + + serial: + # Serial configuration for serial DFSI + port: "/dev/ttyACM0" + baudrate: 115200 + # RT/RT flag enabled (0x02) or disabled (0x04) + rtrt: false + # Use the DIU source flag (0x00) instead of the quantar source flag (0x02) + diu: false + # Jitter buffer length in ms + jitter: 200 + # Debug logging + debug: false + # Trace logging (prints lots of data) + trace: false + +# +# Logging Configuration +# Logging Levels: +# 1 - Debug +# 2 - Message +# 3 - Informational +# 4 - Warning +# 5 - Error +# 6 - Fatal +# +log: + # Console display logging level (used when in foreground). + displayLevel: 1 + # File logging level. + fileLevel: 1 + # Flag indicating file logs should be sent to syslog instead of a file. + useSyslog: false + # Full path for the directory to store the log files. + filePath: . + # Full path for the directory to store the activity log files. + activityFilePath: . + # Log filename prefix. + fileRoot: dvmdfsi diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index fb50d73c..44d9c8f9 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -84,3 +84,11 @@ include(src/remote/CMakeLists.txt) add_executable(dvmcmd ${common_INCLUDE} ${dvmcmd_SRC}) target_link_libraries(dvmcmd PRIVATE common ${OPENSSL_LIBRARIES} asio::asio Threads::Threads) target_include_directories(dvmcmd PRIVATE ${OPENSSL_INCLUDE_DIR} src src/remote) + +# +## dvmdfsi +# +include(src/dfsi/CMakeLists.txt) +add_executable(dvmdfsi ${common_INCLUDE} ${dvmdfsi_SRC}) +target_link_libraries(dvmdfsi PRIVATE common ${OPENSSL_LIBRARIES} asio::asio Threads::Threads) +target_include_directories(dvmdfsi PRIVATE ${OPENSSL_INCLUDE_DIR} src src/host src/dfsi) \ No newline at end of file diff --git a/src/common/Log.h b/src/common/Log.h index ff1247e3..8df438c0 100644 --- a/src/common/Log.h +++ b/src/common/Log.h @@ -33,6 +33,7 @@ #define LOG_DMR "DMR" #define LOG_CAL "CAL" #define LOG_SETUP "SETUP" +#define LOG_SERIAL "SERIAL" // --------------------------------------------------------------------------- // Macros diff --git a/src/dfsi/ActivityLog.cpp b/src/dfsi/ActivityLog.cpp new file mode 100644 index 00000000..0db4446f --- /dev/null +++ b/src/dfsi/ActivityLog.cpp @@ -0,0 +1,163 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#include "ActivityLog.h" +#include "common/network/BaseNetwork.h" +#include "common/Log.h" // for CurrentLogFileLevel() and LogGetNetwork() + +#include + +#if defined(CATCH2_TEST_COMPILATION) +#include +#endif + +#include +#include +#include +#include +#include +#include + +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +#define EOL "\r\n" + +const uint32_t ACT_LOG_BUFFER_LEN = 501U; + +// --------------------------------------------------------------------------- +// Global Variables +// --------------------------------------------------------------------------- + +static std::string m_actFilePath; +static std::string m_actFileRoot; + +static FILE* m_actFpLog = nullptr; + +static struct tm m_actTm; + +// --------------------------------------------------------------------------- +// Global Functions +// --------------------------------------------------------------------------- + +/// +/// Helper to open the activity log file, file handle. +/// +/// True, if log file is opened, otherwise false. +static bool ActivityLogOpen() +{ + if (CurrentLogFileLevel() == 0U) + return true; + + time_t now; + ::time(&now); + + struct tm* tm = ::gmtime(&now); + + if (tm->tm_mday == m_actTm.tm_mday && tm->tm_mon == m_actTm.tm_mon && tm->tm_year == m_actTm.tm_year) { + if (m_actFpLog != nullptr) + return true; + } + else { + if (m_actFpLog != nullptr) + ::fclose(m_actFpLog); + } + + char filename[200U]; + ::sprintf(filename, "%s/%s-%04d-%02d-%02d.activity.log", LogGetFilePath().c_str(), LogGetFileRoot().c_str(), tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday); + + m_actFpLog = ::fopen(filename, "a+t"); + m_actTm = *tm; + + return m_actFpLog != nullptr; +} + +/// +/// Initializes the activity log. +/// +/// Full-path to the activity log file. +/// Prefix of the activity log file name. +bool ActivityLogInitialise(const std::string& filePath, const std::string& fileRoot) +{ +#if defined(CATCH2_TEST_COMPILATION) + return true; +#endif + m_actFilePath = filePath; + m_actFileRoot = fileRoot; + + return ::ActivityLogOpen(); +} + +/// +/// Finalizes the activity log. +/// +void ActivityLogFinalise() +{ +#if defined(CATCH2_TEST_COMPILATION) + return; +#endif + if (m_actFpLog != nullptr) + ::fclose(m_actFpLog); +} + +/// +/// Writes a new entry to the activity log. +/// +/// This is a variable argument function. +/// Formatted string to write to activity log. +void ActivityLog(const char* msg, ...) +{ +#if defined(CATCH2_TEST_COMPILATION) + return; +#endif + assert(msg != nullptr); + + char buffer[ACT_LOG_BUFFER_LEN]; + struct timeval now; + ::gettimeofday(&now, NULL); + + struct tm* tm = ::gmtime(&now.tv_sec); + + ::sprintf(buffer, "A: %04d-%02d-%02d %02d:%02d:%02d.%03lu ", tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, now.tv_usec / 1000U); + + va_list vl, vl_len; + va_start(vl, msg); + va_copy(vl_len, vl); + + size_t len = ::vsnprintf(nullptr, 0U, msg, vl_len); + ::vsnprintf(buffer + ::strlen(buffer), len + 1U, msg, vl); + + va_end(vl_len); + va_end(vl); + + bool ret = ::ActivityLogOpen(); + if (!ret) + return; + + if (LogGetNetwork() != nullptr) { + network::BaseNetwork* network = (network::BaseNetwork*)LogGetNetwork();; + network->writeActLog(buffer); + } + + if (CurrentLogFileLevel() == 0U) + return; + + ::fprintf(m_actFpLog, "%s\n", buffer); + ::fflush(m_actFpLog); + + if (2U >= g_logDisplayLevel && g_logDisplayLevel != 0U) { + ::fprintf(stdout, "%s" EOL, buffer); + ::fflush(stdout); + } +} \ No newline at end of file diff --git a/src/dfsi/ActivityLog.h b/src/dfsi/ActivityLog.h new file mode 100644 index 00000000..d699c9b6 --- /dev/null +++ b/src/dfsi/ActivityLog.h @@ -0,0 +1,32 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#if !defined(__ACTIVITY_LOG_H__) +#define __ACTIVITY_LOG_H__ + +#include "Defines.h" + +#include + +// --------------------------------------------------------------------------- +// Global Functions +// --------------------------------------------------------------------------- + +/// Initializes the activity log. +extern HOST_SW_API bool ActivityLogInitialise(const std::string& filePath, const std::string& fileRoot); +/// Finalizes the activity log. +extern HOST_SW_API void ActivityLogFinalise(); +/// Writes a new entry to the activity log. +extern HOST_SW_API void ActivityLog(const char* msg, ...); + +#endif // __ACTIVITY_LOG_H__ diff --git a/src/dfsi/CMakeLists.txt b/src/dfsi/CMakeLists.txt new file mode 100644 index 00000000..4dc20db5 --- /dev/null +++ b/src/dfsi/CMakeLists.txt @@ -0,0 +1,32 @@ +# 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 / DFSI peer application +#* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +#* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +#* +#* Copyright (C) 2024 Patrick McDonnell, W3AXL +#* +#*/ +file(GLOB dvmdfsi_SRC + # Modem libs for serial communication + "src/host/modem/*.h" + "src/host/modem/*.cpp" + "src/host/modem/port/*.h" + "src/host/modem/port/*.cpp" + # Core network libs + "src/host/network/Network.h" + "src/host/network/Network.cpp" + # DFSI network libs + "src/dfsi/network/*.h" + "src/dfsi/network/*.cpp" + # DFSI rtp libs + "src/dfsi/rtp/*.h" + "src/dfsi/rtp/*.cpp" + # Core DFSI + "src/dfsi/*.h" + "src/dfsi/*.cpp" +) \ No newline at end of file diff --git a/src/dfsi/Defines.h b/src/dfsi/Defines.h new file mode 100644 index 00000000..f71d410f --- /dev/null +++ b/src/dfsi/Defines.h @@ -0,0 +1,36 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#if !defined(__DEFINES_H__) +#define __DEFINES_H__ + +#include "common/Defines.h" + +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +#undef __PROG_NAME__ +#define __PROG_NAME__ "Digital Voice Modem (DVM) DFSI Peer" +#undef __EXE_NAME__ +#define __EXE_NAME__ "dvmdfsi" + +#undef __NETVER__ +#define __NETVER__ "DFSI" VERSION_MAJOR VERSION_REV VERSION_MINOR + +#undef DEFAULT_CONF_FILE +#define DEFAULT_CONF_FILE "dfsi-config.yml" +#undef DEFAULT_LOCK_FILE +#define DEFAULT_LOCK_FILE "/tmp/dvmdfsi.lock" + +#endif // __DEFINES_H__ \ No newline at end of file diff --git a/src/dfsi/Dfsi.cpp b/src/dfsi/Dfsi.cpp new file mode 100644 index 00000000..b6d52f73 --- /dev/null +++ b/src/dfsi/Dfsi.cpp @@ -0,0 +1,400 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#include "Defines.h" +#include "common/dmr/DMRDefines.h" +#include "common/p25/P25Utils.h" +#include "common/network/udp/Socket.h" +#include "common/Log.h" +#include "common/StopWatch.h" +#include "common/Thread.h" +#include "common/Utils.h" +#include "host/ActivityLog.h" +#include "Dfsi.h" +#include "DfsiMain.h" + +using namespace network; +using namespace lookups; + +#include +#include +#include +#include + +#include +#include + +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +#define IDLE_WARMUP_MS 5U + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/// +/// Initializes a new instance of the HostTest class. +/// +/// Full-path to the configuration file. +Dfsi::Dfsi(const std::string& confFile) : + m_confFile(confFile), + m_conf(), + m_network(nullptr), + m_ridLookup(nullptr), + m_tidLookup(nullptr), + m_pingTime(5U), + m_maxMissedPings(5U), + m_updateLookupTime(10U), + m_debug(false), + m_repeatTraffic(true), + m_serial(nullptr) +{ + /* stub */ +} + +/// +/// Finalizes a instance of the HostTest class. +/// +Dfsi::~Dfsi() = default; + +/// +/// Executes the main FNE processing loop. +/// +/// Zero if successful, otherwise error occurred. +int Dfsi::run() +{ + bool ret = false; + + // Try and parse the config yaml + try { + ret = yaml::Parse(m_conf, m_confFile.c_str()); + if (!ret) { + ::fatal("cannot read the configuration file, %s\n", m_confFile.c_str()); + } + } + catch (yaml::OperationException const& e) { + ::fatal("cannot read the configuration file - %s (%s)", m_confFile.c_str(), e.message()); + } + + // Check if we should run as a daemn or not + bool m_daemon = m_conf["daemon"].as(false); + if (m_daemon && g_foreground) + m_daemon = false; + + // initialize system logging + yaml::Node logConf = m_conf["log"]; + ret = ::LogInitialise(logConf["filePath"].as(), logConf["fileRoot"].as(), + logConf["fileLevel"].as(0U), logConf["displayLevel"].as(0U), false, logConf["useSyslog"].as(false)); + if (!ret) { + ::fatal("unable to open the log file\n"); + } + + // Init activity logging + ret = ::ActivityLogInitialise(logConf["activityFilePath"].as(), logConf["fileRoot"].as()); + if (!ret) { + ::fatal("unable to open the activity log file\n"); + } + + // handle POSIX process forking + if (m_daemon) { + // create new process + pid_t pid = ::fork(); + if (pid == -1) { + ::fprintf(stderr, "%s: Couldn't fork() , exiting\n", g_progExe.c_str()); + ::LogFinalise(); + return EXIT_FAILURE; + } + else if (pid != 0) { + ::LogFinalise(); + exit(EXIT_SUCCESS); + } + + // create new session and process group + if (::setsid() == -1) { + ::fprintf(stderr, "%s: Couldn't setsid(), exiting\n", g_progExe.c_str()); + ::LogFinalise(); + return EXIT_FAILURE; + } + + // set the working directory to the root directory + if (::chdir("/") == -1) { + ::fprintf(stderr, "%s: Couldn't cd /, exiting\n", g_progExe.c_str()); + ::LogFinalise(); + return EXIT_FAILURE; + } + + ::close(STDIN_FILENO); + ::close(STDOUT_FILENO); + ::close(STDERR_FILENO); + } + + ::LogInfo(__BANNER__ "\r\n" __PROG_NAME__ " " __VER__ " (built " __BUILD__ ")\r\n" \ + "Copyright (c) 2024 DVMProject (https://github.com/dvmproject) Authors.\r\n" \ + ">> DFSI Network Peer\r\n"); + + // read base parameters from configuration + ret = readParams(); + if (!ret) + return EXIT_FAILURE; + + // initialize peer networking + ret = createPeerNetwork(); + if (!ret) + return EXIT_FAILURE; + + ::LogInfoEx(LOG_HOST, "DFSI peer network is up and running"); + + // Read DFSI config + yaml::Node dfsi_conf = m_conf["dfsi"]; + uint32_t p25BufferSize = dfsi_conf["p25BufferSize"].as(); + + // Read serial config + yaml::Node serial_conf = dfsi_conf["serial"]; + std::string port = serial_conf["port"].as(); + uint32_t baudrate = serial_conf["baudrate"].as(); + bool rtrt = serial_conf["rtrt"].as(); + bool diu = serial_conf["diu"].as(); + uint16_t jitter = serial_conf["jitter"].as(); + bool serial_debug = serial_conf["debug"].as(); + bool serial_trace = serial_conf["trace"].as(); + + LogInfo("Serial Parameters"); + LogInfo(" Port: %s", port.c_str()); + LogInfo(" Baudrate: %u", baudrate); + LogInfo(" RT/RT: %s", rtrt ? "Enabled" : "Disabled"); + LogInfo(" DIU Flag: %s", diu ? "Enabled" : "Disabled"); + LogInfo(" Jitter Size: %u ms", jitter); + LogInfo(" Debug: %s", serial_debug ? "Enabled" : "Disabled"); + LogInfo(" Trace: %s", serial_trace ? "Enabled" : "Disabled"); + + // Create serial service + m_serial = new SerialService(port, baudrate, rtrt, diu, jitter, m_network, p25BufferSize, p25BufferSize, serial_debug, serial_trace); + + // Open serial + ret = m_serial->open(); + if (!ret) + return EXIT_FAILURE; + + StopWatch stopWatch; + stopWatch.start(); + + /// + /// main execution loop + /// + while (!g_killed) { + uint32_t ms = stopWatch.elapsed(); + + ms = stopWatch.elapsed(); + stopWatch.start(); + + // ------------------------------------------------------ + // -- Network RX Clocking -- + // ------------------------------------------------------ + + if (m_network != nullptr) + m_network->clock(ms); + + uint32_t length = 0U; + bool netReadRet = false; + + UInt8Array p25Buffer = m_network->readP25(netReadRet, length); + if (netReadRet) { + uint8_t duid = p25Buffer[22U]; + uint8_t MFId = p25Buffer[15U]; + + uint8_t lco = p25Buffer[4U]; + + uint32_t srcId = __GET_UINT16(p25Buffer, 5U); + uint32_t dstId = __GET_UINT16(p25Buffer, 8U); + + if (!g_hideMessages) + LogMessage(LOG_NET, "P25, duid = $%02X, lco = $%02X, MFId = $%02X, srcId = %u, dstId = %u, len = %u", duid, lco, MFId, srcId, dstId, length); + + // Send the data to the serial handler + m_serial->processP25FromNet(std::move(p25Buffer), length); + } + + // We keep DMR & NXDN in so nothing breaks, even though DFSI doesn't do DMR or NXDNS + UInt8Array dmrBuffer = m_network->readDMR(netReadRet, length); + if (netReadRet) { + uint8_t seqNo = dmrBuffer[4U]; + + uint32_t srcId = __GET_UINT16(dmrBuffer, 5U); + uint32_t dstId = __GET_UINT16(dmrBuffer, 8U); + + uint8_t flco = (dmrBuffer[15U] & 0x40U) == 0x40U ? dmr::FLCO_PRIVATE : dmr::FLCO_GROUP; + + uint32_t slotNo = (dmrBuffer[15U] & 0x80U) == 0x80U ? 2U : 1U; + + if (!g_hideMessages) + LogMessage(LOG_NET, "DMR, slotNo = %u, seqNo = %u, flco = $%02X, srcId = %u, dstId = %u, len = %u", slotNo, seqNo, flco, srcId, dstId, length); + } + + UInt8Array nxdnBuffer = m_network->readNXDN(netReadRet, length); + if (netReadRet) { + uint8_t messageType = nxdnBuffer[4U]; + + uint32_t srcId = __GET_UINT16(nxdnBuffer, 5U); + uint32_t dstId = __GET_UINT16(nxdnBuffer, 8U); + + if (!g_hideMessages) + LogMessage(LOG_NET, "NXDN, messageType = $%02X, srcId = %u, dstId = %u, len = %u", messageType, srcId, dstId, length); + } + + // ------------------------------------------------------ + // -- Network TX Clocking -- + // ------------------------------------------------------ + + // Processes data in the serial rx P25 buffer and sends it to the network buffer for sending + if (m_serial != nullptr) { + m_serial->processP25ToNet(); + } + + // ------------------------------------------------------ + // -- Serial Clocking -- + // ------------------------------------------------------ + + if (m_serial != nullptr) { + m_serial->clock(ms); + } + + // Timekeeping + if (ms < 2U) + Thread::sleep(1U); + } + + ::LogSetNetwork(nullptr); + if (m_network != nullptr) { + m_network->close(); + delete m_network; + } + + if (m_serial != nullptr) { + m_serial->close(); + delete m_serial; + } + + if (m_tidLookup != nullptr) { + m_tidLookup->stop(); + delete m_tidLookup; + } + if (m_ridLookup != nullptr) { + m_ridLookup->stop(); + delete m_ridLookup; + } + + return EXIT_SUCCESS; +} + +// --------------------------------------------------------------------------- +// Private Class Members +// --------------------------------------------------------------------------- + +/// +/// Reads basic configuration parameters from the YAML configuration file. +/// +/// +bool Dfsi::readParams() +{ + // No basic config params right now + + return true; +} + +/// +/// Initializes peer network connectivity. +/// +/// +bool Dfsi::createPeerNetwork() +{ + yaml::Node networkConf = m_conf["network"]; + std::string password = networkConf["password"].as(); + + std::string address = networkConf["address"].as(); + uint16_t port = networkConf["port"].as(); + uint32_t id = networkConf["peerId"].as(); + + bool encrypted = networkConf["encrypted"].as(false); + std::string key = networkConf["presharedKey"].as(); + uint8_t presharedKey[AES_WRAPPED_PCKT_KEY_LEN]; + if (!key.empty()) { + if (key.size() == 32) { + // bryanb: shhhhhhh....dirty nasty hacks + key = key.append(key); // since the key is 32 characters (16 hex pairs), double it on itself for 64 characters (32 hex pairs) + LogWarning(LOG_HOST, "Half-length network preshared encryption key detected, doubling key on itself."); + } + + if (key.size() == 64) { + if ((key.find_first_not_of("0123456789abcdefABCDEF", 2) == std::string::npos)) { + const char* keyPtr = key.c_str(); + ::memset(presharedKey, 0x00U, AES_WRAPPED_PCKT_KEY_LEN); + + for (uint8_t i = 0; i < AES_WRAPPED_PCKT_KEY_LEN; i++) { + char t[4] = {keyPtr[0], keyPtr[1], 0}; + presharedKey[i] = (uint8_t)::strtoul(t, NULL, 16); + keyPtr += 2 * sizeof(char); + } + } + else { + LogWarning(LOG_HOST, "Invalid characters in the network preshared encryption key. Encryption disabled."); + encrypted = false; + } + } + else { + LogWarning(LOG_HOST, "Invalid network preshared encryption key length, key should be 32 hex pairs, or 64 characters. Encryption disabled."); + encrypted = false; + } + } + + std::string identity = networkConf["identity"].as(); + + bool netDebug = networkConf["debug"].as(); + + LogInfo("Network Parameters"); + LogInfo(" Identity: %s", identity.c_str()); + LogInfo(" Peer ID: %u", id); + LogInfo(" Address: %s", address.c_str()); + LogInfo(" Port: %u", port); + LogInfo(" Encrypted: %s", encrypted ? "yes" : "no"); + + if (id > 999999999U) { + ::LogError(LOG_HOST, "Network Peer ID cannot be greater then 999999999."); + return false; + } + + // initialize networking + m_network = new DfsiPeerNetwork(address, port, 0U, id, password, true, netDebug, false, true, false, true, true, true, true, true, false); + m_network->setMetadata(identity, 0U, 0U, 0.0F, 0.0F, 0, 0, 0, 0.0F, 0.0F, 0, ""); + m_network->setLookups(m_ridLookup, m_tidLookup); + + ::LogSetNetwork(m_network); + + if (encrypted) { + m_network->setPresharedKey(presharedKey); + } + + m_network->enable(true); + bool ret = m_network->open(); + if (!ret) { + delete m_network; + m_network = nullptr; + LogError(LOG_HOST, "failed to initialize traffic networking for PEER %u", id); + return false; + } + + ::LogSetNetwork(m_network); + + return true; +} \ No newline at end of file diff --git a/src/dfsi/Dfsi.h b/src/dfsi/Dfsi.h new file mode 100644 index 00000000..f2f16046 --- /dev/null +++ b/src/dfsi/Dfsi.h @@ -0,0 +1,70 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#if !defined(__DFSI_H__) +#define __DFSI_H__ + +#include "Defines.h" +#include "common/lookups/RadioIdLookup.h" +#include "common/lookups/TalkgroupRulesLookup.h" +#include "common/yaml/Yaml.h" +#include "common/Timer.h" +#include "network/DfsiPeerNetwork.h" +#include "network/SerialService.h" + +#include +#include +#include + +// --------------------------------------------------------------------------- +// Class Declaration +// This class implements the core service logic. +// --------------------------------------------------------------------------- + +class HOST_SW_API Dfsi { +public: + /// Initializes a new instance of the HostTest class. + Dfsi(const std::string& confFile); + /// Finalizes a instance of the HostTest class. + ~Dfsi(); + + /// Executes the main host processing loop. + int run(); + +private: + const std::string& m_confFile; + yaml::Node m_conf; + + network::DfsiPeerNetwork* m_network; + + lookups::RadioIdLookup* m_ridLookup; + lookups::TalkgroupRulesLookup* m_tidLookup; + + uint32_t m_pingTime; + uint32_t m_maxMissedPings; + + uint32_t m_updateLookupTime; + + bool m_debug; + + bool m_repeatTraffic; + + network::SerialService* m_serial; + + /// Reads basic configuration parameters from the INI. + bool readParams(); + /// Initializes peer network connectivity. + bool createPeerNetwork(); +}; + +#endif // __DFSI_H__ \ No newline at end of file diff --git a/src/dfsi/DfsiMain.cpp b/src/dfsi/DfsiMain.cpp new file mode 100644 index 00000000..5abf11c8 --- /dev/null +++ b/src/dfsi/DfsiMain.cpp @@ -0,0 +1,247 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#include "Defines.h" +#include "common/Log.h" +#include "dfsi/ActivityLog.h" +#include "DfsiMain.h" +#include "Dfsi.h" + +using namespace network; +using namespace lookups; + +#include +#include +#include + +#include + +// --------------------------------------------------------------------------- +// Macros +// --------------------------------------------------------------------------- + +#define IS(s) (::strcmp(argv[i], s) == 0) + +// --------------------------------------------------------------------------- +// Global Variables +// --------------------------------------------------------------------------- + +int g_signal = 0; +std::string g_progExe = std::string(__EXE_NAME__); +std::string g_iniFile = std::string(DEFAULT_CONF_FILE); +std::string g_lockFile = std::string(DEFAULT_LOCK_FILE); + +std::string g_masterAddress = std::string("127.0.0.1"); +uint16_t g_masterPort = 63031; +uint32_t g_peerId = 9000999; + +bool g_foreground = false; +bool g_killed = false; +bool g_hideMessages = false; + +uint8_t* g_gitHashBytes = nullptr; + +// --------------------------------------------------------------------------- +// Global Functions +// --------------------------------------------------------------------------- + +#if !defined(CATCH2_TEST_COMPILATION) +/// +/// Internal signal handler. +/// +/// +static void sigHandler(int signum) +{ + g_signal = signum; + g_killed = true; +} +#endif + +/// +/// Helper to print a fatal error message and exit. +/// +/// This is a variable argument function. +/// Message. +void fatal(const char* msg, ...) +{ + char buffer[400U]; + ::memset(buffer, 0x20U, 400U); + + va_list vl; + va_start(vl, msg); + + ::vsprintf(buffer, msg, vl); + + va_end(vl); + + ::fprintf(stderr, "%s: %s\n", g_progExe.c_str(), buffer); + exit(EXIT_FAILURE); +} + +/// +/// Helper to pring usage the command line arguments. (And optionally an error.) +/// +/// Error message. +/// Error message arguments. +void usage(const char* message, const char* arg) +{ + ::fprintf(stdout, __PROG_NAME__ " %s (built %s)\r\n", __VER__, __BUILD__); + ::fprintf(stdout, "Copyright (c) 2024 DVMProject (https://github.com/dvmproject) Authors.\n"); + if (message != nullptr) { + ::fprintf(stderr, "%s: ", g_progExe.c_str()); + ::fprintf(stderr, message, arg); + ::fprintf(stderr, "\n\n"); + } + + ::fprintf(stdout, + "usage: %s [-vhf]" + "[--syslog]" + "[-c ]" + "[-a
] [-p ] [-P ]" + "\n\n" + " -v show version information\n" + " -h show this screen\n" + " -f foreground mode\n" + "\n" + " -c specifies the configuration file to use\n" + "\n" + " -- stop handling options\n", + g_progExe.c_str()); + exit(EXIT_FAILURE); +} + +/// +/// Helper to validate the command line arguments. +/// +/// Argument count. +/// Array of argument strings. +/// Count of remaining unprocessed arguments. +int checkArgs(int argc, char* argv[]) +{ + int i, p = 0; + + // iterate through arguments + for (i = 1; i <= argc; i++) + { + if (argv[i] == nullptr) { + break; + } + + if (*argv[i] != '-') { + continue; + } + else if (IS("--")) { + ++p; + break; + } + else if (IS("-f")) { + g_foreground = true; + } + else if (IS("--syslog")) { + g_useSyslog = true; + } + else if (IS("-s")) { + g_hideMessages = true; + } + else if (IS("-c")) { + if (argc-- <= 0) + usage("error: %s", "must specify the configuration file to use"); + g_iniFile = std::string(argv[++i]); + + if (g_iniFile.empty()) + usage("error: %s", "configuration file cannot be blank!"); + + p += 2; + } + else if (IS("-v")) { + ::fprintf(stdout, __PROG_NAME__ " %s (built %s)\r\n", __VER__, __BUILD__); + ::fprintf(stdout, "Copyright (c) 2017-2024 Patrick McDonnell, W3AXL and DVMProject (https://github.com/dvmproject) Authors.\n"); + ::fprintf(stdout, "Portions Copyright (c) 2015-2021 by Jonathan Naylor, G4KLX and others\n\n"); + if (argc == 2) + exit(EXIT_SUCCESS); + } + else if (IS("-h")) { + usage(nullptr, nullptr); + if (argc == 2) + exit(EXIT_SUCCESS); + } + else { + usage("unrecognized option `%s'", argv[i]); + } + } + + if (p < 0 || p > argc) { + p = 0; + } + + return ++p; +} + +// --------------------------------------------------------------------------- +// Program Entry Point +// --------------------------------------------------------------------------- +#if !defined(CATCH2_TEST_COMPILATION) +int main(int argc, char** argv) +{ + g_gitHashBytes = new uint8_t[4U]; + ::memset(g_gitHashBytes, 0x00U, 4U); + + uint32_t hash = ::strtoul(__GIT_VER_HASH__, 0, 16); + __SET_UINT32(hash, g_gitHashBytes, 0U); + + if (argv[0] != nullptr && *argv[0] != 0) + g_progExe = std::string(argv[0]); + + if (argc > 1) { + // check arguments + int i = checkArgs(argc, argv); + if (i < argc) { + argc -= i; + argv += i; + } + else { + argc--; + argv++; + } + } + + ::signal(SIGINT, sigHandler); + ::signal(SIGTERM, sigHandler); + ::signal(SIGHUP, sigHandler); + + int ret = 0; + + do { + g_signal = 0; + g_killed = false; + + Dfsi *dfsi = new Dfsi(g_iniFile); + ret = dfsi->run(); + delete dfsi; + + if (g_signal == 2) + ::LogInfoEx(LOG_HOST, "Exited on receipt of SIGINT"); + + if (g_signal == 15) + ::LogInfoEx(LOG_HOST, "Exited on receipt of SIGTERM"); + + if (g_signal == 1) + ::LogInfoEx(LOG_HOST, "Restarting on receipt of SIGHUP"); + } while (g_signal == 1); + + ::LogFinalise(); + ::ActivityLogFinalise(); + + return ret; +} +#endif \ No newline at end of file diff --git a/src/dfsi/DfsiMain.h b/src/dfsi/DfsiMain.h new file mode 100644 index 00000000..60e2a8b1 --- /dev/null +++ b/src/dfsi/DfsiMain.h @@ -0,0 +1,42 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#if !defined(__DFSI_MAIN_H__) +#define __DFSI_MAIN_H__ + +#include "Defines.h" + +#include + +// --------------------------------------------------------------------------- +// Externs +// --------------------------------------------------------------------------- + +extern int g_signal; +extern std::string g_progExe; +extern std::string g_iniFile; +extern std::string g_lockFile; + +extern bool g_foreground; +extern bool g_killed; +extern bool g_hideMessages; + +extern std::string g_masterAddress; +extern uint16_t g_masterPort; +extern uint32_t g_peerId; + +extern uint8_t* g_gitHashBytes; + +extern HOST_SW_API void fatal(const char* msg, ...); + +#endif // __DFSI_MAIN_H__ \ No newline at end of file diff --git a/src/dfsi/network/CallData.cpp b/src/dfsi/network/CallData.cpp new file mode 100644 index 00000000..186fe1d5 --- /dev/null +++ b/src/dfsi/network/CallData.cpp @@ -0,0 +1,89 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +* Borrowed from work by Bryan Biedenkapp, N2PLL +* +*/ + +#include "CallData.h" + +using namespace network; +using namespace modem; +using namespace p25; +using namespace dfsi; + +VoiceCallData::VoiceCallData() : + srcId(0U), + dstId(0U), + lco(0U), + mfId(P25_MFG_STANDARD), + serviceOptions(0U), + lsd1(0U), + lsd2(0U), + mi(), + algoId(P25_ALGO_UNENCRYPT), + kId(0U), + VHDR1(), + VHDR2(), + netLDU1(), + netLDU2(), + seqNo(0U), + n(0U), + streamId(0U) +{ + mi = new uint8_t[P25_MI_LENGTH_BYTES]; + VHDR1 = new uint8_t[MotVoiceHeader1::HCW_LENGTH]; + VHDR2 = new uint8_t[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); +} + +VoiceCallData::~VoiceCallData() { + delete[] mi; + delete[] VHDR1; + delete[] VHDR2; + delete[] netLDU1; + delete[] netLDU2; +} + +void VoiceCallData::resetCallData() { + srcId = 0U; + dstId = 0U; + lco = 0U; + mfId = P25_MFG_STANDARD; + serviceOptions = 0U; + lsd1 = 0U; + lsd2 = 0U; + + ::memset(mi, 0x00U, P25_MI_LENGTH_BYTES); + + algoId = P25_ALGO_UNENCRYPT; + kId = 0U; + + ::memset(VHDR1, 0x00U, MotVoiceHeader1::HCW_LENGTH); + ::memset(VHDR2, 0x00U, MotVoiceHeader2::HCW_LENGTH); + + ::memset(netLDU1, 0x00U, 9U * 25U); + ::memset(netLDU2, 0x00U, 9U * 25U); + + n = 0U; + seqNo = 0U; + streamId = 0U; +} + +void VoiceCallData::newStreamId() { + std::uniform_int_distribution dist(DVM_RAND_MIN, DVM_RAND_MAX); + streamId = dist(random); +} \ No newline at end of file diff --git a/src/dfsi/network/CallData.h b/src/dfsi/network/CallData.h new file mode 100644 index 00000000..1eeb1e3e --- /dev/null +++ b/src/dfsi/network/CallData.h @@ -0,0 +1,85 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +* Borrowed from work by Bryan Biedenkapp, N2PLL +* +*/ +#if !defined(__DFSI_CALL_DATA_H__) +#define __DFSI_CALL_DATA_H__ + +// DVM Includes +#include "Defines.h" +#include "common/edac/RS634717.h" +#include "common/network/RawFrameQueue.h" +#include "common/p25/data/LowSpeedData.h" +#include "common/p25/P25Defines.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/p25/dfsi/LC.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "common/yaml/Yaml.h" +#include "common/RingBuffer.h" +#include "network/DfsiPeerNetwork.h" +#include "rtp/RtpFrames.h" +#include "host/modem/Modem.h" +#include "host/modem/port/IModemPort.h" +#include "host/modem/port/UARTPort.h" + +// CPP includes +#include + +namespace network { + + class HOST_SW_API VoiceCallData { + + public: + VoiceCallData(); + ~VoiceCallData(); + + void resetCallData(); + + void newStreamId(); + + // Call Data + uint32_t srcId; + uint32_t dstId; + + uint8_t lco; + uint8_t mfId; + uint8_t serviceOptions; + + uint8_t lsd1; + uint8_t lsd2; + + uint8_t* mi; + uint8_t algoId; + uint32_t kId; + + uint8_t* VHDR1; + uint8_t* VHDR2; + + uint8_t* netLDU1; + uint8_t* netLDU2; + + uint32_t seqNo; + uint8_t n; + + uint32_t streamId; + + private: + // Used for stream ID generation + std::mt19937 random; + + }; +} + +#endif // __DFSI_CALL_DATA_H__ \ No newline at end of file diff --git a/src/dfsi/network/DfsiPeerNetwork.cpp b/src/dfsi/network/DfsiPeerNetwork.cpp new file mode 100644 index 00000000..ff445d81 --- /dev/null +++ b/src/dfsi/network/DfsiPeerNetwork.cpp @@ -0,0 +1,337 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#include "dfsi/Defines.h" +#include "common/network/json/json.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/p25/dfsi/LC.h" +#include "common/Utils.h" +#include "network/DfsiPeerNetwork.h" + +using namespace network; + +#include + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/// +/// Initializes a new instance of the PeerNetwork class. +/// +/// Network Hostname/IP address to connect to. +/// Network port number. +/// +/// Unique ID on the network. +/// Network authentication password. +/// Flag indicating full-duplex operation. +/// Flag indicating whether network debug is enabled. +/// Flag indicating whether DMR is enabled. +/// Flag indicating whether P25 is enabled. +/// Flag indicating whether NXDN is enabled. +/// Flag indicating whether DMR slot 1 is enabled for network traffic. +/// Flag indicating whether DMR slot 2 is enabled for network traffic. +/// Flag indicating that the system activity logs will be sent to the network. +/// Flag indicating that the system diagnostic logs will be sent to the network. +/// Flag indicating that the system will accept radio ID and talkgroup ID lookups from the network. +DfsiPeerNetwork::DfsiPeerNetwork(const std::string& address, uint16_t port, uint16_t localPort, uint32_t peerId, const std::string& password, + bool duplex, bool debug, bool dmr, bool p25, bool nxdn, bool slot1, bool slot2, bool allowActivityTransfer, bool allowDiagnosticTransfer, bool updateLookup, bool saveLookup) : + Network(address, port, localPort, peerId, password, duplex, debug, dmr, p25, nxdn, slot1, slot2, allowActivityTransfer, allowDiagnosticTransfer, updateLookup, saveLookup) +{ + assert(!address.empty()); + assert(port > 0U); + assert(!password.empty()); +} + +/// +/// Writes P25 LDU1 frame data to the network. +/// +/// +/// +/// +/// +/// +bool DfsiPeerNetwork::writeP25LDU1(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data, uint8_t frameType) +{ + if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING) + return false; + + bool resetSeq = false; + if (m_p25StreamId == 0U) { + resetSeq = true; + m_p25StreamId = createStreamId(); + } + + uint32_t messageLength = 0U; + UInt8Array message = createP25_LDU1Message_Raw(messageLength, control, lsd, data, frameType); + if (message == nullptr) { + return false; + } + + return writeMaster({ NET_FUNC_PROTOCOL, NET_PROTOCOL_SUBFUNC_P25 }, message.get(), messageLength, pktSeq(resetSeq), m_p25StreamId); +} + +/// +/// Writes P25 LDU2 frame data to the network. +/// +/// +/// +/// +/// +bool DfsiPeerNetwork::writeP25LDU2(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data) +{ + if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING) + return false; + + bool resetSeq = false; + if (m_p25StreamId == 0U) { + resetSeq = true; + m_p25StreamId = createStreamId(); + } + + uint32_t messageLength = 0U; + UInt8Array message = createP25_LDU2Message_Raw(messageLength, control, lsd, data); + if (message == nullptr) { + return false; + } + + return writeMaster({ NET_FUNC_PROTOCOL, NET_PROTOCOL_SUBFUNC_P25 }, message.get(), messageLength, pktSeq(resetSeq), m_p25StreamId); +} + +// --------------------------------------------------------------------------- +// Protected Class Members +// --------------------------------------------------------------------------- + +/// +/// Writes configuration to the network. +/// +/// +bool DfsiPeerNetwork::writeConfig() +{ + if (m_loginStreamId == 0U) { + LogWarning(LOG_NET, "BUGBUG: tried to write network authorisation with no stream ID?"); + return false; + } + + const char* software = __NETVER__; + + json::object config = json::object(); + + // identity and frequency + config["identity"].set(m_identity); // Identity + config["rxFrequency"].set(m_rxFrequency); // Rx Frequency + config["txFrequency"].set(m_txFrequency); // Tx Frequency + + // system info + json::object sysInfo = json::object(); + sysInfo["latitude"].set(m_latitude); // Latitude + sysInfo["longitude"].set(m_longitude); // Longitude + + sysInfo["height"].set(m_height); // Height + sysInfo["location"].set(m_location); // Location + config["info"].set(sysInfo); + + // channel data + json::object channel = json::object(); + channel["txPower"].set(m_power); // Tx Power + channel["txOffsetMhz"].set(m_txOffsetMhz); // Tx Offset (Mhz) + channel["chBandwidthKhz"].set(m_chBandwidthKhz); // Ch. Bandwidth (khz) + channel["channelId"].set(m_channelId); // Channel ID + channel["channelNo"].set(m_channelNo); // Channel No + config["channel"].set(channel); + + // RCON + json::object rcon = json::object(); + rcon["password"].set(m_restApiPassword); // REST API Password + rcon["port"].set(m_restApiPort); // REST API Port + config["rcon"].set(rcon); + + config["software"].set(std::string(software)); // Software ID + + json::value v = json::value(config); + std::string json = v.serialize(); + + char buffer[json.length() + 8U]; + + ::memcpy(buffer + 0U, TAG_REPEATER_CONFIG, 4U); + ::sprintf(buffer + 8U, "%s", json.c_str()); + + if (m_debug) { + Utils::dump(1U, "Network Message, Configuration", (uint8_t*)buffer, json.length() + 8U); + } + + return writeMaster({ NET_FUNC_RPTC, NET_SUBFUNC_NOP }, (uint8_t*)buffer, json.length() + 8U, pktSeq(), m_loginStreamId); +} + +// --------------------------------------------------------------------------- +// Private Class Members +// --------------------------------------------------------------------------- + +/// +/// Creates an P25 LDU1 frame message. +/// +/// +/// +/// +/// +/// +/// +UInt8Array DfsiPeerNetwork::createP25_LDU1Message_Raw(uint32_t& length, const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, + const uint8_t* data, uint8_t frameType) +{ + assert(data != nullptr); + + p25::dfsi::LC dfsiLC = p25::dfsi::LC(control, lsd); + + uint8_t* buffer = new uint8_t[P25_LDU1_PACKET_LENGTH + PACKET_PAD]; + ::memset(buffer, 0x00U, P25_LDU1_PACKET_LENGTH + PACKET_PAD); + + // construct P25 message header + createP25_MessageHdr(buffer, p25::P25_DUID_LDU1, control, lsd, frameType); + + // pack DFSI data + uint32_t count = MSG_HDR_SIZE; + uint8_t imbe[p25::P25_RAW_IMBE_LENGTH_BYTES]; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU1_VOICE1); + ::memcpy(imbe, data + 10U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU1(buffer + 24U, imbe); + count += p25::dfsi::P25_DFSI_LDU1_VOICE1_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU1_VOICE2); + ::memcpy(imbe, data + 26U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU1(buffer + 46U, imbe); + count += p25::dfsi::P25_DFSI_LDU1_VOICE2_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU1_VOICE3); + ::memcpy(imbe, data + 55U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU1(buffer + 60U, imbe); + count += p25::dfsi::P25_DFSI_LDU1_VOICE3_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU1_VOICE4); + ::memcpy(imbe, data + 80U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU1(buffer + 77U, imbe); + count += p25::dfsi::P25_DFSI_LDU1_VOICE4_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU1_VOICE5); + ::memcpy(imbe, data + 105U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU1(buffer + 94U, imbe); + count += p25::dfsi::P25_DFSI_LDU1_VOICE5_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU1_VOICE6); + ::memcpy(imbe, data + 130U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU1(buffer + 111U, imbe); + count += p25::dfsi::P25_DFSI_LDU1_VOICE6_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU1_VOICE7); + ::memcpy(imbe, data + 155U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU1(buffer + 128U, imbe); + count += p25::dfsi::P25_DFSI_LDU1_VOICE7_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU1_VOICE8); + ::memcpy(imbe, data + 180U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU1(buffer + 145U, imbe); + count += p25::dfsi::P25_DFSI_LDU1_VOICE8_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU1_VOICE9); + ::memcpy(imbe, data + 204U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU1(buffer + 162U, imbe); + count += p25::dfsi::P25_DFSI_LDU1_VOICE9_FRAME_LENGTH_BYTES; + + buffer[23U] = count; + + if (m_debug) + Utils::dump(1U, "Network Message, P25 LDU1", buffer, (P25_LDU1_PACKET_LENGTH + PACKET_PAD)); + + length = (P25_LDU1_PACKET_LENGTH + PACKET_PAD); + return UInt8Array(buffer); +} + +/// +/// Creates an P25 LDU2 frame message. +/// +/// +/// +/// +/// +/// +UInt8Array DfsiPeerNetwork::createP25_LDU2Message_Raw(uint32_t& length, const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, + const uint8_t* data) +{ + assert(data != nullptr); + + p25::dfsi::LC dfsiLC = p25::dfsi::LC(control, lsd); + + uint8_t* buffer = new uint8_t[P25_LDU2_PACKET_LENGTH + PACKET_PAD]; + ::memset(buffer, 0x00U, P25_LDU2_PACKET_LENGTH + PACKET_PAD); + + // construct P25 message header + createP25_MessageHdr(buffer, p25::P25_DUID_LDU2, control, lsd, p25::P25_FT_DATA_UNIT); + + // pack DFSI data + uint32_t count = MSG_HDR_SIZE; + uint8_t imbe[p25::P25_RAW_IMBE_LENGTH_BYTES]; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU2_VOICE10); + ::memcpy(imbe, data + 10U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU2(buffer + 24U, imbe); + count += p25::dfsi::P25_DFSI_LDU2_VOICE10_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU2_VOICE11); + ::memcpy(imbe, data + 26U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU2(buffer + 46U, imbe); + count += p25::dfsi::P25_DFSI_LDU2_VOICE11_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU2_VOICE12); + ::memcpy(imbe, data + 55U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU2(buffer + 60U, imbe); + count += p25::dfsi::P25_DFSI_LDU2_VOICE12_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU2_VOICE13); + ::memcpy(imbe, data + 80U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU2(buffer + 77U, imbe); + count += p25::dfsi::P25_DFSI_LDU2_VOICE13_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU2_VOICE14); + ::memcpy(imbe, data + 105U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU2(buffer + 94U, imbe); + count += p25::dfsi::P25_DFSI_LDU2_VOICE14_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU2_VOICE15); + ::memcpy(imbe, data + 130U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU2(buffer + 111U, imbe); + count += p25::dfsi::P25_DFSI_LDU2_VOICE15_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU2_VOICE16); + ::memcpy(imbe, data + 155U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU2(buffer + 128U, imbe); + count += p25::dfsi::P25_DFSI_LDU2_VOICE16_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU2_VOICE17); + ::memcpy(imbe, data + 180U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU2(buffer + 145U, imbe); + count += p25::dfsi::P25_DFSI_LDU2_VOICE17_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(p25::dfsi::P25_DFSI_LDU2_VOICE18); + ::memcpy(imbe, data + 204U, p25::P25_RAW_IMBE_LENGTH_BYTES); + dfsiLC.encodeLDU2(buffer + 162U, imbe); + count += p25::dfsi::P25_DFSI_LDU2_VOICE18_FRAME_LENGTH_BYTES; + + buffer[23U] = count; + + if (m_debug) + Utils::dump(1U, "Network Message, P25 LDU2", buffer, (P25_LDU2_PACKET_LENGTH + PACKET_PAD)); + + length = (P25_LDU2_PACKET_LENGTH + PACKET_PAD); + return UInt8Array(buffer); +} \ No newline at end of file diff --git a/src/dfsi/network/DfsiPeerNetwork.h b/src/dfsi/network/DfsiPeerNetwork.h new file mode 100644 index 00000000..68456aad --- /dev/null +++ b/src/dfsi/network/DfsiPeerNetwork.h @@ -0,0 +1,56 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#if !defined(__DFSI_PEER_NETWORK_H__) +#define __DFSI_PEER_NETWORK_H__ + +#include "Defines.h" +#include "host/network/Network.h" + +#include +#include + +namespace network +{ + // --------------------------------------------------------------------------- + // Class Declaration + // Implements the core peer networking logic. + // --------------------------------------------------------------------------- + + class HOST_SW_API DfsiPeerNetwork : public Network { + public: + /// Initializes a new instance of the PeerNetwork class. + DfsiPeerNetwork(const std::string& address, uint16_t port, uint16_t localPort, uint32_t peerId, const std::string& password, + bool duplex, bool debug, bool dmr, bool p25, bool nxdn, bool slot1, bool slot2, bool allowActivityTransfer, bool allowDiagnosticTransfer, bool updateLookup, bool saveLookup); + + /// Writes P25 LDU1 frame data to the network. + bool writeP25LDU1(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data, + uint8_t frameType) override; + /// Writes P25 LDU2 frame data to the network. + bool writeP25LDU2(const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, const uint8_t* data) override; + + protected: + /// Writes configuration to the network. + bool writeConfig() override; + + private: + /// Creates an P25 LDU1 frame message. + UInt8Array createP25_LDU1Message_Raw(uint32_t& length, const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, + const uint8_t* data, uint8_t frameType); + /// Creates an P25 LDU2 frame message. + UInt8Array createP25_LDU2Message_Raw(uint32_t& length, const p25::lc::LC& control, const p25::data::LowSpeedData& lsd, + const uint8_t* data); + }; +} // namespace network + +#endif // __DFSI_PEER_NETWORK_H__ \ No newline at end of file diff --git a/src/dfsi/network/SerialService.cpp b/src/dfsi/network/SerialService.cpp new file mode 100644 index 00000000..a79ba2ff --- /dev/null +++ b/src/dfsi/network/SerialService.cpp @@ -0,0 +1,1844 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ + +#include "common/p25/lc/tdulc/TDULCFactory.h" + +#include "network/SerialService.h" + +#include "dfsi/ActivityLog.h" + +using namespace network; +using namespace modem; +using namespace p25; +using namespace dfsi; + +SerialService::SerialService(const std::string& portName, uint32_t baudrate, bool rtrt, bool diu, uint16_t jitter, DfsiPeerNetwork* network, uint32_t p25TxQueueSize, uint32_t p25RxQueueSize, bool debug, bool trace) : + m_portName(portName), + m_baudrate(baudrate), + m_rtrt(rtrt), + m_diu(diu), + m_port(), + m_jitter(jitter), + m_debug(debug), + m_trace(trace), + m_network(network), + m_lastIMBE(nullptr), + m_lastHeard(), + m_sequences(), + m_msgBuffer(nullptr), + m_msgState(RESP_START), + m_msgLength(0U), + m_msgOffset(0U), + m_msgType(CMD_GET_STATUS), + m_msgDoubleLength(false), + m_netFrames(0U), + m_netLost(0U), + m_rxP25Queue(p25RxQueueSize, "RX P25 Queue"), + m_txP25Queue(p25TxQueueSize, "TX P25 Queue"), + m_lastP25Tx(0U), + m_rs(), + m_rxP25LDUCounter(0U), + m_netCallInProgress(false), + m_lclCallInProgress(false), + m_rxVoiceControl(nullptr), + m_rxVoiceLsd(nullptr), + m_rxVoiceCallData(nullptr) +{ + assert(!portName.empty()); + assert(baudrate > 0U); + + // Setup serial + port::SERIAL_SPEED serialSpeed = port::SERIAL_115200; + + m_port = new port::UARTPort(portName, serialSpeed, false); + + m_lastIMBE = new uint8_t[11U]; + ::memcpy(m_lastIMBE, P25_NULL_IMBE, 11U); + + m_msgBuffer = new uint8_t[BUFFER_LENGTH]; +} + +SerialService::~SerialService() +{ + if (m_port != nullptr) { + delete m_port; + } + + // Delete our buffers + delete[] m_lastIMBE; + delete[] m_msgBuffer; + + // Delete our rx P25 objects + delete m_rxVoiceControl; + delete m_rxVoiceLsd; + delete m_rxVoiceCallData; +} + +void SerialService::clock(uint32_t ms) +{ + // Get data from serial port + RESP_TYPE_DVM type = readSerial(); + + // Handle what we got + if (type == RTM_TIMEOUT) { + // Do nothing + } + else if (type == RTM_ERROR) { + // Also do nothing + } + else { + // Get cmd byte offset + uint8_t cmdOffset = 2U; + if (m_msgDoubleLength) + cmdOffset = 3U; + + // Get command type + switch (m_msgBuffer[2U]) { + + // P25 data is handled identically to the dvmhost modem + case CMD_P25_DATA: + { + // Get length + uint8_t length[2U]; + if (m_msgLength > 255U) + length[0U] = ((m_msgLength - cmdOffset) >> 8U) & 0xFFU; + else + length[0U] = 0x00U; + length[1U] = (m_msgLength - cmdOffset) & 0xFFU; + m_rxP25Queue.addData(length, 2U); + + // Add data tag to queue + uint8_t data = TAG_DATA; + m_rxP25Queue.addData(&data, 1U); + + // Add P25 data to buffer + m_rxP25Queue.addData(m_msgBuffer + (cmdOffset + 1U), m_msgLength - (cmdOffset + 1U)); + + if (m_debug) { + LogDebug(LOG_SERIAL, "Got P25 data from V24 board (len: %u)", m_msgLength); + } + } + break; + + // P25 data lost is also handled, though the V24 board doesn't implement it (yet?) + case CMD_P25_LOST: + { + if (m_debug) + LogDebug(LOG_SERIAL, "Got P25 lost msg from V24 board"); + + if (m_msgDoubleLength) { + LogError(LOG_SERIAL, "CMD_P25_LOST got double length byte?"); + break; + } + + uint8_t data = 1U; + m_rxP25Queue.addData(&data, 1U); + + data = TAG_LOST; + m_rxP25Queue.addData(&data, 1U); + } + break; + + // Handle debug messages + case CMD_DEBUG1: + case CMD_DEBUG2: + case CMD_DEBUG3: + case CMD_DEBUG4: + case CMD_DEBUG5: + case CMD_DEBUG_DUMP: + printDebug(m_msgBuffer, m_msgLength); + break; + + // Fallback if we get a message we have no clue how to handle + default: + { + LogError(LOG_SERIAL, "Unhandled command from V24 board: %02X", m_msgBuffer[2U]); + } + } + } + + // Write anything waiting to the serial port + int out = writeSerial(); + if (m_trace && out > 0) { + LogDebug(LOG_SERIAL, "Wrote %u-byte message to the serial V24 device", out); + } else if (out < 0) { + LogError(LOG_SERIAL, "Failed to write to serial port!"); + } +} + +bool SerialService::open() +{ + LogInfoEx(LOG_SERIAL, "Opening port %s at %u baud", m_portName.c_str(), m_baudrate); + + bool ret = m_port->open(); + + if (!ret) { + LogError(LOG_SERIAL, "Failed to open port!"); + return false; + } + + m_msgState = RESP_START; + + return true; +} + +void SerialService::close() +{ + LogInfoEx(LOG_SERIAL, "Closing port"); + m_port->close(); +} + +/// +/// Process P25 data from the peer network and send to writeP25Frame() +/// +void SerialService::processP25FromNet(UInt8Array p25Buffer, uint32_t length) +{ + // If there's a local call in progress, ignore the frames + if (m_lclCallInProgress) { + LogWarning(LOG_SERIAL, "Local call in progress, ignoring frames from network"); + return; + } + + // Decode grant info + bool grantDemand = (p25Buffer[14U] & 0x80U) == 0x80U; + + // We don't use these (yet?) + //bool grantDenial = (p25Buffer[14U] & 0x40U) == 0x40U; + //bool unitToUnit = (p25Buffer[14U] & 0x01U) == 0x01U; + + // Decode network header + uint8_t duid = p25Buffer[22U]; + uint8_t mfid = p25Buffer[15U]; + + // Setup P25 data handlers + UInt8Array data; + uint8_t frameLength = p25Buffer[23U]; + + // Handle PDUs + if (duid == p25::P25_DUID_PDU) { + frameLength = length; + data = std::unique_ptr(new uint8_t[length]); + ::memset(data.get(), 0x00U, length); + ::memcpy(data.get(), p25Buffer.get(), length); + LogInfoEx(LOG_SERIAL, "Got P25 PDU, we don't handle these (yet)"); + } + // Handle everything else + else { + if (frameLength <= 24) { + data = std::unique_ptr(new uint8_t[frameLength]); + ::memset(data.get(), 0x00U, frameLength); + } + else { + data = std::unique_ptr(new uint8_t[frameLength]); + ::memset(data.get(), 0x00U, frameLength); + ::memcpy(data.get(), p25Buffer.get() + 24U, frameLength); + } + } + + // Get basic info + uint8_t lco = p25Buffer[4U]; + uint32_t srcId = __GET_UINT16(p25Buffer, 5U); + uint32_t dstId = __GET_UINT16(p25Buffer, 8U); + uint32_t sysId = (p25Buffer[11U] << 8) | (p25Buffer[12U] << 0); + uint32_t netId = __GET_UINT16(p25Buffer, 16U); + uint8_t lsd1 = p25Buffer[20U]; + uint8_t lsd2 = p25Buffer[21U]; + uint8_t frameType = p25::P25_FT_DATA_UNIT; + + // Default any 0's + if (netId == 0U) { + netId = lc::LC::getSiteData().netId(); + } + + if (sysId == 0U) { + sysId = lc::LC::getSiteData().sysId(); + } + + if (m_debug) { + LogDebug(LOG_NET, "P25, duid = $%02X, lco = $%02X, MFId = $%02X, srcId = %u, dstId = %u, len = %u", duid, lco, mfid, srcId, dstId, length); + } + + lc::LC control; + data::LowSpeedData lsd; + + // is this a LDU1, is this the first of a call? + if (duid == p25::P25_DUID_LDU1) { + frameType = p25Buffer[180U]; + + if (m_debug) { + LogDebug(LOG_NET, "P25, frameType = $%02X", frameType); + } + + if (frameType == p25::P25_FT_HDU_VALID) { + uint8_t algId = p25Buffer[181U]; + uint32_t kid = (p25Buffer[182U] << 8) | (p25Buffer[183U] << 0); + + // copy MI data + uint8_t mi[p25::P25_MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, p25::P25_MI_LENGTH_BYTES); + + for (uint8_t i = 0; i < p25::P25_MI_LENGTH_BYTES; i++) { + mi[i] = p25Buffer[184U + i]; + } + + if (m_debug) { + LogDebug(LOG_NET, "P25, HDU algId = $%02X, kId = $%04X", algId, kid); + } + /*if (m_trace) { + Utils::dump(1U, "P25 HDU Network MI", mi, p25::P25_MI_LENGTH_BYTES); + }*/ + + control.setAlgId(algId); + control.setKId(kid); + control.setMI(mi); + } + } + + control.setLCO(lco); + control.setSrcId(srcId); + control.setDstId(dstId); + control.setMFId(mfid); + + control.setNetId(netId); + control.setSysId(sysId); + + lsd.setLSD1(lsd1); + lsd.setLSD2(lsd2); + + /*if (m_trace) { + Utils::dump(2U, "!!! *P25 Network Frame", data.get(), frameLength); + }*/ + + uint8_t* message = data.get(); + + //Utils::dump(2U, "!!! *P25 Network Frame", message, frameLength); + + // forward onto the specific processor for final processing and delivery + switch (duid) { + case P25_DUID_LDU1: + { + if ((message[0U] == dfsi::P25_DFSI_LDU1_VOICE1) && (message[22U] == dfsi::P25_DFSI_LDU1_VOICE2) && + (message[36U] == dfsi::P25_DFSI_LDU1_VOICE3) && (message[53U] == dfsi::P25_DFSI_LDU1_VOICE4) && + (message[70U] == dfsi::P25_DFSI_LDU1_VOICE5) && (message[87U] == dfsi::P25_DFSI_LDU1_VOICE6) && + (message[104U] == dfsi::P25_DFSI_LDU1_VOICE7) && (message[121U] == dfsi::P25_DFSI_LDU1_VOICE8) && + (message[138U] == dfsi::P25_DFSI_LDU1_VOICE9)) { + uint32_t count = 0U; + dfsi::LC dfsiLC = dfsi::LC(control, lsd); + + uint8_t netLDU1[9U * 25U]; + ::memset(netLDU1, 0x00U, 9U * 25U); + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU1_VOICE1); + dfsiLC.decodeLDU1(message + count, netLDU1 + 10U); + count += dfsi::P25_DFSI_LDU1_VOICE1_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU1_VOICE2); + dfsiLC.decodeLDU1(message + count, netLDU1 + 26U); + count += dfsi::P25_DFSI_LDU1_VOICE2_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU1_VOICE3); + dfsiLC.decodeLDU1(message + count, netLDU1 + 55U); + count += dfsi::P25_DFSI_LDU1_VOICE3_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU1_VOICE4); + dfsiLC.decodeLDU1(message + count, netLDU1 + 80U); + count += dfsi::P25_DFSI_LDU1_VOICE4_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU1_VOICE5); + dfsiLC.decodeLDU1(message + count, netLDU1 + 105U); + count += dfsi::P25_DFSI_LDU1_VOICE5_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU1_VOICE6); + dfsiLC.decodeLDU1(message + count, netLDU1 + 130U); + count += dfsi::P25_DFSI_LDU1_VOICE6_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU1_VOICE7); + dfsiLC.decodeLDU1(message + count, netLDU1 + 155U); + count += dfsi::P25_DFSI_LDU1_VOICE7_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU1_VOICE8); + dfsiLC.decodeLDU1(message + count, netLDU1 + 180U); + count += dfsi::P25_DFSI_LDU1_VOICE8_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU1_VOICE9); + dfsiLC.decodeLDU1(message + count, netLDU1 + 204U); + count += dfsi::P25_DFSI_LDU1_VOICE9_FRAME_LENGTH_BYTES; + + control = lc::LC(*dfsiLC.control()); + + // Override the source/destination from the FNE RTP header (handles rewrites properly) + control.setSrcId(srcId); + control.setDstId(dstId); + + LogInfoEx(LOG_NET, P25_LDU1_STR " audio, srcId = %u, dstId = %u, group = %u, emerg = %u, encrypt = %u, prio = %u", + control.getSrcId(), control.getDstId(), control.getGroup(), control.getEmergency(), control.getEncrypted(), control.getPriority()); + + //Utils::dump("P25 LDU1 from net", netLDU1, 9U * 25U); + + writeP25Frame(duid, dfsiLC, netLDU1); + } + } + break; + case P25_DUID_LDU2: + { + if ((message[0U] == dfsi::P25_DFSI_LDU2_VOICE10) && (message[22U] == dfsi::P25_DFSI_LDU2_VOICE11) && + (message[36U] == dfsi::P25_DFSI_LDU2_VOICE12) && (message[53U] == dfsi::P25_DFSI_LDU2_VOICE13) && + (message[70U] == dfsi::P25_DFSI_LDU2_VOICE14) && (message[87U] == dfsi::P25_DFSI_LDU2_VOICE15) && + (message[104U] == dfsi::P25_DFSI_LDU2_VOICE16) && (message[121U] == dfsi::P25_DFSI_LDU2_VOICE17) && + (message[138U] == dfsi::P25_DFSI_LDU2_VOICE18)) { + uint32_t count = 0U; + dfsi::LC dfsiLC = dfsi::LC(control, lsd); + + uint8_t netLDU2[9U * 25U]; + ::memset(netLDU2, 0x00U, 9U * 25U); + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU2_VOICE10); + dfsiLC.decodeLDU2(message + count, netLDU2 + 10U); + count += dfsi::P25_DFSI_LDU2_VOICE10_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU2_VOICE11); + dfsiLC.decodeLDU2(message + count, netLDU2 + 26U); + count += dfsi::P25_DFSI_LDU2_VOICE11_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU2_VOICE12); + dfsiLC.decodeLDU2(message + count, netLDU2 + 55U); + count += dfsi::P25_DFSI_LDU2_VOICE12_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU2_VOICE13); + dfsiLC.decodeLDU2(message + count, netLDU2 + 80U); + count += dfsi::P25_DFSI_LDU2_VOICE13_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU2_VOICE14); + dfsiLC.decodeLDU2(message + count, netLDU2 + 105U); + count += dfsi::P25_DFSI_LDU2_VOICE14_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU2_VOICE15); + dfsiLC.decodeLDU2(message + count, netLDU2 + 130U); + count += dfsi::P25_DFSI_LDU2_VOICE15_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU2_VOICE16); + dfsiLC.decodeLDU2(message + count, netLDU2 + 155U); + count += dfsi::P25_DFSI_LDU2_VOICE16_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU2_VOICE17); + dfsiLC.decodeLDU2(message + count, netLDU2 + 180U); + count += dfsi::P25_DFSI_LDU2_VOICE17_FRAME_LENGTH_BYTES; + + dfsiLC.setFrameType(dfsi::P25_DFSI_LDU2_VOICE18); + dfsiLC.decodeLDU2(message + count, netLDU2 + 204U); + count += dfsi::P25_DFSI_LDU2_VOICE18_FRAME_LENGTH_BYTES; + + control = lc::LC(*dfsiLC.control()); + LogInfoEx(LOG_NET, P25_LDU2_STR " audio, algo = $%02X, kid = $%04X", control.getAlgId(), control.getKId()); + + //Utils::dump("P25 LDU2 from net", netLDU2, 9U * 25U); + + writeP25Frame(duid, dfsiLC, netLDU2); + } + } + break; + + case P25_DUID_TSDU: + //processTSDU(data.get(), frameLength, control, lsd); // We don't handle TSDUs right now + break; + + case P25_DUID_TDU: + case P25_DUID_TDULC: + { + if (duid == P25_DUID_TDULC) { + std::unique_ptr tdulc = lc::tdulc::TDULCFactory::createTDULC(data.get()); + if (tdulc == nullptr) { + LogWarning(LOG_NET, P25_TDULC_STR ", undecodable TDULC"); + } + else { + if (tdulc->getLCO() != LC_CALL_TERM) + break; + } + } + + // is this an TDU with a grant demand? + if (duid == P25_DUID_TDU && grantDemand) { + break; // ignore grant demands + } + + // Log print + LogInfoEx(LOG_NET, P25_TDU_STR ", srcId = %u, dstId = %u", srcId, dstId); + + // End the call + endOfStream(); + + // Update our sequence number + m_sequences[dstId] = RTP_END_OF_CALL_SEQ; + } + break; + } +} + +/// +/// Retrieve and process a P25 frame from the rx P25 queue +/// +/// This function pieces together LDU1/LDU2 messages from individual DFSI frames received over the serial port +/// It's called multiple times before an LDU is sent, and each time adds more data pieces to the LDUs +void SerialService::processP25ToNet() +{ + + // Buffer to store the retrieved P25 frame + uint8_t data[P25_PDU_FRAME_LENGTH_BYTES * 2U]; + + // Get a P25 frame from the RX queue + uint32_t len = readP25Frame(data); + + // If we didn't read anything, return + if (len <= 0U) { + return; + } + + // If there's already a call from the network in progress, ignore any additional frames comfing from the V24 interface + if (m_netCallInProgress) { + LogWarning(LOG_SERIAL, "Remote call in progress, ignoring frames from V24"); + return; + } + + //LogDebug(LOG_SERIAL, "processP25ToNet() got data (len: %u)", len); + + /*if (m_trace) { + Utils::dump(1U, "data: ", data, len); + }*/ + + // Create a new link control object if needed + if (m_rxVoiceControl == nullptr) { + m_rxVoiceControl = new lc::LC(); + } + + // Create a new lsd object if needed + if (m_rxVoiceLsd == nullptr) { + m_rxVoiceLsd = new data::LowSpeedData(); + } + + // Create a new call data object if needed + if (m_rxVoiceCallData == nullptr) { + m_rxVoiceCallData = new VoiceCallData(); + } + + // Parse out the data + uint8_t tag = data[0U]; + + // Sanity check + if (tag != TAG_DATA) { + LogError(LOG_SERIAL, "Unexpected data tag in RX P25 frame buffer: 0x%02X", tag); + return; + } + + // Get the DFSI data (skip the 0x00 padded byte at the start) + uint8_t dfsiData[len - 2]; + ::memset(dfsiData, 0x00U, len - 2); + ::memcpy(dfsiData, data + 2U, len - 2); + + // Extract DFSI frame type + uint8_t frameType = dfsiData[0U]; + + //LogDebug(LOG_SERIAL, "Handling DFSI frameType 0x%02X", frameType); + + // Switch based on DFSI frame type + switch (frameType) { + // Start/Stop Frame + case P25_DFSI_MOT_START_STOP: + { + // Decode the frame + MotStartOfStream start = MotStartOfStream(dfsiData); + // Handle start/stop + if (start.startStop == StartStopFlag::START) { + // Flag we have a local call (i.e. from V24) in progress + m_lclCallInProgress = true; + // Reset the call data (just in case) + m_rxVoiceCallData->resetCallData(); + // Generate a new random stream ID + m_rxVoiceCallData->newStreamId(); + // Log + LogInfoEx(LOG_SERIAL, "V24 CALL START [STREAM ID %u]", m_rxVoiceCallData->streamId); + } else { + if (m_lclCallInProgress) { + // Flag call over + m_lclCallInProgress = false; + // Log + LogInfoEx(LOG_SERIAL, "V24 CALL END"); + // Send the TDU (using call data which we hope has been filled earlier) + m_network->writeP25TDU(*m_rxVoiceControl, *m_rxVoiceLsd); + // Reset + m_rxVoiceCallData->resetCallData(); + } + } + } + break; + // VHDR 1 Frame + case P25_DFSI_MOT_VHDR_1: + { + // Decode + MotVoiceHeader1 vhdr1 = MotVoiceHeader1(dfsiData); + + // Copy to call data VHDR1 + m_rxVoiceCallData->VHDR1 = new uint8_t[vhdr1.HCW_LENGTH]; + ::memcpy(m_rxVoiceCallData->VHDR1, vhdr1.header, vhdr1.HCW_LENGTH); + // Debug Log + if (m_debug) { + LogDebug(LOG_SERIAL, "V24 VHDR1 [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VHDR 2 Frame + case P25_DFSI_MOT_VHDR_2: + { + // Decode + MotVoiceHeader2 vhdr2 = MotVoiceHeader2(dfsiData); + // Copy to call data + ::memcpy(m_rxVoiceCallData->VHDR2, vhdr2.header, vhdr2.HCW_LENGTH); + // Debug Log + if (m_debug) { + LogDebug(LOG_SERIAL, "V24 VHDR2 [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + + // Buffer for raw VHDR data + uint8_t raw[P25_DFSI_VHDR_RAW_LEN]; + // Get VHDR1 data + ::memcpy(raw, m_rxVoiceCallData->VHDR1, 8U); + ::memcpy(raw + 8U, m_rxVoiceCallData->VHDR1 + 9U, 8U); + ::memcpy(raw + 16U, m_rxVoiceCallData->VHDR1 + 18U, 2U); + // Get VHDR2 data + ::memcpy(raw + 18U, m_rxVoiceCallData->VHDR2, 8U); + ::memcpy(raw + 26U, m_rxVoiceCallData->VHDR2 + 9U, 8U); + ::memcpy(raw + 34U, m_rxVoiceCallData->VHDR2 + 18U, 2U); + + // Buffer for decoded VHDR data + uint8_t vhdr[P25_DFSI_VHDR_LEN]; + + // Copy over the data, decoding hex with the weird bit stuffing nonsense + uint offset = 0U; + for (uint32_t i = 0; i < P25_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_SERIAL, "V24 traffic failed to decode RS (36,20,17) FEC [STREAM ID %u]", m_rxVoiceCallData->streamId); + } else { + // Copy Message Indicator + ::memcpy(m_rxVoiceCallData->mi, vhdr, P25_MI_LENGTH_BYTES); + // Get additional info + m_rxVoiceCallData->mfId = vhdr[9U]; + m_rxVoiceCallData->algoId = vhdr[10U]; + m_rxVoiceCallData->kId = __GET_UINT16B(vhdr, 11U); + m_rxVoiceCallData->dstId = __GET_UINT16B(vhdr, 13U); + } + // Log if we decoded succesfully + if (m_debug) { + LogDebug(LOG_SERIAL, "P25, HDU algId = $%02X, kId = $%04X, dstId = $%04X", m_rxVoiceCallData->algoId, m_rxVoiceCallData->kId, m_rxVoiceCallData->dstId); + } + } + catch (...) { + LogError(LOG_SERIAL, "V24 traffic got exception while trying to decode RS data for VHDR [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VOICE1/10 create a start voice frame + case P25_DFSI_LDU1_VOICE1: + { + // Decode + MotStartVoiceFrame svf = MotStartVoiceFrame(dfsiData); + // Copy + ::memcpy(m_rxVoiceCallData->netLDU1 + 10U, svf.fullRateVoice->imbeData, svf.fullRateVoice->IMBE_BUF_LEN); + // Increment our voice frame counter + m_rxVoiceCallData->n++; + } + break; + case P25_DFSI_LDU2_VOICE10: + { + // Decode + MotStartVoiceFrame svf = MotStartVoiceFrame(dfsiData); + // Copy + ::memcpy(m_rxVoiceCallData->netLDU2 + 10U, svf.fullRateVoice->imbeData, svf.fullRateVoice->IMBE_BUF_LEN); + // Increment our voice frame counter + m_rxVoiceCallData->n++; + } + break; + // The remaining LDUs all create full rate voice frames so we do that here + default: + { + // Decode + MotFullRateVoice voice = MotFullRateVoice(dfsiData); + // Copy based on frame type + switch (frameType) { + // VOICE2 + case P25_DFSI_LDU1_VOICE2: + { + ::memcpy(m_rxVoiceCallData->netLDU1 + 26U, voice.imbeData, voice.IMBE_BUF_LEN); + } + break; + // VOICE3 + case P25_DFSI_LDU1_VOICE3: + { + ::memcpy(m_rxVoiceCallData->netLDU1 + 55U, voice.imbeData, voice.IMBE_BUF_LEN); + if (voice.additionalData != nullptr) { + m_rxVoiceCallData->lco = voice.additionalData[0U]; + m_rxVoiceCallData->mfId = voice.additionalData[1U]; + m_rxVoiceCallData->serviceOptions = voice.additionalData[2U]; + } else { + LogWarning(LOG_SERIAL, "V24 VC3 traffic missing metadata [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VOICE4 + case P25_DFSI_LDU1_VOICE4: + { + ::memcpy(m_rxVoiceCallData->netLDU1 + 80U, voice.imbeData, voice.IMBE_BUF_LEN); + if (voice.additionalData != nullptr) { + m_rxVoiceCallData->dstId = __GET_UINT16(voice.additionalData, 0U); + } else { + LogWarning(LOG_SERIAL, "V24 VC4 traffic missing metadata [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VOICE5 + case P25_DFSI_LDU1_VOICE5: + { + ::memcpy(m_rxVoiceCallData->netLDU1 + 105U, voice.imbeData, voice.IMBE_BUF_LEN); + if (voice.additionalData != nullptr) { + m_rxVoiceCallData->srcId = __GET_UINT16(voice.additionalData, 0U); + } else { + LogWarning(LOG_SERIAL, "V24 VC5 traffic missing metadata [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VOICE6 + case P25_DFSI_LDU1_VOICE6: + { + ::memcpy(m_rxVoiceCallData->netLDU1 + 130U, voice.imbeData, voice.IMBE_BUF_LEN); + } + break; + // VOICE7 + case P25_DFSI_LDU1_VOICE7: + { + ::memcpy(m_rxVoiceCallData->netLDU1 + 155U, voice.imbeData, voice.IMBE_BUF_LEN); + } + break; + // VOICE8 + case P25_DFSI_LDU1_VOICE8: + { + ::memcpy(m_rxVoiceCallData->netLDU1 + 180U, voice.imbeData, voice.IMBE_BUF_LEN); + } + break; + // VOICE9 + case P25_DFSI_LDU1_VOICE9: + { + ::memcpy(m_rxVoiceCallData->netLDU1 + 204U, voice.imbeData, voice.IMBE_BUF_LEN); + if (voice.additionalData != nullptr) { + m_rxVoiceCallData->lsd1 = voice.additionalData[0U]; + m_rxVoiceCallData->lsd2 = voice.additionalData[1U]; + } else { + LogWarning(LOG_SERIAL, "V24 VC9 traffic missing metadata [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VOICE11 + case P25_DFSI_LDU2_VOICE11: + { + ::memcpy(m_rxVoiceCallData->netLDU2 + 26U, voice.imbeData, voice.IMBE_BUF_LEN); + } + break; + // VOICE12 + case P25_DFSI_LDU2_VOICE12: + { + ::memcpy(m_rxVoiceCallData->netLDU2 + 55U, voice.imbeData, voice.IMBE_BUF_LEN); + if (voice.additionalData != nullptr) { + ::memcpy(m_rxVoiceCallData->mi, voice.additionalData, 3U); + } else { + LogWarning(LOG_SERIAL, "V24 VC12 traffic missing metadata [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VOICE13 + case P25_DFSI_LDU2_VOICE13: + { + ::memcpy(m_rxVoiceCallData->netLDU2 + 80U, voice.imbeData, voice.IMBE_BUF_LEN); + if (voice.additionalData != nullptr) { + ::memcpy(m_rxVoiceCallData->mi + 3U, voice.additionalData, 3U); + } else { + LogWarning(LOG_SERIAL, "V24 VC13 traffic missing metadata [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VOICE14 + case P25_DFSI_LDU2_VOICE14: + { + ::memcpy(m_rxVoiceCallData->netLDU2 + 105U, voice.imbeData, voice.IMBE_BUF_LEN); + if (voice.additionalData != nullptr) { + ::memcpy(m_rxVoiceCallData->mi + 6U, voice.additionalData, 3U); + } else { + LogWarning(LOG_SERIAL, "V24 VC14 traffic missing metadata [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VOICE15 + case P25_DFSI_LDU2_VOICE15: + { + ::memcpy(m_rxVoiceCallData->netLDU2 + 130U, voice.imbeData, voice.IMBE_BUF_LEN); + if (voice.additionalData != nullptr) { + m_rxVoiceCallData->algoId = voice.additionalData[0U]; + m_rxVoiceCallData->kId = __GET_UINT16B(voice.additionalData, 1U); + } else { + LogWarning(LOG_SERIAL, "V24 VC15 traffic missing metadata [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + // VOICE16 + case P25_DFSI_LDU2_VOICE16: + { + ::memcpy(m_rxVoiceCallData->netLDU2 + 155U, voice.imbeData, voice.IMBE_BUF_LEN); + } + break; + // VOICE17 + case P25_DFSI_LDU2_VOICE17: + { + ::memcpy(m_rxVoiceCallData->netLDU2 + 180U, voice.imbeData, voice.IMBE_BUF_LEN); + } + break; + // VOICE18 + case P25_DFSI_LDU2_VOICE18: + { + ::memcpy(m_rxVoiceCallData->netLDU2 + 204U, voice.imbeData, voice.IMBE_BUF_LEN); + if (voice.additionalData != nullptr) { + m_rxVoiceCallData->lsd1 = voice.additionalData[0U]; + m_rxVoiceCallData->lsd2 = voice.additionalData[1U]; + } else { + LogWarning(LOG_SERIAL, "V24 VC18 traffic missing metadata [STREAM ID %u]", m_rxVoiceCallData->streamId); + } + } + break; + } + + // Increment our voice frame counter + m_rxVoiceCallData->n++; + } + break; + } + + // Get LC & LSD data if we're ready for either LDU1 or LDU2 (don't do this every frame to be more efficient) + + if (m_rxVoiceCallData->n == 9U || m_rxVoiceCallData->n == 18U) { + // Create LC + m_rxVoiceControl->setSrcId(m_rxVoiceCallData->srcId); + m_rxVoiceControl->setDstId(m_rxVoiceCallData->dstId); + // Get service options + bool emergency = ((m_rxVoiceCallData->serviceOptions & 0xFFU) & 0x80U) == 0x80U; // Emergency Flag + bool encryption = ((m_rxVoiceCallData->serviceOptions & 0xFFU) & 0x40U) == 0x40U; // Encryption Flag + uint8_t priority = ((m_rxVoiceCallData->serviceOptions & 0xFFU) & 0x07U); // Priority + m_rxVoiceControl->setEmergency(emergency); + m_rxVoiceControl->setEncrypted(encryption); + m_rxVoiceControl->setPriority(priority); + // Get more data + m_rxVoiceControl->setMI(m_rxVoiceCallData->mi); + m_rxVoiceControl->setAlgId(m_rxVoiceCallData->algoId); + m_rxVoiceControl->setKId(m_rxVoiceCallData->kId); + // Get LSD + m_rxVoiceLsd->setLSD1(m_rxVoiceCallData->lsd1); + m_rxVoiceLsd->setLSD2(m_rxVoiceCallData->lsd2); + } + + // Send LDU1 if ready + if (m_rxVoiceCallData->n == 9U) { + // Send (TODO: dynamically set HDU_VALID or DATA_VALID depending on start of call or not) + bool ret = m_network->writeP25LDU1(*m_rxVoiceControl, *m_rxVoiceLsd, m_rxVoiceCallData->netLDU1, P25_FT_HDU_VALID); + // Print + LogInfoEx(LOG_NET, P25_LDU1_STR " audio, srcId = %u, dstId = %u", m_rxVoiceCallData->srcId, m_rxVoiceCallData->dstId); + // Optional Debug + if (ret) { + if (m_debug) + LogDebug(LOG_SERIAL, "V24 LDU1 [STREAM ID %u, SRC %u, DST %u]", m_rxVoiceCallData->streamId, m_rxVoiceCallData->srcId, m_rxVoiceCallData->dstId); + /*if (m_trace) + Utils::dump(1U, "LDU1 to net", m_rxVoiceCallData->netLDU1, 9U * 25U);*/ + } + else { + LogError(LOG_SERIAL, "V24 LDU1 failed to write to network"); + } + } + + // Send LDU2 if ready + if (m_rxVoiceCallData->n == 18U) { + // Send + bool ret = m_network->writeP25LDU2(*m_rxVoiceControl, *m_rxVoiceLsd, m_rxVoiceCallData->netLDU2); + // Print + LogInfoEx(LOG_SERIAL, P25_LDU2_STR " audio, algo = $%02X, kid = $%04X", m_rxVoiceCallData->algoId, m_rxVoiceCallData->kId); + // Optional Debug + if (ret) { + if (m_debug) + LogDebug(LOG_SERIAL, "V24 LDU2 [STREAM ID %u, SRC %u, DST %u]", m_rxVoiceCallData->streamId, m_rxVoiceCallData->srcId, m_rxVoiceCallData->dstId); + /*if (m_trace) + Utils::dump(1U, "LDU2 to net", m_rxVoiceCallData->netLDU2, 9U * 25U);*/ + } + else { + LogError(LOG_SERIAL, "V24 LDU2 failed to write to network"); + } + // Reset counter since we've sent both frames + m_rxVoiceCallData->n = 0; + } +} + +/// Read a data message from the serial port +/// This is borrowed from the Modem::getResponse() function +/// Response type +RESP_TYPE_DVM SerialService::readSerial() +{ + // Flag for a 16-bit (i.e. 2-byte) length + m_msgDoubleLength = false; + + // If we're waiting for a message start byte, read a single byte + if (m_msgState == RESP_START) { + // Try and read a byte from the serial port + int ret = m_port->read(m_msgBuffer + 0U, 1U); + + // Catch read error + if (ret < 0) { + LogError(LOG_SERIAL, "Error reading from serial port, ret = %d", ret); + m_msgState = RESP_START; + return RTM_ERROR; + } + + // Handle no data + if (ret == 0) { + return RTM_TIMEOUT; + } + + // Handle anything other than a start flag while in RESP_START mode + if (m_msgBuffer[0U] != DVM_SHORT_FRAME_START && + m_msgBuffer[0U] != DVM_LONG_FRAME_START) { + ::memset(m_msgBuffer, 0x00U, BUFFER_LENGTH); + return RTM_ERROR; + } + + // Detect short vs long frame + if (m_msgBuffer[0U] == DVM_LONG_FRAME_START) { + m_msgDoubleLength = true; + } + + // Advance state machine + m_msgState = RESP_LENGTH1; + } + + // Check length byte (1/2) + if (m_msgState == RESP_LENGTH1) { + // Read length + int ret = m_port->read(m_msgBuffer + 1U, 1U); + + // Catch read error + if (ret < 0) { + LogError(LOG_SERIAL, "Error reading from serial port, ret = %d", ret); + m_msgState = RESP_START; + return RTM_ERROR; + } + + // Handle no data + if (ret == 0) { + return RTM_TIMEOUT; + } + + // Handle invalid length + if (m_msgBuffer[1U] >= 250U && !m_msgDoubleLength) { + LogError(LOG_SERIAL, "Invalid length received from the modem, len = %u", m_msgBuffer[1U]); + return RTM_ERROR; + } + + // Handle double-byte length + if (m_msgDoubleLength) { + m_msgState = RESP_LENGTH2; + m_msgLength = ( (m_msgBuffer[1U] & 0xFFU) << 8 ); + } else { + // Advnace to type byte if single-byte length + m_msgState = RESP_TYPE; + m_msgLength = m_msgBuffer[1U]; + } + + // Set proper data offset + m_msgOffset = 2U; + } + + // Check length byte (2/2) + if (m_msgState == RESP_LENGTH2) { + // Read + int ret = m_port->read(m_msgBuffer + 2U, 1U); + + // Catch read error + if (ret < 0) { + LogError(LOG_SERIAL, "Error reading from serial port, ret = %d", ret); + m_msgState = RESP_START; + return RTM_ERROR; + } + + // Handle no data + if (ret == 0) { + return RTM_TIMEOUT; + } + + // Calculate total length + m_msgLength = (m_msgLength + (m_msgBuffer[2U] & 0xFFU) ); + + // Advance state machine + m_msgState = RESP_TYPE; + + // Set flag + m_msgDoubleLength = true; + + // Set proper data offset + m_msgOffset = 3U; + } + + if (m_msgState == RESP_TYPE) { + // Read type + int ret = m_port->read(m_msgBuffer + m_msgOffset, 1U); + + // Catch read error + if (ret < 0) { + LogError(LOG_SERIAL, "Error reading from serial port, ret = %d", ret); + m_msgState = RESP_START; + return RTM_ERROR; + } + + // Handle no data + if (ret == 0) { + return RTM_TIMEOUT; + } + + // Get command type + m_msgType = (DVM_COMMANDS)m_msgBuffer[m_msgOffset]; + + // Move on + m_msgState = RESP_DATA; + m_msgOffset++; + } + + // Get the data + if (m_msgState == RESP_DATA) { + if (m_trace) { + LogDebug(LOG_SERIAL, "readSerial(), RESP_DATA, len = %u, offset = %u, type = %02X", m_msgLength, m_msgOffset, m_msgType); + } + + // Get the data based on the earlier length + while (m_msgOffset < m_msgLength) { + int ret = m_port->read(m_msgBuffer + m_msgOffset, m_msgLength - m_msgOffset); + + if (ret < 0) { + LogError(LOG_SERIAL, "Error reading from serial port, ret = %d", ret); + m_msgState = RESP_START; + return RTM_ERROR; + } + + if (ret == 0) + return RTM_TIMEOUT; + + if (ret > 0) + m_msgOffset += ret; + } + + if (m_debug && m_trace) + Utils::dump(1U, "Serial RX Data", m_msgBuffer, m_msgLength); + } + + m_msgState = RESP_START; + m_msgOffset = 0U; + + return RTM_OK; +} + +/// Called from clock thread, checks for an available P25 frame to write and sends it based on jitter timing requirements +/// Very similar to the readP25Frame function below +/// +/// Note: the length encoded at the start does not include the length, tag, or timestamp bytes +int SerialService::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) { + // Peek everything up to the timestamp + 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_SERIAL, "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; +} + +/// +/// Gets a frame of P25 data from the RX queue +/// +/// The data buffer to populate +/// The size of the P25 data retreived, including the leading data tag +uint32_t SerialService::readP25Frame(uint8_t* data) +{ + + /** + * Serial RX ringbuffer format: + * + * | 0x01 | 0x02 | 0x03 | 0x04 | ... | + * | Length | Tag | data | + */ + + // sanity check + assert(data != nullptr); + + // Check empty + if (m_rxP25Queue.isEmpty()) + return 0U; + + // Get length bytes + uint8_t length[2U]; + ::memset(length, 0x00U, 2U); + m_rxP25Queue.peek(length, 2U); + + // Convert length bytes to a length int + uint16_t len = 0U; + len = (length[0] << 8) + length[1U]; + + // this ensures we never get in a situation where we have length stuck on the queue + if (m_rxP25Queue.dataSize() == 2U && len > m_rxP25Queue.dataSize()) { + m_rxP25Queue.get(length, 2U); // ensure we pop the length off + return 0U; + } + + if (m_rxP25Queue.dataSize() >= len) { + m_rxP25Queue.get(length, 2U); // ensure we pop the length off + m_rxP25Queue.get(data, len); + return len; + } + + return 0U; +} + +/// +/// Break apart a P25 LDU and add to the TX queue, timed appropriately +/// +/// DUID flag for the LDU +/// Link Control data +/// LDU data +/// +/// This is very similar to the C# Mot_DFSISendFrame functions, we don't implement the TIA DFSI sendframe in serial +/// because the only devices we connect to over serial V24 are Moto +void SerialService::writeP25Frame(uint8_t duid, dfsi::LC& lc, uint8_t* ldu) +{ + // Sanity check + assert(ldu != nullptr); + + // Get now + int64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + // Break out the control components + lc::LC control = lc::LC(*lc.control()); + data::LowSpeedData lsd = data::LowSpeedData(*lc.lsd()); + + // Get the service options + uint8_t serviceOptions = + (control.getEmergency() ? 0x80U : 0x00U) + + (control.getEncrypted() ? 0x40U : 0x00U) + + (control.getPriority() & 0x07U); + + // Get the MI + uint8_t mi[P25_MI_LENGTH_BYTES]; + control.getMI(mi); + + // Calculate reed-solomon encoding depending on DUID type + uint8_t rs[P25_LDU_LC_FEC_LENGTH_BYTES]; + ::memset(rs, 0x00U, P25_LDU_LC_FEC_LENGTH_BYTES); + switch(duid) { + case P25_DUID_LDU1: + { + rs[0U] = control.getLCO(); // LCO + rs[1U] = control.getMFId(); // MFId + rs[2U] = serviceOptions; // Service Options + uint32_t dstId = control.getDstId(); + rs[3U] = (dstId >> 16) & 0xFFU; // Target Address + rs[4U] = (dstId >> 8) & 0xFFU; + rs[5U] = (dstId >> 0) & 0xFFU; + uint32_t srcId = control.getSrcId(); + rs[6U] = (srcId >> 16) & 0xFFU; // Source Address + rs[7U] = (srcId >> 8) & 0xFFU; + rs[8U] = (srcId >> 0) & 0xFFU; + + // encode RS (24,12,13) FEC + m_rs.encode241213(rs); + } + break; + case P25_DUID_LDU2: + { + for (uint32_t i = 0; i < P25_MI_LENGTH_BYTES; i++) + rs[i] = mi[i]; // Message Indicator + + rs[9U] = control.getAlgId(); // Algorithm ID + rs[10U] = (control.getKId() >> 8) & 0xFFU; // Key ID + rs[11U] = (control.getKId() >> 0) & 0xFFU; // ... + + // encode RS (24,16,9) FEC + m_rs.encode24169(rs); + } + break; + } + + // Placeholder for last call timestamp retrieved below (not yet used) + //int64_t lastHeard = 0U; + + // Placeholder for sequence number retrieved below + uint32_t sequence = 0U; + + // Get the last heard value (not used currently, TODO: use this for covnentional TGID timeout purposes) + /*{ + auto entry = m_lastHeard.find(control.getDstId()); + if (entry != m_lastHeard.end()) { + lastHeard = m_lastHeard[control.getDstId()]; + } + }*/ + + // Get the last sequence number + { + auto entry = m_sequences.find(control.getDstId()); + if (entry != m_sequences.end()) { + sequence = m_sequences[control.getDstId()]; + } + } + + // Check if we need to start a new data stream + if (duid == P25_DUID_LDU1 && ((sequence == 0U) || (sequence == RTP_END_OF_CALL_SEQ))) { + // Start the new stream + startOfStream(lc); + // Update our call entries + m_lastHeard[control.getDstId()] = now; + m_sequences[control.getDstId()] = ++sequence; + + // Log + LogInfoEx(LOG_SERIAL, "CALL START: %svoice call from %u to TG %u", (control.getAlgId() != P25_ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); + + ActivityLog("network %svoice transmission call from %u to TG %u", (control.getAlgId() != P25_ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); + } else { + // If this TGID isn't either lookup, consider it a late entry and start a new stream + if ( (m_sequences.find(control.getDstId()) == m_sequences.end()) || (m_lastHeard.find(control.getDstId()) == m_lastHeard.end()) ) { + // Start the stream, same as above + startOfStream(lc); + m_lastHeard[control.getDstId()] = now; + m_sequences[control.getDstId()] = ++sequence; + + LogInfoEx(LOG_SERIAL, "LATE CALL START: %svoice call from %u to TG %u", (control.getAlgId() != P25_ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); + ActivityLog("network %svoice transmission late entry from %u to TG %u", (control.getAlgId() != P25_ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); + } + } + + // Check if we need to end the call + if ( (duid == P25_DUID_TDU) || (duid == P25_DUID_TDULC) ) { + // Stop + endOfStream(); + // Log + LogInfoEx(LOG_SERIAL, "CALL END: %svoice call from %u to TG %u", (control.getAlgId() != P25_ALGO_UNENCRYPT) ? "encrypted " : "", control.getSrcId(), control.getDstId()); + // Clear our counters + m_sequences[control.getDstId()] = RTP_END_OF_CALL_SEQ; + } + + // Break out the 9 individual P25 packets + for (int n = 0; n < 9; n++) { + + // We use this buffer and fullrate voice object to slim down the code to a common sendFrame routine at the bottom + uint8_t* buffer = nullptr; + uint16_t bufferSize = 0; + MotFullRateVoice voice = MotFullRateVoice(); + + switch (n) { + case 0: // VOICE1/10 + { + // Set frametype + voice.frameType = (duid == P25_DUID_LDU1) ? P25_DFSI_LDU1_VOICE1 : P25_DFSI_LDU2_VOICE10; + // Create the new frame objects + MotStartVoiceFrame svf = MotStartVoiceFrame(); + svf.startOfStream = new MotStartOfStream(); + svf.fullRateVoice = new MotFullRateVoice(); + // Set values appropriately + svf.startOfStream->startStop = StartStopFlag::START; + svf.startOfStream->rt = m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED; + // Set frame type + svf.fullRateVoice->frameType = voice.frameType; + // Set source flag & ICW flag + svf.fullRateVoice->source = m_diu ? SOURCE_DIU : SOURCE_QUANTAR; + svf.icw = m_diu ? ICW_DIU : ICW_QUANTAR; + // Copy data + ::memcpy(svf.fullRateVoice->imbeData, ldu + 10U, svf.fullRateVoice->IMBE_BUF_LEN); + // Encode + buffer = new uint8_t[svf.LENGTH]; + ::memset(buffer, 0x00U, svf.LENGTH); + svf.encode(buffer); + bufferSize = svf.LENGTH; + } + break; + case 1: // VOICE2/11 + { + voice.frameType = (duid == P25_DUID_LDU1) ? P25_DFSI_LDU1_VOICE2 : P25_DFSI_LDU2_VOICE11; + // Set source flag + voice.source = m_diu ? SOURCE_DIU : SOURCE_QUANTAR; + ::memcpy(voice.imbeData, ldu + 26U, voice.IMBE_BUF_LEN); + } + break; + case 2: // VOICE3/12 + { + voice.frameType = (duid == P25_DUID_LDU1) ? P25_DFSI_LDU1_VOICE3 : P25_DFSI_LDU2_VOICE12; + ::memcpy(voice.imbeData, ldu + 55U, voice.IMBE_BUF_LEN); + // Create the additional data array + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + // Copy additional data + if (voice.frameType == P25_DUID_LDU1) { + voice.additionalData[0U] = control.getLCO(); + voice.additionalData[1U] = control.getMFId(); + voice.additionalData[2U] = serviceOptions; + } else { + voice.additionalData[0U] = mi[0U]; + voice.additionalData[1U] = mi[1U]; + voice.additionalData[2U] = mi[2U]; + } + } + break; + case 3: // VOICE4/13 + { + voice.frameType = (duid == P25_DUID_LDU1) ? P25_DFSI_LDU1_VOICE4 : P25_DFSI_LDU2_VOICE13; + ::memcpy(voice.imbeData, ldu + 80U, voice.IMBE_BUF_LEN); + // Create the additional data array + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + // We set the additional data based on LDU1/2 + switch (duid) { + case P25_DUID_LDU1: + { + // Destination address (3 bytes) + __SET_UINT16(control.getDstId(), voice.additionalData, 0U); + } + break; + case P25_DUID_LDU2: + { + // Message Indicator + voice.additionalData[0U] = mi[3U]; + voice.additionalData[1U] = mi[4U]; + voice.additionalData[2U] = mi[5U]; + } + break; + } + } + break; + case 4: // VOICE5/14 + { + voice.frameType = (duid == P25_DUID_LDU1) ? P25_DFSI_LDU1_VOICE5 : P25_DFSI_LDU2_VOICE14; + ::memcpy(voice.imbeData, ldu + 105U, voice.IMBE_BUF_LEN); + // Create the additional data array + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + // Same as case 3 above + switch (duid) { + case P25_DUID_LDU1: + { + // Source address (3 bytes) + __SET_UINT16(control.getSrcId(), voice.additionalData, 0U); + } + break; + case P25_DUID_LDU2: + { + // Message Indicator + voice.additionalData[0U] = mi[6U]; + voice.additionalData[1U] = mi[7U]; + voice.additionalData[2U] = mi[8U]; + } + break; + } + } + break; + case 5: // VOICE6/15 + { + voice.frameType = (duid == P25_DUID_LDU1) ? P25_DFSI_LDU1_VOICE6 : P25_DFSI_LDU2_VOICE15; + ::memcpy(voice.imbeData, ldu + 130U, voice.IMBE_BUF_LEN); + // Create the additional data array + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + // Another switch on LDU1/2 + switch (duid) { + case P25_DUID_LDU1: + { + // RS encoding + voice.additionalData[0U] = rs[9U]; + voice.additionalData[1U] = rs[10U]; + voice.additionalData[2U] = rs[11U]; + } + break; + case P25_DUID_LDU2: + { + voice.additionalData[0U] = control.getAlgId(); // Algo ID + __SET_UINT16B(control.getKId(), voice.additionalData, 1U); // Key ID + } + break; + } + } + break; + case 6: // VOICE7/16 + { + voice.frameType = (duid == P25_DUID_LDU1) ? P25_DFSI_LDU1_VOICE7 : P25_DFSI_LDU2_VOICE16; + ::memcpy(voice.imbeData, ldu + 155U, voice.IMBE_BUF_LEN); + // Create the additional data array + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + // RS data offsets are the same regardless of LDU + voice.additionalData[0U] = rs[12U]; + voice.additionalData[1U] = rs[13U]; + voice.additionalData[2U] = rs[14U]; + } + break; + case 7: // VOICE8/17 + { + voice.frameType = (duid == P25_DUID_LDU1) ? P25_DFSI_LDU1_VOICE8 : P25_DFSI_LDU2_VOICE17; + ::memcpy(voice.imbeData, ldu + 180U, voice.IMBE_BUF_LEN); + // Create the additional data array + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + // RS data offsets are the same regardless of LDU + voice.additionalData[0U] = rs[15U]; + voice.additionalData[1U] = rs[16U]; + voice.additionalData[2U] = rs[17U]; + } + break; + case 8: // VOICE9/18 + { + voice.frameType = (duid == P25_DUID_LDU1) ? P25_DFSI_LDU1_VOICE9 : P25_DFSI_LDU2_VOICE18; + ::memcpy(voice.imbeData, ldu + 204U, voice.IMBE_BUF_LEN); + // Create the additional data array + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + // Get low speed data bytes + 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(); + } + + // Debug logging + if (m_trace) { + Utils::dump("Encoded V24 voice frame data", buffer, bufferSize); + } + + // Send if we have data (which we always should) + if (buffer != nullptr) { + addTxToQueue(buffer, bufferSize, SERIAL_TX_TYPE::IMBE); + } + + } +} + +/// +/// Send a start of stream sequence (HDU, etc) to the connected serial V24 device +/// +/// Link control data object +void SerialService::startOfStream(const LC& lc) +{ + // Flag that we have a network call in progress + m_netCallInProgress = true; + + // Create new start of stream + MotStartOfStream start = MotStartOfStream(); + start.startStop = StartStopFlag::START; + start.rt = m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED; + + // Create buffer for bytes and encode + uint8_t buffer[start.LENGTH]; + ::memset(buffer, 0x00U, start.LENGTH); + start.encode(buffer); + + // Optional debug & trace + if (m_debug) + LogDebug(LOG_SERIAL, "encoded mot p25 start frame"); + if (m_trace) + Utils::dump(1U, "data", buffer, start.LENGTH); + + // Send start frame + addTxToQueue(buffer, start.LENGTH, network::SERIAL_TX_TYPE::NONIMBE); + + // Break out the control components + lc::LC control = lc::LC(*lc.control()); + data::LowSpeedData lsd = data::LowSpeedData(*lc.lsd()); + + // Init message indicator & get + uint8_t mi[P25_MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, P25_MI_LENGTH_BYTES); + control.getMI(mi); + + // Init VHDR data array + uint8_t vhdr[P25_DFSI_VHDR_LEN]; + ::memset(vhdr, 0x00U, P25_DFSI_VHDR_LEN); + + // Copy MI to VHDR + ::memcpy(vhdr, mi, P25_MI_LENGTH_BYTES); + + // Set values + 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 (some kind of bit packing thing I don't understand) + uint8_t raw[P25_DFSI_VHDR_RAW_LEN]; + uint32_t offset = 0; + for (uint8_t i = 0; i < P25_DFSI_VHDR_RAW_LEN; i++, offset += 6) { + raw[i] = Utils::bin2Hex(vhdr, offset); + } + + // Prepare VHDR1 + MotVoiceHeader1 vhdr1 = MotVoiceHeader1(); + vhdr1.startOfStream = new MotStartOfStream(); + vhdr1.startOfStream->startStop = StartStopFlag::START; + vhdr1.startOfStream->rt = m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED; + vhdr1.icw = m_diu ? ICW_DIU : ICW_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 buffer1[vhdr1.LENGTH]; + ::memset(buffer1, 0x00U, vhdr1.LENGTH); + vhdr1.encode(buffer1); + + if (m_debug) + LogDebug(LOG_SERIAL, "encoded mot VHDR1 p25 frame"); + if (m_trace) + Utils::dump(1U, "data", buffer1, vhdr1.LENGTH); + + addTxToQueue(buffer1, vhdr1.LENGTH, SERIAL_TX_TYPE::NONIMBE); + + // 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 buffer2[vhdr2.LENGTH]; + ::memset(buffer2, 0x00U, vhdr2.LENGTH); + vhdr2.encode(buffer2); + + if (m_debug) + LogDebug(LOG_SERIAL, "encoded mot VHDR2 p25 frame"); + if (m_trace) + Utils::dump(1U, "data", buffer2, vhdr2.LENGTH); + + addTxToQueue(buffer2, vhdr2.LENGTH, SERIAL_TX_TYPE::NONIMBE); +} + +/// +/// Send an end of stream sequence (TDU, etc) to the connected serial V24 device +/// +/// Link control data object +void SerialService::endOfStream() +{ + // Create the new end of stream (which looks like a start of stream with the stop flag) + MotStartOfStream end = MotStartOfStream(); + end.startStop = StartStopFlag::STOP; + + // Create buffer and encode + uint8_t buffer[end.LENGTH]; + ::memset(buffer, 0x00U, end.LENGTH); + end.encode(buffer); + + if (m_trace) { + LogDebug(LOG_SERIAL, "encoded mot p25 end frame"); + Utils::dump(1U, "data", buffer, end.LENGTH); + } + + // Send start frame + addTxToQueue(buffer, end.LENGTH, SERIAL_TX_TYPE::NONIMBE); + + // Our net call is done + m_netCallInProgress = false; +} + +/// +/// Helper to add a V24 dataframe to the P25 TX queue with the proper timestamp and formatting +/// +/// Data array to send +/// Length of data in array +/// Type of message to send (used for proper jitter clocking) +void SerialService::addTxToQueue(uint8_t* data, uint16_t len, SERIAL_TX_TYPE msgType) +{ + // If the port isn't connected, just return + if (m_port == nullptr) { return; } + + // 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 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 == IMBE) { + // IMBEs must go out at 20ms intervals + msgTime = m_lastP25Tx + 20; + } 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 + 5; + } + } + } + + // Increment the length by 4 for the header bytes + 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); + + //Utils::dump(1U, "SERIAL TX DATA", data, len - 4U); + + // Update the last message time + m_lastP25Tx = msgTime; +} + +/// +/// Helper to insert IMBE silence frames for missing audio. +/// +/// +/// +void SerialService::insertMissingAudio(uint8_t *data, uint32_t& lost) +{ + if (data[10U] == 0x00U) { + ::memcpy(data + 10U, m_lastIMBE, 11U); + lost++; + } + else { + ::memcpy(m_lastIMBE, data + 10U, 11U); + } + + if (data[26U] == 0x00U) { + ::memcpy(data + 26U, m_lastIMBE, 11U); + lost++; + } + else { + ::memcpy(m_lastIMBE, data + 26U, 11U); + } + + if (data[55U] == 0x00U) { + ::memcpy(data + 55U, m_lastIMBE, 11U); + lost++; + } + else { + ::memcpy(m_lastIMBE, data + 55U, 11U); + } + + if (data[80U] == 0x00U) { + ::memcpy(data + 80U, m_lastIMBE, 11U); + lost++; + } + else { + ::memcpy(m_lastIMBE, data + 80U, 11U); + } + + if (data[105U] == 0x00U) { + ::memcpy(data + 105U, m_lastIMBE, 11U); + lost++; + } + else { + ::memcpy(m_lastIMBE, data + 105U, 11U); + } + + if (data[130U] == 0x00U) { + ::memcpy(data + 130U, m_lastIMBE, 11U); + lost++; + } + else { + ::memcpy(m_lastIMBE, data + 130U, 11U); + } + + if (data[155U] == 0x00U) { + ::memcpy(data + 155U, m_lastIMBE, 11U); + lost++; + } + else { + ::memcpy(m_lastIMBE, data + 155U, 11U); + } + + if (data[180U] == 0x00U) { + ::memcpy(data + 180U, m_lastIMBE, 11U); + lost++; + } + else { + ::memcpy(m_lastIMBE, data + 180U, 11U); + } + + if (data[204U] == 0x00U) { + ::memcpy(data + 204U, m_lastIMBE, 11U); + lost++; + } + else { + ::memcpy(m_lastIMBE, data + 204U, 11U); + } +} + +void SerialService::printDebug(const uint8_t* buffer, uint16_t len) +{ + if (m_msgDoubleLength && buffer[3U] == CMD_DEBUG_DUMP) { + uint8_t data[512U]; + ::memset(data, 0x00U, 512U); + ::memcpy(data, buffer, len); + + Utils::dump(1U, "V24 Debug Dump", data, len); + return; + } + else { + if (m_msgDoubleLength) { + LogError(LOG_SERIAL, "Invalid debug data received from the V24 board, len = %u", len); + return; + } + } + + // Handle the individual debug types + if (buffer[2U] == CMD_DEBUG1) { + LogDebug(LOG_SERIAL, "V24 USB: %.*s", len - 3U, buffer + 3U); + } + else if (buffer[2U] == CMD_DEBUG2) { + short val1 = (buffer[len - 2U] << 8) | buffer[len - 1U]; + LogDebug(LOG_SERIAL, "V24 USB: %.*s %X", len - 5U, buffer + 3U, val1); + } + else if (buffer[2U] == CMD_DEBUG3) { + short val1 = (buffer[len - 4U] << 8) | buffer[len - 3U]; + short val2 = (buffer[len - 2U] << 8) | buffer[len - 1U]; + LogDebug(LOG_SERIAL, "V24 USB: %.*s %X %X", len - 7U, buffer + 3U, val1, val2); + } + else if (buffer[2U] == CMD_DEBUG4) { + short val1 = (buffer[len - 6U] << 8) | buffer[len - 5U]; + short val2 = (buffer[len - 4U] << 8) | buffer[len - 3U]; + short val3 = (buffer[len - 2U] << 8) | buffer[len - 1U]; + LogDebug(LOG_SERIAL, "V24 USB: %.*s %X %X %X", len - 9U, buffer + 3U, val1, val2, val3); + } + else if (buffer[2U] == CMD_DEBUG5) { + short val1 = (buffer[len - 8U] << 8) | buffer[len - 7U]; + short val2 = (buffer[len - 6U] << 8) | buffer[len - 5U]; + short val3 = (buffer[len - 4U] << 8) | buffer[len - 3U]; + short val4 = (buffer[len - 2U] << 8) | buffer[len - 1U]; + LogDebug(LOG_SERIAL, "V24 USB: %.*s %X %X %X %X", len - 11U, buffer + 3U, val1, val2, val3, val4); + } + else if (buffer[2U] == CMD_DEBUG_DUMP) { + uint8_t data[255U]; + ::memset(data, 0x00U, 255U); + ::memcpy(data, buffer, len); + Utils::dump(1U, "V24 USB Debug Dump", data, len); + } +} \ No newline at end of file diff --git a/src/dfsi/network/SerialService.h b/src/dfsi/network/SerialService.h new file mode 100644 index 00000000..9c0d2573 --- /dev/null +++ b/src/dfsi/network/SerialService.h @@ -0,0 +1,157 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ + +#if !defined(__SERIAL_SERVICE_H__) +#define __SERIAL_SERVICE_H__ + +// DVM Includes +#include "Defines.h" +#include "common/edac/RS634717.h" +#include "common/network/RawFrameQueue.h" +#include "common/p25/data/LowSpeedData.h" +#include "common/p25/P25Defines.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/p25/dfsi/LC.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "common/yaml/Yaml.h" +#include "common/RingBuffer.h" +#include "network/DfsiPeerNetwork.h" +#include "network/CallData.h" +#include "rtp/RtpFrames.h" +#include "host/modem/Modem.h" +#include "host/modem/port/IModemPort.h" +#include "host/modem/port/UARTPort.h" + +// System Includes +#include +#include +#include +#include +#include +#include + +using namespace modem; +using namespace p25; +using namespace dfsi; + +namespace network +{ + + // DFSI serial tx flags used to determine proper jitter handling of data in ringbuffer + enum SERIAL_TX_TYPE { + NONIMBE, + IMBE + }; + + // --------------------------------------------------------------------------- + // Class Declaration + // Serial V24 service + // --------------------------------------------------------------------------- + + class HOST_SW_API SerialService { + public: + SerialService(const std::string& portName, uint32_t baudrate, bool rtrt, bool diu, uint16_t jitter, DfsiPeerNetwork* network, uint32_t p25TxQueueSize, uint32_t p25RxQueueSize, bool debug, bool trace); + + ~SerialService(); + + void clock(uint32_t ms); + + bool open(); + + void close(); + + // Handle P25 data from network to V24 + void processP25FromNet(UInt8Array p25Buffer, uint32_t length); + + // Handle P25 data from V24 to network + void processP25ToNet(); + + private: + std::string m_portName; + uint32_t m_baudrate; + bool m_rtrt; + bool m_diu; + + port::IModemPort* m_port; + uint16_t m_jitter; + + bool m_debug; + bool m_trace; + + DfsiPeerNetwork* m_network; + + uint8_t* m_lastIMBE; + + // Tables to keep track of calls + std::unordered_map m_lastHeard; + std::unordered_map m_sequences; + + uint8_t* m_msgBuffer; + RESP_STATE m_msgState; + uint16_t m_msgLength; + uint16_t m_msgOffset; + DVM_COMMANDS m_msgType; + bool m_msgDoubleLength; + + uint32_t m_netFrames; + uint32_t m_netLost; + + RingBuffer m_rxP25Queue; + RingBuffer m_txP25Queue; + + uint64_t m_lastP25Tx; + + edac::RS634717 m_rs; + + // Counter for assembling a full LDU from individual frames in the RX queue + uint8_t m_rxP25LDUCounter; + + // Flags to indicate if calls to/from the FNE are already in progress + bool m_netCallInProgress; + bool m_lclCallInProgress; + + // Control and LSD objects for current RX P25 data being built to send to the net + lc::LC* m_rxVoiceControl; + data::LowSpeedData* m_rxVoiceLsd; + + // Call Data object for current RX P25 call (VHDR, LDU, etc) + VoiceCallData* m_rxVoiceCallData; + + // Functions called by clock() to read/write from/to the serial port + RESP_TYPE_DVM readSerial(); + int writeSerial(); + + uint32_t readP25Frame(uint8_t* data); + void writeP25Frame(uint8_t duid, dfsi::LC& lc, uint8_t* ldu); + + // Helpers for TX stream data + void startOfStream(const LC& lc); + void endOfStream(); + + // Helper for timing TX data appropriately based on frame type + void addTxToQueue(uint8_t* data, uint16_t length, SERIAL_TX_TYPE msgType); + + /// Helper to insert IMBE silence frames for missing audio. + void insertMissingAudio(uint8_t* data, uint32_t& lost); + + void printDebug(const uint8_t* buffer, uint16_t length); + }; + + // Defines for Mot DFSI + + +} // namespace network + +#endif // __SERIAL_SERVICE_H__ \ No newline at end of file diff --git a/src/dfsi/rtp/MotFullRateVoice.cpp b/src/dfsi/rtp/MotFullRateVoice.cpp new file mode 100644 index 00000000..ea5d2350 --- /dev/null +++ b/src/dfsi/rtp/MotFullRateVoice.cpp @@ -0,0 +1,211 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ + +#include "rtp/MotFullRateVoice.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/Utils.h" +#include "common/Log.h" + +#include +#include + +using namespace p25; +using namespace dfsi; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/// +/// Initializes a instance of the MotFullRateVoice class. +/// +MotFullRateVoice::MotFullRateVoice() +{ + frameType = P25_DFSI_LDU1_VOICE1; + additionalData = nullptr; + source = SOURCE_QUANTAR; + imbeData = new uint8_t[IMBE_BUF_LEN]; + ::memset(imbeData, 0x00U, IMBE_BUF_LEN); +} + +/// +/// Initializes a instance of the MotFullRateVoice class. +/// +/// +MotFullRateVoice::MotFullRateVoice(uint8_t* data) +{ + decode(data); +} + +/// +/// Finalizes a instance of the MotFullRateVoice class. +/// +MotFullRateVoice::~MotFullRateVoice() +{ + delete[] imbeData; + delete[] additionalData; +} + +/// +/// +/// +/// +uint32_t MotFullRateVoice::size() +{ + uint32_t length = 0; + + // Set length appropriately based on frame type + if (isVoice1or2or10or11()) { + length += SHORTENED_LENGTH; + } else { + length += LENGTH; + } + + // These are weird + if (isVoice9or18()) { + length -= 1; + } + + return length; +} + +/// +/// Decode a full rate voice frame. +/// +/// +/// +/// +bool MotFullRateVoice::decode(const uint8_t* data, bool shortened) +{ + assert(data != nullptr); + + imbeData = new uint8_t[IMBE_BUF_LEN]; + ::memset(imbeData, 0x00U, IMBE_BUF_LEN); + + frameType = data[0U]; + + if (isVoice2or11()) { + shortened = true; + } + + + if (shortened) { + ::memcpy(imbeData, data + 1U, IMBE_BUF_LEN); + source = (SourceFlag)data[12U]; + // Forgot to set this originally and left additionalData uninitialized, whoops! + additionalData = nullptr; + } else { + // Frames 0x6A and 0x73 are missing the 0x00 padding byte, so we start IMBE data 1 byte earlier + uint8_t imbeStart = 5U; + if (isVoice9or18()) { + imbeStart = 4U; + } + + additionalData = new uint8_t[ADDITIONAL_LENGTH]; + ::memset(additionalData, 0x00U, ADDITIONAL_LENGTH); + ::memcpy(additionalData, data + 1U, ADDITIONAL_LENGTH); + + // Copy IMBE data based on our imbe start position + ::memcpy(imbeData, data + imbeStart, IMBE_BUF_LEN); + + source = (SourceFlag)data[IMBE_BUF_LEN + imbeStart]; + } + + return true; +} + +/// +/// Encode a full rate voice frame. +/// +/// +/// +void MotFullRateVoice::encode(uint8_t* data, bool shortened) +{ + assert(data != nullptr); + + // Check if we're a shortened frame + data[0U] = frameType; + if (isVoice2or11()) { + shortened = true; + } + + // Copy based on shortened frame or not + if (shortened) { + ::memcpy(data + 1U, imbeData, IMBE_BUF_LEN); + data[12U] = (uint8_t)source; + } + // If not shortened, our IMBE data start position depends on frame type + else { + // Starting index for the IMBE data + uint8_t imbeStart = 5U; + if (isVoice9or18()) { + imbeStart = 4U; + } + + // Check if we have additional data + if (additionalData != nullptr) { + ::memcpy(data + 1U, additionalData, ADDITIONAL_LENGTH); + } + + // Copy rest of data + ::memcpy(data + imbeStart, imbeData, IMBE_BUF_LEN); + + // Source byte at the end + data[11U + imbeStart] = (uint8_t)source; + } +} + +// --------------------------------------------------------------------------- +// Protected Class Members +// --------------------------------------------------------------------------- + +/// +/// +/// +/// +bool MotFullRateVoice::isVoice1or2or10or11() +{ + if ( (frameType == P25_DFSI_LDU1_VOICE1) || (frameType == P25_DFSI_LDU1_VOICE2) || (frameType == P25_DFSI_LDU2_VOICE10) || (frameType == P25_DFSI_LDU2_VOICE11) ) { + return true; + } else { + return false; + } +} + +/// +/// +/// +/// +bool MotFullRateVoice::isVoice2or11() +{ + if ( (frameType == P25_DFSI_LDU1_VOICE2) || (frameType == P25_DFSI_LDU2_VOICE11) ) { + return true; + } else { + return false; + } +} + +/// +/// +/// +/// +bool MotFullRateVoice::isVoice9or18() +{ + if ( (frameType == P25_DFSI_LDU1_VOICE9) || (frameType == P25_DFSI_LDU2_VOICE18) ) { + return true; + } else { + return false; + } +} diff --git a/src/dfsi/rtp/MotFullRateVoice.h b/src/dfsi/rtp/MotFullRateVoice.h new file mode 100644 index 00000000..2ce453da --- /dev/null +++ b/src/dfsi/rtp/MotFullRateVoice.h @@ -0,0 +1,85 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ +#if !defined(__MOT_FULL_RATE_VOICE_H__) +#define __MOT_FULL_RATE_VOICE_H__ + +#include "Defines.h" +#include "common/Defines.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "rtp/RtpDefines.h" + +namespace p25 +{ + namespace dfsi + { + // --------------------------------------------------------------------------- + // Class Declaration + // Implements a P25 Motorola full rate voice packet. + // + // Byte 0 1 2 3 + // Bit 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | FT | Addtl Data | Addtl Data | Addtl Data | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Reserved | IMBE 1 | IMBE 2 | IMBE 3 | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | IMBE 4 | IMBE 5 | IMBE 6 | IMBE 7 | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | IMBE 8 | IMBE 9 | IMBE 10 | IMBE 11 | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Src Flag | + // +=+=+=+=+=+=+=+=+ + // --------------------------------------------------------------------------- + + class HOST_SW_API MotFullRateVoice { + public: + // Frame information + static const uint8_t LENGTH = 17; + static const uint8_t SHORTENED_LENGTH = 13; + static const uint8_t ADDITIONAL_LENGTH = 4; + static const uint8_t IMBE_BUF_LEN = 11; + + uint8_t frameType; + uint8_t* imbeData; + uint8_t* additionalData; + SourceFlag source; + + /// Initializes a copy instance of the MotFullRateVoice class. + MotFullRateVoice(); + /// Initializes a copy instance of the MotFullRateVoice class. + MotFullRateVoice(uint8_t* data); + /// Finalizes a instance of the MotFullRateVoice class. + ~MotFullRateVoice(); + + /// + uint32_t size(); + /// Decode a full rate voice frame. + bool decode(const uint8_t* data, bool shortened = false); + /// Encode a full rate voice frame. + void encode(uint8_t* data, bool shortened = false); + + private: + /// + bool isVoice1or2or10or11(); + /// + bool isVoice2or11(); + /// + bool isVoice9or18(); + }; + } // namespace dfsi +} // namespace p25 + +#endif // __MOT_FULL_RATE_VOICE_H__ \ No newline at end of file diff --git a/src/dfsi/rtp/MotRtpFrames.cpp b/src/dfsi/rtp/MotRtpFrames.cpp new file mode 100644 index 00000000..815e95be --- /dev/null +++ b/src/dfsi/rtp/MotRtpFrames.cpp @@ -0,0 +1,432 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ + +#include "rtp/MotRtpFrames.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/Utils.h" +#include "common/Log.h" + +#include +#include + +using namespace p25; +using namespace dfsi; + +// --------------------------------------------------------------------------- +// Motorola full rate voice data +// --------------------------------------------------------------------------- + +MotFullRateVoice::MotFullRateVoice() +{ + frameType = P25_DFSI_LDU1_VOICE1; + additionalData = nullptr; + source = SOURCE_QUANTAR; + imbeData = new uint8_t[IMBE_BUF_LEN]; + ::memset(imbeData, 0x00U, IMBE_BUF_LEN); +} + +MotFullRateVoice::MotFullRateVoice(uint8_t* data) +{ + decode(data); +} + +MotFullRateVoice::~MotFullRateVoice() +{ + delete[] imbeData; + delete[] additionalData; +} + +uint32_t MotFullRateVoice::size() +{ + uint32_t length = 0; + + // Set length appropriately based on frame type + if (isVoice1or2or10or11()) { + length += SHORTENED_LENGTH; + } else { + length += LENGTH; + } + + // These are weird + if (isVoice9or18()) { + length -= 1; + } + + return length; +} + +/// Decode a block of bytes into a full rate voice IMBE block +bool MotFullRateVoice::decode(uint8_t* data, bool shortened) +{ + // Sanity check + assert(data != nullptr); + + imbeData = new uint8_t[IMBE_BUF_LEN]; + ::memset(imbeData, 0x00U, IMBE_BUF_LEN); + + frameType = data[0U]; + + if (isVoice2or11()) { + shortened = true; + } + + + if (shortened) { + ::memcpy(imbeData, data + 1U, IMBE_BUF_LEN); + source = (SourceFlag)data[12U]; + // Forgot to set this originally and left additionalData uninitialized, whoops! + additionalData = nullptr; + } else { + // Frames 0x6A and 0x73 are missing the 0x00 padding byte, so we start IMBE data 1 byte earlier + uint8_t imbeStart = 5U; + if (isVoice9or18()) { + imbeStart = 4U; + } + + additionalData = new uint8_t[ADDITIONAL_LENGTH]; + ::memset(additionalData, 0x00U, ADDITIONAL_LENGTH); + ::memcpy(additionalData, data + 1U, ADDITIONAL_LENGTH); + + // Copy IMBE data based on our imbe start position + ::memcpy(imbeData, data + imbeStart, IMBE_BUF_LEN); + + source = (SourceFlag)data[IMBE_BUF_LEN + imbeStart]; + } + + return true; +} + +void MotFullRateVoice::encode(uint8_t* data, bool shortened) +{ + // Sanity check + assert(data != nullptr); + + // Check if we're a shortened frame + data[0U] = frameType; + if (isVoice2or11()) { + shortened = true; + } + + // Copy based on shortened frame or not + if (shortened) { + ::memcpy(data + 1U, imbeData, IMBE_BUF_LEN); + data[12U] = (uint8_t)source; + } + // If not shortened, our IMBE data start position depends on frame type + else { + // Starting index for the IMBE data + uint8_t imbeStart = 5U; + if (isVoice9or18()) { + imbeStart = 4U; + } + + // Check if we have additional data + if (additionalData != nullptr) { + ::memcpy(data + 1U, additionalData, ADDITIONAL_LENGTH); + } + + // Copy rest of data + ::memcpy(data + imbeStart, imbeData, IMBE_BUF_LEN); + + // Source byte at the end + data[11U + imbeStart] = (uint8_t)source; + } +} + +bool MotFullRateVoice::isVoice1or2or10or11() +{ + if ( (frameType == P25_DFSI_LDU1_VOICE1) || (frameType == P25_DFSI_LDU1_VOICE2) || (frameType == P25_DFSI_LDU2_VOICE10) || (frameType == P25_DFSI_LDU2_VOICE11) ) { + return true; + } else { + return false; + } +} + +bool MotFullRateVoice::isVoice2or11() +{ + if ( (frameType == P25_DFSI_LDU1_VOICE2) || (frameType == P25_DFSI_LDU2_VOICE11) ) { + return true; + } else { + return false; + } +} + +bool MotFullRateVoice::isVoice9or18() +{ + if ( (frameType == P25_DFSI_LDU1_VOICE9) || (frameType == P25_DFSI_LDU2_VOICE18) ) { + return true; + } else { + return false; + } +} + +// --------------------------------------------------------------------------- +// Motorola start of stream frame (10 bytes long) +// --------------------------------------------------------------------------- + +MotStartOfStream::MotStartOfStream() +{ + rt = DISABLED; + startStop = START; + streamType = VOICE; +} + +MotStartOfStream::MotStartOfStream(uint8_t* data) +{ + decode(data); +} + +bool MotStartOfStream::decode(uint8_t* data) +{ + // Sanity check + assert(data != nullptr); + + // Get parameters + rt = (RTFlag)data[2U]; + startStop = (StartStopFlag)data[3U]; + streamType = (StreamTypeFlag)data[4U]; + + return true; +} + +void MotStartOfStream::encode(uint8_t* data) +{ + // Sanity check + assert(data != nullptr); + + // Copy data + data[0U] = P25_DFSI_MOT_START_STOP; + data[1U] = FIXED_MARKER; + data[2U] = rt; + data[3U] = startStop; + data[4U] = streamType; +} + +// --------------------------------------------------------------------------- +// Motorola start voice frame +// --------------------------------------------------------------------------- + +MotStartVoiceFrame::MotStartVoiceFrame() +{ + icw = ICW_DIU; + rssi = 0; + rssiValidity = INVALID; + nRssi = 0; + adjMM = 0; + + startOfStream = nullptr; + fullRateVoice = nullptr; +} + +MotStartVoiceFrame::MotStartVoiceFrame(uint8_t* data) +{ + decode(data); +} + +MotStartVoiceFrame::~MotStartVoiceFrame() +{ + delete startOfStream; + delete fullRateVoice; +} + +bool MotStartVoiceFrame::decode(uint8_t* data) +{ + assert(data != nullptr); + + // Create a new startOfStream + startOfStream = new MotStartOfStream(); + + // Create a buffer to decode the start record skipping the 10th byte (adjMM) + uint8_t startBuffer[startOfStream->LENGTH]; + ::memset(startBuffer, 0x00U, startOfStream->LENGTH); + ::memcpy(startBuffer, data, 9U); + + // Decode start of stream + startOfStream->decode(startBuffer); + + // Decode the full rate voice frames + fullRateVoice = new MotFullRateVoice(); + uint8_t voiceBuffer[fullRateVoice->SHORTENED_LENGTH]; + ::memset(voiceBuffer, 0x00U, fullRateVoice->SHORTENED_LENGTH); + voiceBuffer[0U] = data[0U]; + ::memcpy(voiceBuffer + 1U, data + 10U, fullRateVoice->SHORTENED_LENGTH - 1); + fullRateVoice->decode(voiceBuffer, true); + + // Get rest of data + icw = (ICWFlag)data[5U]; + rssi = data[6U]; + rssiValidity = (RssiValidityFlag)data[7U]; + nRssi = data[8U]; + adjMM = data[9U]; + + return true; +} + +void MotStartVoiceFrame::encode(uint8_t* data) +{ + // Sanity checks + assert(data != nullptr); + assert(startOfStream != nullptr); + assert(fullRateVoice != nullptr); + + // Encode start of stream + if (startOfStream != nullptr) { + uint8_t buffer[startOfStream->LENGTH]; + startOfStream->encode(buffer); + // Copy to data array (skipping first and last bytes) + ::memcpy(data + 1U, buffer + 1U, startOfStream->LENGTH - 2); + } + + // Encode full rate voice + if (fullRateVoice != nullptr) { + uint8_t buffer[fullRateVoice->SHORTENED_LENGTH]; + fullRateVoice->encode(buffer, true); + data[0U] = fullRateVoice->frameType; + ::memcpy(data + 10U, buffer + 1U, fullRateVoice->SHORTENED_LENGTH - 1); + } + + // Copy the rest + data[5U] = icw; + data[6U] = rssi; + data[7U] = rssiValidity; + data[8U] = nRssi; + data[9U] = adjMM; +} + +// --------------------------------------------------------------------------- +// Motorola voice header 1 +// --------------------------------------------------------------------------- + +MotVoiceHeader1::MotVoiceHeader1() +{ + icw = ICW_DIU; + rssi = 0; + rssiValidity = INVALID; + nRssi = 0; + + startOfStream = nullptr; + header = new uint8_t[HCW_LENGTH]; + ::memset(header, 0x00U, HCW_LENGTH); +} + +MotVoiceHeader1::MotVoiceHeader1(uint8_t* data) +{ + decode(data); +} + +MotVoiceHeader1::~MotVoiceHeader1() +{ + delete startOfStream; + delete[] header; +} + +bool MotVoiceHeader1::decode(uint8_t* data) +{ + assert(data != nullptr); + + // Create a start of stream + startOfStream = new MotStartOfStream(); + uint8_t buffer[startOfStream->LENGTH]; + ::memset(buffer, 0x00U, startOfStream->LENGTH); + // We copy the bytes from [1:4] + ::memcpy(buffer + 1U, data + 1U, 4); + startOfStream->decode(buffer); + + // Decode the other stuff + icw = (ICWFlag)data[5U]; + rssi = data[6U]; + rssiValidity = (RssiValidityFlag)data[7U]; + nRssi = data[8U]; + + // Our header includes the trailing source and check bytes + header = new uint8_t[HCW_LENGTH]; + ::memset(header, 0x00U, HCW_LENGTH); + ::memcpy(header, data + 9U, HCW_LENGTH); + + return true; +} + +void MotVoiceHeader1::encode(uint8_t* data) +{ + assert(data != nullptr); + assert(startOfStream != nullptr); + + data[0U] = P25_DFSI_MOT_VHDR_1; + + if (startOfStream != nullptr) { + uint8_t buffer[startOfStream->LENGTH]; + ::memset(buffer, 0x00U, startOfStream->LENGTH); + startOfStream->encode(buffer); + // Copy the 4 start record bytes from the start of stream frame + ::memcpy(data + 1U, buffer + 1U, 4U); + } + + data[5U] = icw; + data[6U] = rssi; + data[7U] = (uint8_t)rssiValidity; + data[8U] = nRssi; + + // Our header includes the trailing source and check bytes + if (header != nullptr) { + ::memcpy(data + 9U, header, HCW_LENGTH); + } +} + +// --------------------------------------------------------------------------- +// Motorola voice header 2 +// --------------------------------------------------------------------------- + +MotVoiceHeader2::MotVoiceHeader2() +{ + source = SOURCE_QUANTAR; + + header = new uint8_t[HCW_LENGTH]; + ::memset(header, 0x00U, HCW_LENGTH); +} + +MotVoiceHeader2::MotVoiceHeader2(uint8_t* data) +{ + decode(data); +} + +MotVoiceHeader2::~MotVoiceHeader2() +{ + delete[] header; +} + +bool MotVoiceHeader2::decode(uint8_t* data) +{ + assert(data != nullptr); + + source = (SourceFlag)data[21]; + + header = new uint8_t[HCW_LENGTH]; + ::memset(header, 0x00U, HCW_LENGTH); + ::memcpy(header, data + 1U, HCW_LENGTH); + + return true; +} + +void MotVoiceHeader2::encode(uint8_t* data) +{ + assert(data != nullptr); + + data[0U] = P25_DFSI_MOT_VHDR_2; + + if (header != nullptr) { + ::memcpy(data + 1U, header, HCW_LENGTH); + } + + data[LENGTH - 1U] = (uint8_t)source; +} \ No newline at end of file diff --git a/src/dfsi/rtp/MotRtpFrames.h b/src/dfsi/rtp/MotRtpFrames.h new file mode 100644 index 00000000..f441ce85 --- /dev/null +++ b/src/dfsi/rtp/MotRtpFrames.h @@ -0,0 +1,169 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* +*/ +#if !defined(__MOT_RTP_FRAMES_H__) +#define __MOT_RTP_FRAMES_H__ + +#include "Defines.h" +#include "common/Defines.h" +#include "common/Log.h" +#include "common/Utils.h" + +namespace p25 +{ + namespace dfsi + { + enum RTFlag { + ENABLED = 0x02U, + DISABLED = 0x04U + }; + + enum StartStopFlag { + START = 0x0CU, + STOP = 0x25U + }; + + enum StreamTypeFlag { + VOICE = 0x0BU + }; + + enum RssiValidityFlag { + INVALID = 0x00U, + VALID = 0x1A + }; + + enum SourceFlag { + SOURCE_DIU = 0x00U, + SOURCE_QUANTAR = 0x02U + }; + + enum ICWFlag { + ICW_DIU = 0x00U, + ICW_QUANTAR = 0x1B + }; + + // --------------------------------------------------------------------------- + // Classes for Motorola-specific DFSI Frames (aka "THE" manufacturer) + // --------------------------------------------------------------------------- + + class MotFullRateVoice { + public: + // Frame information + static const uint8_t LENGTH = 17; + static const uint8_t SHORTENED_LENGTH = 13; + static const uint8_t ADDITIONAL_LENGTH = 4; + static const uint8_t IMBE_BUF_LEN = 11; + + uint8_t frameType; + uint8_t* imbeData; + uint8_t* additionalData; + SourceFlag source; + + MotFullRateVoice(); + MotFullRateVoice(uint8_t* data); + ~MotFullRateVoice(); + + uint32_t size(); + bool decode(uint8_t* data, bool shortened = false); + void encode(uint8_t* data, bool shortened = false); + private: + bool isVoice1or2or10or11(); + bool isVoice2or11(); + bool isVoice9or18(); + }; + + class MotStartOfStream { + public: + static const uint8_t LENGTH = 10; + static const uint8_t FIXED_MARKER = 0x02; + + uint8_t marker = FIXED_MARKER; + + RTFlag rt; + StartStopFlag startStop; + StreamTypeFlag streamType; + + MotStartOfStream(); + MotStartOfStream(uint8_t* data); + + bool decode(uint8_t* data); + void encode(uint8_t* data); + }; + + class MotStartVoiceFrame { + public: + static const uint8_t LENGTH = 22; + + ICWFlag icw; + uint8_t rssi; + RssiValidityFlag rssiValidity; + uint8_t nRssi; + uint8_t adjMM; + + MotStartOfStream* startOfStream; + MotFullRateVoice* fullRateVoice; + + MotStartVoiceFrame(); + MotStartVoiceFrame(uint8_t* data); + ~MotStartVoiceFrame(); + + bool decode(uint8_t* data); + void encode(uint8_t* data); + }; + + class MotVoiceHeader1 { + public: + static const uint8_t LENGTH = 30; + static const uint8_t HCW_LENGTH = 21; + + ICWFlag icw; + uint8_t rssi; + RssiValidityFlag rssiValidity; + uint8_t nRssi; + + uint8_t* header; + MotStartOfStream* startOfStream; + + MotVoiceHeader1(); + MotVoiceHeader1(uint8_t* data); + ~MotVoiceHeader1(); + + bool decode(uint8_t* data); + void encode(uint8_t* data); + }; + + class MotVoiceHeader2 { + public: + static const uint8_t LENGTH = 22; + static const uint8_t HCW_LENGTH = 20; + + ICWFlag icw; + uint8_t rssi; + RssiValidityFlag rssiValidity; + uint8_t nRssi; + MotStartOfStream startOfStream; + SourceFlag source; + + uint8_t* header; + + MotVoiceHeader2(); + MotVoiceHeader2(uint8_t* data); + ~MotVoiceHeader2(); + + bool decode(uint8_t* data); + void encode(uint8_t* data); + }; + } +} + +#endif // __MOT_RTP_FRAMES_H__ \ No newline at end of file diff --git a/src/dfsi/rtp/MotStartOfStream.cpp b/src/dfsi/rtp/MotStartOfStream.cpp new file mode 100644 index 00000000..ee79ad68 --- /dev/null +++ b/src/dfsi/rtp/MotStartOfStream.cpp @@ -0,0 +1,81 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ + +#include "rtp/MotStartOfStream.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/Utils.h" +#include "common/Log.h" + +#include +#include + +using namespace p25; +using namespace dfsi; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/// +/// Initializes a instance of the MotStartOfStream class. +/// +MotStartOfStream::MotStartOfStream() +{ + rt = DISABLED; + startStop = START; + streamType = VOICE; +} + +/// +/// Initializes a instance of the MotStartOfStream class. +/// +/// +MotStartOfStream::MotStartOfStream(uint8_t* data) +{ + decode(data); +} + +/// +/// Decode a start of stream frame. +/// +/// +/// +bool MotStartOfStream::decode(const uint8_t* data) +{ + assert(data != nullptr); + + // Get parameters + rt = (RTFlag)data[2U]; + startStop = (StartStopFlag)data[3U]; + streamType = (StreamTypeFlag)data[4U]; + + return true; +} + +/// +/// Encode a start of stream frame. +/// +/// +void MotStartOfStream::encode(uint8_t* data) +{ + assert(data != nullptr); + + // Copy data + data[0U] = P25_DFSI_MOT_START_STOP; + data[1U] = FIXED_MARKER; + data[2U] = rt; + data[3U] = startStop; + data[4U] = streamType; +} diff --git a/src/dfsi/rtp/MotStartOfStream.h b/src/dfsi/rtp/MotStartOfStream.h new file mode 100644 index 00000000..5466665c --- /dev/null +++ b/src/dfsi/rtp/MotStartOfStream.h @@ -0,0 +1,67 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ +#if !defined(__MOT_START_OF_STREAM_H__) +#define __MOT_START_OF_STREAM_H__ + +#include "Defines.h" +#include "common/Defines.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "rtp/RtpDefines.h" + +namespace p25 +{ + namespace dfsi + { + // --------------------------------------------------------------------------- + // Class Declaration + // Implements a P25 Motorola start of stream packet. + // + // Byte 0 1 2 3 + // Bit 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Fixed Mark | RT Mode Flag | Start/Stop | Type Flag | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Reserved | + // + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | | + // +-+-+-+-+-+-+-+-+ + // --------------------------------------------------------------------------- + + class HOST_SW_API MotStartOfStream { + public: + static const uint8_t LENGTH = 10; + static const uint8_t FIXED_MARKER = 0x02; + + uint8_t marker = FIXED_MARKER; + + RTFlag rt; + StartStopFlag startStop; + StreamTypeFlag streamType; + + /// Initializes a copy instance of the MotStartOfStream class. + MotStartOfStream(); + /// Initializes a copy instance of the MotStartOfStream class. + MotStartOfStream(uint8_t* data); + + /// Decode a start of stream frame. + bool decode(const uint8_t* data); + /// Encode a start of stream frame. + void encode(uint8_t* data); + }; + } // namespace dfsi +} // namespace p25 + +#endif // __MOT_START_OF_STREAM_H__ \ No newline at end of file diff --git a/src/dfsi/rtp/MotStartVoiceFrame.cpp b/src/dfsi/rtp/MotStartVoiceFrame.cpp new file mode 100644 index 00000000..61840061 --- /dev/null +++ b/src/dfsi/rtp/MotStartVoiceFrame.cpp @@ -0,0 +1,134 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ + +#include "rtp/MotStartVoiceFrame.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/Utils.h" +#include "common/Log.h" + +#include +#include + +using namespace p25; +using namespace dfsi; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/// +/// Initializes a instance of the MotStartVoiceFrame class. +/// +MotStartVoiceFrame::MotStartVoiceFrame() +{ + icw = ICW_DIU; + rssi = 0; + rssiValidity = INVALID; + nRssi = 0; + adjMM = 0; + + startOfStream = nullptr; + fullRateVoice = nullptr; +} + +/// +/// Initializes a instance of the MotStartVoiceFrame class. +/// +/// +MotStartVoiceFrame::MotStartVoiceFrame(uint8_t* data) +{ + decode(data); +} + +/// +/// Finalizes a instance of the MotStartVoiceFrame class. +/// +MotStartVoiceFrame::~MotStartVoiceFrame() +{ + delete startOfStream; + delete fullRateVoice; +} + +/// +/// Decode a start voice frame. +/// +/// +/// +bool MotStartVoiceFrame::decode(const uint8_t* data) +{ + assert(data != nullptr); + + // Create a new startOfStream + startOfStream = new MotStartOfStream(); + + // Create a buffer to decode the start record skipping the 10th byte (adjMM) + uint8_t startBuffer[startOfStream->LENGTH]; + ::memset(startBuffer, 0x00U, startOfStream->LENGTH); + ::memcpy(startBuffer, data, 9U); + + // Decode start of stream + startOfStream->decode(startBuffer); + + // Decode the full rate voice frames + fullRateVoice = new MotFullRateVoice(); + uint8_t voiceBuffer[fullRateVoice->SHORTENED_LENGTH]; + ::memset(voiceBuffer, 0x00U, fullRateVoice->SHORTENED_LENGTH); + voiceBuffer[0U] = data[0U]; + ::memcpy(voiceBuffer + 1U, data + 10U, fullRateVoice->SHORTENED_LENGTH - 1); + fullRateVoice->decode(voiceBuffer, true); + + // Get rest of data + icw = (ICWFlag)data[5U]; + rssi = data[6U]; + rssiValidity = (RssiValidityFlag)data[7U]; + nRssi = data[8U]; + adjMM = data[9U]; + + return true; +} + +/// +/// Encode a start voice frame. +/// +/// +void MotStartVoiceFrame::encode(uint8_t* data) +{ + assert(data != nullptr); + assert(startOfStream != nullptr); + assert(fullRateVoice != nullptr); + + // Encode start of stream + if (startOfStream != nullptr) { + uint8_t buffer[startOfStream->LENGTH]; + startOfStream->encode(buffer); + // Copy to data array (skipping first and last bytes) + ::memcpy(data + 1U, buffer + 1U, startOfStream->LENGTH - 2); + } + + // Encode full rate voice + if (fullRateVoice != nullptr) { + uint8_t buffer[fullRateVoice->SHORTENED_LENGTH]; + fullRateVoice->encode(buffer, true); + data[0U] = fullRateVoice->frameType; + ::memcpy(data + 10U, buffer + 1U, fullRateVoice->SHORTENED_LENGTH - 1); + } + + // Copy the rest + data[5U] = icw; + data[6U] = rssi; + data[7U] = rssiValidity; + data[8U] = nRssi; + data[9U] = adjMM; +} diff --git a/src/dfsi/rtp/MotStartVoiceFrame.h b/src/dfsi/rtp/MotStartVoiceFrame.h new file mode 100644 index 00000000..df4a8c64 --- /dev/null +++ b/src/dfsi/rtp/MotStartVoiceFrame.h @@ -0,0 +1,79 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ +#if !defined(__MOT_START_VOICE_FRAME_H__) +#define __MOT_START_VOICE_FRAME_H__ + +#include "Defines.h" +#include "common/Defines.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "rtp/RtpDefines.h" +#include "rtp/MotStartOfStream.h" +#include "rtp/MotFullRateVoice.h" + +namespace p25 +{ + namespace dfsi + { + // --------------------------------------------------------------------------- + // Class Declaration + // Implements a P25 Motorola voice frame 1/10 start. + // + // Byte 0 1 2 3 + // Bit 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Encoded Motorola Start of Stream | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | ICW Flag ? | RSSI | RSSI Valid | RSSI | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Adj MM ? | Full Rate Voice Frame | + // +-+-+-+-+-+-+-+-+ + + // | | + // + + + // | | + // + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | | + // +=+=+=+=+=+=+=+=+ + // --------------------------------------------------------------------------- + + class HOST_SW_API MotStartVoiceFrame { + public: + static const uint8_t LENGTH = 22; + + ICWFlag icw; + uint8_t rssi; + RssiValidityFlag rssiValidity; + uint8_t nRssi; + uint8_t adjMM; + + MotStartOfStream* startOfStream; + MotFullRateVoice* fullRateVoice; + + /// Initializes a copy instance of the MotStartVoiceFrame class. + MotStartVoiceFrame(); + /// Initializes a copy instance of the MotStartVoiceFrame class. + MotStartVoiceFrame(uint8_t* data); + /// Finalizes a instance of the MotStartVoiceFrame class. + ~MotStartVoiceFrame(); + + /// Decode a start voice frame. + bool decode(const uint8_t* data); + /// Encode a start voice frame. + void encode(uint8_t* data); + }; + } // namespace dfsi +} // namespace p25 + +#endif // __MOT_START_VOICE_FRAME_H__ \ No newline at end of file diff --git a/src/dfsi/rtp/MotVoiceHeader1.cpp b/src/dfsi/rtp/MotVoiceHeader1.cpp new file mode 100644 index 00000000..b9b41814 --- /dev/null +++ b/src/dfsi/rtp/MotVoiceHeader1.cpp @@ -0,0 +1,123 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ + +#include "rtp/MotVoiceHeader1.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/Utils.h" +#include "common/Log.h" + +#include +#include + +using namespace p25; +using namespace dfsi; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/// +/// Initializes a instance of the MotVoiceHeader1 class. +/// +MotVoiceHeader1::MotVoiceHeader1() +{ + icw = ICW_DIU; + rssi = 0; + rssiValidity = INVALID; + nRssi = 0; + + startOfStream = nullptr; + header = new uint8_t[HCW_LENGTH]; + ::memset(header, 0x00U, HCW_LENGTH); +} + +/// +/// Initializes a instance of the MotVoiceHeader1 class. +/// +/// +MotVoiceHeader1::MotVoiceHeader1(uint8_t* data) +{ + decode(data); +} + +/// +/// Finalizes a instance of the MotVoiceHeader1 class. +/// +MotVoiceHeader1::~MotVoiceHeader1() +{ + delete startOfStream; + delete[] header; +} + +/// +/// Decode a voice header 1 frame. +/// +/// +/// +bool MotVoiceHeader1::decode(const uint8_t* data) +{ + assert(data != nullptr); + + // Create a start of stream + startOfStream = new MotStartOfStream(); + uint8_t buffer[startOfStream->LENGTH]; + ::memset(buffer, 0x00U, startOfStream->LENGTH); + // We copy the bytes from [1:4] + ::memcpy(buffer + 1U, data + 1U, 4); + startOfStream->decode(buffer); + + // Decode the other stuff + icw = (ICWFlag)data[5U]; + rssi = data[6U]; + rssiValidity = (RssiValidityFlag)data[7U]; + nRssi = data[8U]; + + // Our header includes the trailing source and check bytes + header = new uint8_t[HCW_LENGTH]; + ::memset(header, 0x00U, HCW_LENGTH); + ::memcpy(header, data + 9U, HCW_LENGTH); + + return true; +} + +/// +/// Encode a voice header 1 frame. +/// +/// +void MotVoiceHeader1::encode(uint8_t* data) +{ + assert(data != nullptr); + assert(startOfStream != nullptr); + + data[0U] = P25_DFSI_MOT_VHDR_1; + + if (startOfStream != nullptr) { + uint8_t buffer[startOfStream->LENGTH]; + ::memset(buffer, 0x00U, startOfStream->LENGTH); + startOfStream->encode(buffer); + // Copy the 4 start record bytes from the start of stream frame + ::memcpy(data + 1U, buffer + 1U, 4U); + } + + data[5U] = icw; + data[6U] = rssi; + data[7U] = (uint8_t)rssiValidity; + data[8U] = nRssi; + + // Our header includes the trailing source and check bytes + if (header != nullptr) { + ::memcpy(data + 9U, header, HCW_LENGTH); + } +} diff --git a/src/dfsi/rtp/MotVoiceHeader1.h b/src/dfsi/rtp/MotVoiceHeader1.h new file mode 100644 index 00000000..bbb1d25d --- /dev/null +++ b/src/dfsi/rtp/MotVoiceHeader1.h @@ -0,0 +1,82 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ +#if !defined(__MOT_VOICE_HEADER_1_H__) +#define __MOT_VOICE_HEADER_1_H__ + +#include "Defines.h" +#include "common/Defines.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "rtp/RtpDefines.h" +#include "rtp/MotStartOfStream.h" + +namespace p25 +{ + namespace dfsi + { + // --------------------------------------------------------------------------- + // Class Declaration + // Implements a P25 Motorola voice header frame 1. + // + // Byte 0 1 2 3 + // Bit 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Encoded Motorola Start of Stream | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | ICW Flag ? | RSSI | RSSI Valid | RSSI | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Header Control Word | + // + + + // | | + // + + + // | | + // + + + // | | + // + + + // | | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Src Flag | + // +-+-+-+-+-+-+-+-+ + // --------------------------------------------------------------------------- + + class HOST_SW_API MotVoiceHeader1 { + public: + static const uint8_t LENGTH = 30; + static const uint8_t HCW_LENGTH = 21; + + ICWFlag icw; + uint8_t rssi; + RssiValidityFlag rssiValidity; + uint8_t nRssi; + + uint8_t* header; + MotStartOfStream* startOfStream; + + /// Initializes a copy instance of the MotVoiceHeader1 class. + MotVoiceHeader1(); + /// Initializes a copy instance of the MotVoiceHeader1 class. + MotVoiceHeader1(uint8_t* data); + /// Finalizes a instance of the MotVoiceHeader1 class. + ~MotVoiceHeader1(); + + /// Decode a voice header 1 frame. + bool decode(const uint8_t* data); + /// Encode a voice header 1 frame. + void encode(uint8_t* data); + }; + } // namespace dfsi +} // namespace p25 + +#endif // __MOT_VOICE_HEADER_1_H__ \ No newline at end of file diff --git a/src/dfsi/rtp/MotVoiceHeader2.cpp b/src/dfsi/rtp/MotVoiceHeader2.cpp new file mode 100644 index 00000000..1073f303 --- /dev/null +++ b/src/dfsi/rtp/MotVoiceHeader2.cpp @@ -0,0 +1,92 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ + +#include "rtp/MotVoiceHeader2.h" +#include "common/p25/dfsi/DFSIDefines.h" +#include "common/Utils.h" +#include "common/Log.h" + +#include +#include + +using namespace p25; +using namespace dfsi; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/// +/// Initializes a instance of the MotVoiceHeader2 class. +/// +MotVoiceHeader2::MotVoiceHeader2() +{ + source = SOURCE_QUANTAR; + + header = new uint8_t[HCW_LENGTH]; + ::memset(header, 0x00U, HCW_LENGTH); +} + +/// +/// Initializes a instance of the MotVoiceHeader2 class. +/// +/// +MotVoiceHeader2::MotVoiceHeader2(uint8_t* data) +{ + decode(data); +} + +/// +/// Finalizes a instance of the MotVoiceHeader2 class. +/// +MotVoiceHeader2::~MotVoiceHeader2() +{ + delete[] header; +} + +/// +/// Decode a voice header 2 frame. +/// +/// +/// +bool MotVoiceHeader2::decode(const uint8_t* data) +{ + assert(data != nullptr); + + source = (SourceFlag)data[21]; + + header = new uint8_t[HCW_LENGTH]; + ::memset(header, 0x00U, HCW_LENGTH); + ::memcpy(header, data + 1U, HCW_LENGTH); + + return true; +} + +/// +/// Encode a voice header 2 frame. +/// +/// +void MotVoiceHeader2::encode(uint8_t* data) +{ + assert(data != nullptr); + + data[0U] = P25_DFSI_MOT_VHDR_2; + + if (header != nullptr) { + ::memcpy(data + 1U, header, HCW_LENGTH); + } + + data[LENGTH - 1U] = (uint8_t)source; +} diff --git a/src/dfsi/rtp/MotVoiceHeader2.h b/src/dfsi/rtp/MotVoiceHeader2.h new file mode 100644 index 00000000..ebe3b435 --- /dev/null +++ b/src/dfsi/rtp/MotVoiceHeader2.h @@ -0,0 +1,79 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ +#if !defined(__MOT_VOICE_HEADER_2_H__) +#define __MOT_VOICE_HEADER_2_H__ + +#include "Defines.h" +#include "common/Defines.h" +#include "common/Log.h" +#include "common/Utils.h" +#include "rtp/RtpDefines.h" +#include "rtp/MotStartOfStream.h" + +namespace p25 +{ + namespace dfsi + { + // --------------------------------------------------------------------------- + // Class Declaration + // Implements a P25 Motorola voice header frame 2. + // + // Byte 0 1 2 3 + // Bit 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | Header Control Word | + // + + + // | | + // + + + // | | + // + + + // | | + // + + + // | | + // + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // | | Reserved | + // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + // --------------------------------------------------------------------------- + + class HOST_SW_API MotVoiceHeader2 { + public: + static const uint8_t LENGTH = 22; + static const uint8_t HCW_LENGTH = 20; + + ICWFlag icw; + uint8_t rssi; + RssiValidityFlag rssiValidity; + uint8_t nRssi; + MotStartOfStream startOfStream; + SourceFlag source; + + uint8_t* header; + + /// Initializes a copy instance of the MotVoiceHeader2 class. + MotVoiceHeader2(); + /// Initializes a copy instance of the MotVoiceHeader2 class. + MotVoiceHeader2(uint8_t* data); + /// Finalizes a instance of the MotVoiceHeader2 class. + ~MotVoiceHeader2(); + + /// Decode a voice header 2 frame. + bool decode(const uint8_t* data); + /// Encode a voice header 2 frame. + void encode(uint8_t* data); + }; + } // namespace dfsi +} // namespace p25 + +#endif // __MOT_VOICE_HEADER_2_H__ \ No newline at end of file diff --git a/src/dfsi/rtp/RtpDefines.h b/src/dfsi/rtp/RtpDefines.h new file mode 100644 index 00000000..70f4b235 --- /dev/null +++ b/src/dfsi/rtp/RtpDefines.h @@ -0,0 +1,77 @@ +// 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. +* +* @package DVM / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ +#if !defined(__RTP_DEFINES_H__) +#define __RTP_DEFINES_H__ + +#include "common/Defines.h" + +namespace p25 +{ + namespace dfsi + { + // --------------------------------------------------------------------------- + // Constants + // --------------------------------------------------------------------------- + + /// + /// + /// + enum RTFlag { + ENABLED = 0x02U, + DISABLED = 0x04U + }; + + /// + /// + /// + enum StartStopFlag { + START = 0x0CU, + STOP = 0x25U + }; + + /// + /// + /// + enum StreamTypeFlag { + VOICE = 0x0BU + }; + + /// + /// + /// + enum RssiValidityFlag { + INVALID = 0x00U, + VALID = 0x1A + }; + + /// + /// + /// + enum SourceFlag { + SOURCE_DIU = 0x00U, + SOURCE_QUANTAR = 0x02U + }; + + /// + /// + /// + enum ICWFlag { + ICW_DIU = 0x00U, + ICW_QUANTAR = 0x1B + }; + } // namespace dfsi +} // namespace p25 + +#endif // __RTP_DEFINES_H__ diff --git a/src/dfsi/rtp/RtpFrames.h b/src/dfsi/rtp/RtpFrames.h new file mode 100644 index 00000000..808595e8 --- /dev/null +++ b/src/dfsi/rtp/RtpFrames.h @@ -0,0 +1,26 @@ +// 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 / DFSI peer application +* @derivedfrom MMDVMHost (https://github.com/g4klx/MMDVMHost) +* @license GPLv2 License (https://opensource.org/licenses/GPL-2.0) +* +* Copyright (C) 2024 Patrick McDonnell, W3AXL +* Copyright (C) 2024 Bryan Biedenkapp, N2PLL +* +*/ +#if !defined(__RTP_FRAMES_H__) +#define __RTP_FRAMES_H__ + +#include "Defines.h" + +#include "rtp/MotFullRateVoice.h" +#include "rtp/MotStartOfStream.h" +#include "rtp/MotStartVoiceFrame.h" +#include "rtp/MotVoiceHeader1.h" +#include "rtp/MotVoiceHeader2.h" + +#endif // __RTP_FRAMES_H__ \ No newline at end of file diff --git a/src/fw/hotspot b/src/fw/hotspot index 8d6611ef..88224479 160000 --- a/src/fw/hotspot +++ b/src/fw/hotspot @@ -1 +1 @@ -Subproject commit 8d6611ef703abd74d08bce2afe46d0e09676383a +Subproject commit 882244795b9bec1f2c86d596d4ab4ecd4346c8b9 diff --git a/src/fw/modem b/src/fw/modem index d79b1d7b..a9ac06bc 160000 --- a/src/fw/modem +++ b/src/fw/modem @@ -1 +1 @@ -Subproject commit d79b1d7b8d4b175be86cd0e5dc844c6ca5ae6733 +Subproject commit a9ac06bc01309355b0da3530a5009c2c42cf5de5 diff --git a/src/host/modem/port/UARTPort.cpp b/src/host/modem/port/UARTPort.cpp index 18b0b0e2..e9091916 100644 --- a/src/host/modem/port/UARTPort.cpp +++ b/src/host/modem/port/UARTPort.cpp @@ -167,7 +167,7 @@ int UARTPort::write(const uint8_t* buffer, uint32_t length) n = ::write(m_fd, buffer + ptr, length - ptr); if (n < 0) { if (errno != EAGAIN) { - ::LogError(LOG_HOST, "Error returned from write(), errno=%d", errno); + ::LogError(LOG_HOST, "Error returned from write(), errno=%d (%s)", errno, strerror(errno)); return -1; } }