From da2f6130a89c587650d34df369fc8be9cb943620 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Sat, 13 Jul 2024 07:54:48 -0400 Subject: [PATCH] [NOTICE: this commit is *EXPERIMENTAL* and implements *very* early data support, it is expected to be buggy, incomplete or broken] implement VTUN interface (fne0) on the dvmfne (this requires dvmfne to be run as root on Linux system [untested on non-Linux] to create the tun interface); implement VIFace random MAC generation; implement P25 TIA-102.BAEB SCEP ARP (SCEP is always used for conventional data) on dvmfne; refactor P25 packet status bits (this may break things else where, beware!); properly implement P25 status bit flipping on inbound channel activity; implement functionality in the P25 and DMR DataHeader classes to calculate the proper raw PDU frame length; implement dvmfne ARP table creation and maintainence (this is buggy and requires more implementation and test); implement dvmfne VTUN -> PDU and PDU -> VTUN IP traffic forwarding (this is incomplete and requires more implementation and test); correct several issues on dvmhost p25::packet::Data with overlapped buffers; --- configs/fne-config.example.yml | 21 +- src/common/dmr/data/DataHeader.cpp | 16 + src/common/dmr/data/DataHeader.h | 5 + src/common/network/viface/VIFace.cpp | 29 +- src/common/network/viface/VIFace.h | 4 + src/common/p25/P25Defines.h | 7 + src/common/p25/P25Utils.cpp | 45 +- src/common/p25/P25Utils.h | 26 +- src/common/p25/data/DataHeader.cpp | 24 + src/common/p25/data/DataHeader.h | 5 + src/fne/HostFNE.cpp | 97 ++++ src/fne/HostFNE.h | 19 + src/fne/network/callhandler/TagP25Data.cpp | 11 +- src/fne/network/callhandler/TagP25Data.h | 7 + .../callhandler/packetdata/P25PacketData.cpp | 435 +++++++++++++++++- .../callhandler/packetdata/P25PacketData.h | 69 +++ src/host/p25/Control.cpp | 2 +- src/host/p25/packet/ControlSignaling.cpp | 28 +- src/host/p25/packet/ControlSignaling.h | 2 + src/host/p25/packet/Data.cpp | 181 +++++--- src/host/p25/packet/Data.h | 16 +- src/host/p25/packet/Voice.cpp | 39 +- src/host/p25/packet/Voice.h | 2 + src/host/setup/SetupApplication.h | 2 +- 24 files changed, 951 insertions(+), 141 deletions(-) diff --git a/configs/fne-config.example.yml b/configs/fne-config.example.yml index b18aa374..e053cf7f 100644 --- a/configs/fne-config.example.yml +++ b/configs/fne-config.example.yml @@ -225,4 +225,23 @@ system: # Full path to the white/blacklist file. file: peer_list.dat # Amount of time between updates of white/blacklist file. (minutes) - time: 2 \ No newline at end of file + time: 2 + +# +# Packet Data Virtual Network Tunnel Configuration +# +vtun: + # Flag indicating the virtual network tunnel is enabled. + # (If this is enabled, dvmfne must be run as root to create the TUN interface.) + enabled: false + # Operational mode for the network tunnel (dmr or p25). + digitalMode: p25 + + # Kernel Interface Name + interfaceName: fne0 + # IP address of the tunnel network interface + address: 192.168.1.254 + # Netmask of the tunnel network interface + netmask: 255.255.255.0 + # Broadcast address of the tunnel network interface + broadcast: 192.168.1.255 diff --git a/src/common/dmr/data/DataHeader.cpp b/src/common/dmr/data/DataHeader.cpp index abc44d48..a7a6be9d 100644 --- a/src/common/dmr/data/DataHeader.cpp +++ b/src/common/dmr/data/DataHeader.cpp @@ -418,6 +418,22 @@ uint32_t DataHeader::getPacketLength() const } } +/* Gets the total length in bytes of entire PDU. */ + +uint32_t DataHeader::getPDULength() const +{ + if (m_DPF == DPF::RESPONSE) { + return 0U; // responses have no packet length as they are header only + } + + if (m_DPF == DPF::CONFIRMED_DATA) { + return DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES * m_blocksToFollow; + } + else { + return DMR_PDU_UNCONFIRMED_LENGTH_BYTES * m_blocksToFollow; + } +} + /* Gets the raw header data. */ uint32_t DataHeader::getData(uint8_t* buffer) const diff --git a/src/common/dmr/data/DataHeader.h b/src/common/dmr/data/DataHeader.h index b05f40c5..231cc5ea 100644 --- a/src/common/dmr/data/DataHeader.h +++ b/src/common/dmr/data/DataHeader.h @@ -75,6 +75,11 @@ namespace dmr * @returns uint32_t Total length of packet in bytes. */ uint32_t getPacketLength() const; + /** + * @brief Gets the total length in bytes of entire PDU. + * @returns uint32_t Total length of PDU in bytes. + */ + uint32_t getPDULength() const; /** * @brief Gets the raw header data. diff --git a/src/common/network/viface/VIFace.cpp b/src/common/network/viface/VIFace.cpp index 61fc9e4e..9d455e9b 100644 --- a/src/common/network/viface/VIFace.cpp +++ b/src/common/network/viface/VIFace.cpp @@ -19,6 +19,7 @@ using namespace network::viface; #include #include #include +#include #include #include @@ -332,7 +333,7 @@ VIFace::VIFace(std::string name, bool tap, int id) : throw std::runtime_error("Unable to create IPv4 socket channel to the NET kernel."); } - // Set id + // set id if (id < 0) { m_id = m_idSeq; m_idSeq++; @@ -543,6 +544,30 @@ bool VIFace::write(const uint8_t* buffer, uint32_t length, ssize_t* lenWritten) return result; } +/* Set the MAC address of the virtual interface to a random value. */ + +void VIFace::setRandomMAC() +{ + // generate random MAC + std::random_device rd; + std::mt19937 mt(rd()); + + std::ostringstream addr; + addr << std::hex << std::setfill('0'); + for (int i = 0; i < 6; i++) { + uint8_t macByte = 0U; + std::uniform_int_distribution dist(1U, 254U); + macByte = dist(mt); + + addr << std::setw(2) << (uint8_t)(0xFFU & macByte); + if (i != 5) { + addr << ":"; + } + } + + m_mac = addr.str(); +} + /* Set the MAC address of the virtual interface. */ void VIFace::setMAC(std::string mac) @@ -567,7 +592,7 @@ std::string VIFace::getMAC() const std::ostringstream addr; addr << std::hex << std::setfill('0'); for (int i = 0; i < 6; i++) { - addr << std::setw(2) << (uint8_t)(0xFF & ifr.ifr_hwaddr.sa_data[i]); + addr << std::setw(2) << (uint8_t)(0xFFU & ifr.ifr_hwaddr.sa_data[i]); if (i != 5) { addr << ":"; } diff --git a/src/common/network/viface/VIFace.h b/src/common/network/viface/VIFace.h index d0f5f419..5b482aa2 100644 --- a/src/common/network/viface/VIFace.h +++ b/src/common/network/viface/VIFace.h @@ -121,6 +121,10 @@ namespace network */ bool write(const uint8_t* buffer, uint32_t length, ssize_t* lenWritten = nullptr); + /** + * @brief Set the MAC address of the virtual interface to a random value. + */ + void setRandomMAC(); /** * @brief Set the MAC address of the virtual interface. * diff --git a/src/common/p25/P25Defines.h b/src/common/p25/P25Defines.h index 649beefd..acd8cba8 100644 --- a/src/common/p25/P25Defines.h +++ b/src/common/p25/P25Defines.h @@ -92,6 +92,10 @@ namespace p25 const uint32_t P25_PDU_CONFIRMED_DATA_LENGTH_BYTES = 16U; const uint32_t P25_PDU_UNCONFIRMED_LENGTH_BYTES = 12U; + const uint32_t P25_PDU_ARP_PCKT_LENGTH = 22U; + const uint8_t P25_PDU_ARP_HW_ADDR_LENGTH = 3U; + const uint8_t P25_PDU_ARP_PROTO_ADDR_LENGTH = 4U; + const uint32_t P25_PDU_FEC_LENGTH_BYTES = 25U; const uint32_t P25_PDU_FEC_LENGTH_BITS = P25_PDU_FEC_LENGTH_BYTES * 8U - 4U; // Trellis is actually 196 bits @@ -169,6 +173,9 @@ namespace p25 /** @brief All-call Talkgroup ID */ const uint32_t TGID_ALL = 0xFFFFU; + /** @brief ARP Hardware Type */ + const uint16_t P25_PDU_ARP_CAI_TYPE = 0x21U; + /** @brief ARP Request */ const uint8_t P25_PDU_ARP_REQUEST = 0x01U; /** @brief ARP Reply */ diff --git a/src/common/p25/P25Utils.cpp b/src/common/p25/P25Utils.cpp index 0e73fd3b..f4efa990 100644 --- a/src/common/p25/P25Utils.cpp +++ b/src/common/p25/P25Utils.cpp @@ -21,9 +21,9 @@ using namespace p25::defines; // Static Class Members // --------------------------------------------------------------------------- -/* Helper to set the busy status bits on P25 frame data. */ +/* Helper to set the status bits on P25 frame data. */ -void P25Utils::setBusyBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2) +void P25Utils::setStatusBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2) { assert(data != nullptr); @@ -31,37 +31,60 @@ void P25Utils::setBusyBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2) WRITE_BIT(data, ssOffset + 1U, b2); } -/* Helper to add the busy status bits on P25 frame data. */ +/* Helper to add the status bits on P25 frame data. */ -void P25Utils::addBusyBits(uint8_t* data, uint32_t length, bool b1, bool b2) +void P25Utils::addStatusBits(uint8_t* data, uint32_t length, bool inbound, bool control) { assert(data != nullptr); // insert the "10" (Unknown, use for inbound or outbound) status bits for (uint32_t ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += P25_SS_INCREMENT) { uint32_t ss1Pos = ss0Pos + 1U; - WRITE_BIT(data, ss0Pos, true); // 1 - WRITE_BIT(data, ss1Pos, false); // 0 + WRITE_BIT(data, ss0Pos, true); // 1 + WRITE_BIT(data, ss1Pos, false); // 0 } // interleave the requested status bits (every other) for (uint32_t ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += (P25_SS_INCREMENT * 2U)) { uint32_t ss1Pos = ss0Pos + 1U; - WRITE_BIT(data, ss0Pos, b1); - WRITE_BIT(data, ss1Pos, b2); + if (inbound) { + WRITE_BIT(data, ss0Pos, false); // 0 + WRITE_BIT(data, ss1Pos, true); // 1 + } else { + if (control) { + WRITE_BIT(data, ss0Pos, true); // 1 + WRITE_BIT(data, ss1Pos, false); // 0 + } else { + WRITE_BIT(data, ss0Pos, true); // 1 + WRITE_BIT(data, ss1Pos, true); // 1 + } + } } } /* Helper to add the idle status bits on P25 frame data. */ -void P25Utils::addIdleBits(uint8_t* data, uint32_t length, bool b1, bool b2) +void P25Utils::addIdleStatusBits(uint8_t* data, uint32_t length) +{ + assert(data != nullptr); + + for (uint32_t ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += (P25_SS_INCREMENT * 5U)) { + uint32_t ss1Pos = ss0Pos + 1U; + WRITE_BIT(data, ss0Pos, true); // 1 + WRITE_BIT(data, ss1Pos, false); // 0 + } +} + +/* Helper to add the trunk start slot status bits on P25 frame data. */ + +void P25Utils::addTrunkSlotStatusBits(uint8_t* data, uint32_t length) { assert(data != nullptr); for (uint32_t ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += (P25_SS_INCREMENT * 5U)) { uint32_t ss1Pos = ss0Pos + 1U; - WRITE_BIT(data, ss0Pos, b1); - WRITE_BIT(data, ss1Pos, b2); + WRITE_BIT(data, ss0Pos, true); // 1 + WRITE_BIT(data, ss1Pos, true); // 1 } } diff --git a/src/common/p25/P25Utils.h b/src/common/p25/P25Utils.h index b9678c4f..bfad54d1 100644 --- a/src/common/p25/P25Utils.h +++ b/src/common/p25/P25Utils.h @@ -118,29 +118,37 @@ namespace p25 } /** - * @brief Helper to set the busy status bits on P25 frame data. + * @brief Helper to set the status bits on P25 frame data. * @param data P25 frame data buffer. * @param ssOffset * @param b1 Status Bit 1 * @param b2 Status Bit 2 */ - static void setBusyBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2); + static void setStatusBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2); /** - * @brief Helper to add the busy status bits on P25 frame data. + * @brief Helper to add the status bits on P25 frame data. + * This appropriately sets the status bits for the P25 frame, starting with 1,0 and then + * properly setting 0,1 for inbound traffic, or 1,1 for idle (or 1,0 for control channels). * @param data P25 frame data buffer. * @param length - * @param b1 Status Bit 1 - * @param b2 Status Bit 2 + * @param inbound Flag indicating inbound channel is busy. + * @param control Flag indicating the channel is a control channel. */ - static void addBusyBits(uint8_t* data, uint32_t length, bool b1, bool b2); + static void addStatusBits(uint8_t *data, uint32_t length, bool inbound, bool control = false); /** * @brief Helper to add the idle status bits on P25 frame data. + * This sets the status bits to 1,0 interleaved every 5th status bit pair. + * @param data P25 frame data buffer. + * @param length + */ + static void addIdleStatusBits(uint8_t* data, uint32_t length); + /** + * @brief Helper to add the trunk start slot status bits on P25 frame data. + * This sets the status bits to 1,1 interleaved every 5th status bit pair. * @param data P25 frame data buffer. * @param length - * @param b1 Status Bit 1 - * @param b2 Status Bit 2 */ - static void addIdleBits(uint8_t* data, uint32_t length, bool b1, bool b2); + static void addTrunkSlotStatusBits(uint8_t* data, uint32_t length); /** * @brief Decode bit interleaving. diff --git a/src/common/p25/data/DataHeader.cpp b/src/common/p25/data/DataHeader.cpp index 3c06b728..5baa3e09 100644 --- a/src/common/p25/data/DataHeader.cpp +++ b/src/common/p25/data/DataHeader.cpp @@ -430,6 +430,22 @@ uint32_t DataHeader::getPacketLength() const } } +/* Gets the total length in bytes of entire PDU. */ + +uint32_t DataHeader::getPDULength() const +{ + if (m_fmt == PDUFormatType::RSP) { + return 0U; // responses have no packet length as they are header only + } + + if (m_fmt == PDUFormatType::CONFIRMED) { + return P25_PDU_CONFIRMED_DATA_LENGTH_BYTES * m_blocksToFollow; + } + else { + return P25_PDU_UNCONFIRMED_LENGTH_BYTES * m_blocksToFollow; + } +} + /* Gets the raw header data. */ uint32_t DataHeader::getData(uint8_t* buffer) const @@ -457,6 +473,14 @@ uint32_t DataHeader::getExtAddrData(uint8_t* buffer) const void DataHeader::calculateLength(uint32_t packetLength) { uint32_t len = packetLength + 4U; // packet length + CRC32 + if (m_fmt == PDUFormatType::UNCONFIRMED && m_sap == PDUSAP::EXT_ADDR) { + len += P25_PDU_HEADER_LENGTH_BYTES; + } + + if (m_fmt == PDUFormatType::CONFIRMED && m_sap == PDUSAP::EXT_ADDR) { + len += 4U; + } + uint32_t blockLen = (m_fmt == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; if (len > blockLen) { diff --git a/src/common/p25/data/DataHeader.h b/src/common/p25/data/DataHeader.h index aefeebe4..4d2117f6 100644 --- a/src/common/p25/data/DataHeader.h +++ b/src/common/p25/data/DataHeader.h @@ -86,6 +86,11 @@ namespace p25 * @returns uint32_t Total length of packet in bytes. */ uint32_t getPacketLength() const; + /** + * @brief Gets the total length in bytes of entire PDU. + * @returns uint32_t Total length of PDU in bytes. + */ + uint32_t getPDULength() const; /** * @brief Gets the raw header data. diff --git a/src/fne/HostFNE.cpp b/src/fne/HostFNE.cpp index 8a2f530d..f6b9c5ee 100644 --- a/src/fne/HostFNE.cpp +++ b/src/fne/HostFNE.cpp @@ -21,6 +21,7 @@ #include "FNEMain.h" using namespace network; +using namespace network::viface; using namespace lookups; #include @@ -35,6 +36,7 @@ using namespace lookups; // --------------------------------------------------------------------------- #define IDLE_WARMUP_MS 5U +#define DEFAULT_MTU_SIZE 496 // --------------------------------------------------------------------------- // Public Class Members @@ -47,6 +49,9 @@ HostFNE::HostFNE(const std::string& confFile) : m_conf(), m_network(nullptr), m_diagNetwork(nullptr), + m_vtunEnabled(false), + m_packetDataMode(PacketDataMode::PROJECT25), + m_tun(nullptr), m_dmrEnabled(false), m_p25Enabled(false), m_nxdnEnabled(false), @@ -171,6 +176,11 @@ int HostFNE::run() if (!ret) return EXIT_FAILURE; + // initialize virtual networking + ret = createVirtualNetworking(); + if (!ret) + return EXIT_FAILURE; + ::LogInfoEx(LOG_HOST, "FNE is up and running"); StopWatch stopWatch; @@ -207,6 +217,38 @@ int HostFNE::run() diagNetworkLoop.setName("dvmfne:diag-network-loop"); } + ThreadFunc vtunLoop([&, this]() { + if (g_killed) + return; + + if (!m_vtunEnabled) + return; + + if (m_tun != nullptr) { + while (!g_killed) { + uint8_t packet[DEFAULT_MTU_SIZE]; + ::memset(packet, 0x00U, DEFAULT_MTU_SIZE); + + ssize_t len = m_tun->read(packet); + if (len > 0) { + switch (m_packetDataMode) { + case PacketDataMode::DMR: + // TODO: not supported yet + break; + + case PacketDataMode::PROJECT25: + m_network->p25TrafficHandler()->processPacketFrame(packet, DEFAULT_MTU_SIZE); + break; + } + } + + Thread::sleep(5U); + } + } + }); + vtunLoop.run(); + vtunLoop.setName("dvmfne:vtun-loop"); + // main execution loop while (!g_killed) { uint32_t ms = stopWatch.elapsed(); @@ -250,6 +292,10 @@ int HostFNE::run() diagNetworkLoop.wait(); } + if (m_vtunEnabled) { + vtunLoop.wait(); + } + if (m_network != nullptr) { m_network->close(); delete m_network; @@ -287,6 +333,14 @@ int HostFNE::run() delete m_peerListLookup; } + if (m_tun != nullptr) { + if (m_tun->isUp()) { + m_tun->down(); + } + + delete m_tun; + } + return EXIT_SUCCESS; } @@ -676,6 +730,49 @@ bool HostFNE::createPeerNetworks() return true; } +/* Initializes virtual networking. */ + +bool HostFNE::createVirtualNetworking() +{ + yaml::Node vtunConf = m_conf["vtun"]; + + bool vtunEnabled = vtunConf["enabled"].as(false); + if (vtunEnabled) { + m_vtunEnabled = vtunEnabled; + + std::string vtunName = vtunConf["interfaceName"].as("fne0"); + std::string ipv4Address = vtunConf["address"].as("192.168.1.254"); + std::string ipv4Netmask = vtunConf["netmask"].as("255.255.255.0"); + std::string ipv4Broadcast = vtunConf["broadcast"].as("192.168.1.255"); + std::string packetDataModeStr = vtunConf["digitalMode"].as("p25"); + + if (packetDataModeStr == "dmr") { + m_packetDataMode = PacketDataMode::DMR; + } else { + m_packetDataMode = PacketDataMode::PROJECT25; + } + + LogInfo("Virtual Network Parameters"); + LogInfo(" Interface Name: %s", vtunName.c_str()); + LogInfo(" Address: %s", ipv4Address.c_str()); + LogInfo(" Netmask: %s", ipv4Netmask.c_str()); + LogInfo(" Broadcast: %s", ipv4Broadcast.c_str()); + LogInfo(" Digital Packet Mode: %s", packetDataModeStr.c_str()); + + // initialize networking + m_tun = new VIFace(vtunName, false); + + m_tun->setIPv4(ipv4Address); + m_tun->setIPv4Netmask(ipv4Netmask); + m_tun->setIPv4Broadcast(ipv4Broadcast); + m_tun->setMTU(DEFAULT_MTU_SIZE); + + m_tun->up(); + } + + return true; +} + /* Processes any peer network traffic. */ void HostFNE::processPeer(network::PeerNetwork* peerNetwork) diff --git a/src/fne/HostFNE.h b/src/fne/HostFNE.h index 8598b849..e5838b01 100644 --- a/src/fne/HostFNE.h +++ b/src/fne/HostFNE.h @@ -20,6 +20,7 @@ #include "common/lookups/RadioIdLookup.h" #include "common/lookups/TalkgroupRulesLookup.h" #include "common/lookups/PeerListLookup.h" +#include "common/network/viface/VIFace.h" #include "common/yaml/Yaml.h" #include "common/Timer.h" #include "network/FNENetwork.h" @@ -51,6 +52,14 @@ namespace network { namespace callhandler { class HOST_SW_API TagNXDNData; } } */ class HOST_SW_API HostFNE { public: + /** + * @brief Virtual Network Packet Data Digital Mode + */ + enum PacketDataMode { + DMR, //! Digital Mobile Radio + PROJECT25 //! Project 25 + }; + /** * @brief Initializes a new instance of the HostFNE class. * @param confFile Full-path to the configuration file. @@ -80,6 +89,10 @@ private: network::FNENetwork* m_network; network::DiagNetwork* m_diagNetwork; + bool m_vtunEnabled; + PacketDataMode m_packetDataMode; + network::viface::VIFace* m_tun; + bool m_dmrEnabled; bool m_p25Enabled; bool m_nxdnEnabled; @@ -123,6 +136,12 @@ private: */ bool createPeerNetworks(); + /** + * @brief Initializes virtual networking. + * @returns bool True, if network connectivity was initialized, otherwise false. + */ + bool createVirtualNetworking(); + /** * @brief Processes any peer network traffic. * @param peerNetwork Instance of PeerNetwork to process traffic for. diff --git a/src/fne/network/callhandler/TagP25Data.cpp b/src/fne/network/callhandler/TagP25Data.cpp index 117743e5..fa53b2f6 100644 --- a/src/fne/network/callhandler/TagP25Data.cpp +++ b/src/fne/network/callhandler/TagP25Data.cpp @@ -421,6 +421,13 @@ bool TagP25Data::processGrantReq(uint32_t srcId, uint32_t dstId, bool unitToUnit return true; } +/* Process a data frame from the virtual IP network. */ + +void TagP25Data::processPacketFrame(const uint8_t* data, uint32_t len) +{ + m_packetData->processPacketFrame(data, len); +} + /* Helper to playback a parrot frame to the network. */ void TagP25Data::playbackParrot() @@ -1224,10 +1231,10 @@ void TagP25Data::write_TSDU(uint32_t peerId, lc::TSBK* tsbk) tsbk->encode(data); // Add busy bits - P25Utils::addBusyBits(data, P25_TSDU_FRAME_LENGTH_BYTES, true, false); + P25Utils::addStatusBits(data, P25_TSDU_FRAME_LENGTH_BYTES, false); // Set first busy bits to 1,1 - P25Utils::setBusyBits(data, P25_SS0_START, true, true); + P25Utils::setStatusBits(data, P25_SS0_START, true, true); if (m_debug) { LogDebug(LOG_RF, P25_TSDU_STR ", lco = $%02X, mfId = $%02X, lastBlock = %u, AIV = %u, EX = %u, srcId = %u, dstId = %u, sysId = $%03X, netId = $%05X", diff --git a/src/fne/network/callhandler/TagP25Data.h b/src/fne/network/callhandler/TagP25Data.h index 00421335..d4852e31 100644 --- a/src/fne/network/callhandler/TagP25Data.h +++ b/src/fne/network/callhandler/TagP25Data.h @@ -79,6 +79,13 @@ namespace network */ bool processGrantReq(uint32_t srcId, uint32_t dstId, bool unitToUnit, uint32_t peerId, uint16_t pktSeq, uint32_t streamId); + /** + * @brief Process a data frame from the virtual IP network. + * @param data Network data buffer. + * @param len Length of data. + */ + void processPacketFrame(const uint8_t* data, uint32_t len); + /** * @brief Helper to playback a parrot frame to the network. */ diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.cpp b/src/fne/network/callhandler/packetdata/P25PacketData.cpp index bf2df369..7def016e 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.cpp +++ b/src/fne/network/callhandler/packetdata/P25PacketData.cpp @@ -8,6 +8,7 @@ * */ #include "fne/Defines.h" +#include "common/p25/sndcp/SNDCPFactory.h" #include "common/p25/Sync.h" #include "common/edac/CRC.h" #include "common/Clock.h" @@ -24,10 +25,19 @@ using namespace network::callhandler; using namespace network::callhandler::packetdata; using namespace p25; using namespace p25::defines; +using namespace p25::sndcp; #include #include +#include + +// --------------------------------------------------------------------------- +// Static Class Members +// --------------------------------------------------------------------------- + +uint8_t P25PacketData::m_sendSeqNo = 0U; + // --------------------------------------------------------------------------- // Constants // --------------------------------------------------------------------------- @@ -43,7 +53,9 @@ const uint8_t DATA_CALL_COLL_TIMEOUT = 60U; P25PacketData::P25PacketData(FNENetwork* network, TagP25Data* tag, bool debug) : m_network(network), m_tag(tag), + m_dataFrames(), m_status(), + m_arpTable(), m_debug(debug) { assert(network != nullptr); @@ -121,6 +133,15 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee m_status[peerId] = status; + // a PDU header only with no blocks to follow is usually a response header + if (status->header.getBlocksToFollow() == 0U) { + dispatch(peerId); + + delete status; + m_status.erase(peerId); + return true; + } + LogMessage(LOG_NET, "P25, Data Call Start, peer = %u, llId = %u, streamId = %u, external = %u", peerId, status->llId, streamId, external); return true; } else { @@ -154,18 +175,6 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee } } - // a PDU header only with no blocks to follow is usually a response header - if (status->header.getBlocksToFollow() == 0U) { - dispatch(peerId); - - LogMessage(LOG_NET, "P25, Data Call End, peer = %u, srcId = %u, dstId = %u, streamId = %u, external = %u", - peerId, status->header.getSrcLLId(), status->header.getLLId(), streamId, external); - - delete status; - m_status.erase(peerId); - return true; - } - ::memcpy(status->netPDU + status->dataOffset, data + 24U, blockLength); status->dataOffset += blockLength; status->netPDUCount++; @@ -306,6 +315,61 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee return true; } +/* Process a data frame from the virtual IP network. */ + +void P25PacketData::processPacketFrame(const uint8_t* data, uint32_t len, bool alreadyQueued) +{ + struct ip* ipHeader = (struct ip*)data; + + char srcIp[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(ipHeader->ip_src), srcIp, INET_ADDRSTRLEN); + + char dstIp[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(ipHeader->ip_dst), dstIp, INET_ADDRSTRLEN); + + uint8_t proto = ipHeader->ip_p; + uint16_t pktLen = Utils::reverseEndian(ipHeader->ip_len); // bryanb: this could be problematic on different endianness + + LogMessage(LOG_NET, "P25, VTUN -> PDU IP Data, srcIp = %s, dstIp = %s, pktLen = %u, proto = %02X", srcIp, dstIp, pktLen, proto); + + Utils::dump(1U, "P25PacketData::processPacketFrame() packet", data, pktLen); + + uint32_t dstLlId = getLLIdAddress(Utils::reverseEndian(ipHeader->ip_dst.s_addr)); + if (dstLlId == 0U) { + LogMessage(LOG_NET, "P25, no ARP entry for, dstIp = %s", dstIp); + write_PDU_ARP(Utils::reverseEndian(ipHeader->ip_dst.s_addr)); + + // TODO: queue data frame for retransmission? + } + else { + // assemble a P25 PDU frame header for transport... + data::DataHeader rspHeader = data::DataHeader(); + rspHeader.setFormat(PDUFormatType::CONFIRMED); + rspHeader.setMFId(MFG_STANDARD); + rspHeader.setAckNeeded(true); + rspHeader.setOutbound(true); + rspHeader.setSAP(PDUSAP::EXT_ADDR); + rspHeader.setLLId(dstLlId); + rspHeader.setBlocksToFollow(1U); + + rspHeader.setEXSAP(PDUSAP::PACKET_DATA); + rspHeader.setSrcLLId(WUID_FNE); + + rspHeader.calculateLength(pktLen); + uint32_t pduLength = rspHeader.getPDULength(); + + uint8_t pduUserData[pduLength]; + ::memset(pduUserData, 0x00U, pduLength); + ::memcpy(pduUserData + 4U, data, pktLen); + + Utils::dump(1U, "P25PacketData::processPacketFrame() pduUserData", pduUserData, pduLength); + + dispatchUserFrameToFNE(rspHeader, true, pduUserData); + } + + Thread::sleep(1750); +} + // --------------------------------------------------------------------------- // Private Class Members // --------------------------------------------------------------------------- @@ -325,9 +389,100 @@ void P25PacketData::dispatch(uint32_t peerId) if (m_network->m_dumpDataPacket && status->dataBlockCnt > 0U) { Utils::dump(1U, "PDU Packet", status->pduUserData, status->pduUserDataLength); + } + + uint8_t sap = (status->extendedAddress) ? status->header.getEXSAP() : status->header.getSAP(); + + // don't dispatch SNDCP control, conventional data registration or ARP + if (sap != PDUSAP::SNDCP_CTRL_DATA && sap != PDUSAP::CONV_DATA_REG && + sap != PDUSAP::ARP) { + dispatchToFNE(peerId); + } + + // handle standard P25 service access points + switch (sap) { + case PDUSAP::ARP: + { + // is the host virtual tunneling enabled? + if (!m_network->m_host->m_vtunEnabled) + break; + + uint32_t fneIPv4 = __IP_FROM_STR(m_network->m_host->m_tun->getIPv4()); + + uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH]; + ::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH); + ::memcpy(arpPacket, status->pduUserData + 12U, P25_PDU_ARP_PCKT_LENGTH); + + uint16_t opcode = __GET_UINT16B(arpPacket, 6U); + uint32_t srcHWAddr = __GET_UINT16(arpPacket, 8U); + uint32_t srcProtoAddr = __GET_UINT32(arpPacket, 11U); + uint32_t tgtHWAddr = __GET_UINT16(arpPacket, 15U); + uint32_t tgtProtoAddr = __GET_UINT32(arpPacket, 18U); + + if (opcode == P25_PDU_ARP_REQUEST) { + LogMessage(LOG_NET, P25_PDU_STR ", ARP request, who has %s? tell %s (%u)", __IP_FROM_UINT(tgtProtoAddr).c_str(), __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr); + if (fneIPv4 == tgtProtoAddr) { + write_PDU_ARP_Reply(fneIPv4, srcHWAddr, srcProtoAddr, WUID_FNE); + } else { + write_PDU_ARP_Reply(tgtProtoAddr, srcHWAddr, srcProtoAddr); + } + } else if (opcode == P25_PDU_ARP_REPLY) { + LogMessage(LOG_NET, P25_PDU_STR ", ARP reply, %s is at %u", __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr); + if (fneIPv4 == srcProtoAddr) { + LogWarning(LOG_NET, P25_PDU_STR ", ARP reply, %u is trying to masquerade as us...", srcHWAddr); + } else { + m_arpTable[srcHWAddr] = srcProtoAddr; + } + } + } + break; + case PDUSAP::PACKET_DATA: + { + // is the host virtual tunneling enabled? + if (!m_network->m_host->m_vtunEnabled) + break; + + int dataPktOffset = 0U; + if (status->header.getFormat() == PDUFormatType::CONFIRMED && status->extendedAddress) + dataPktOffset = 4U; + if (status->header.getFormat() == PDUFormatType::UNCONFIRMED && status->extendedAddress) + dataPktOffset = 12U; + + struct ip* ipHeader = (struct ip*)(status->pduUserData + dataPktOffset); + + char srcIp[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(ipHeader->ip_src), srcIp, INET_ADDRSTRLEN); + + char dstIp[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(ipHeader->ip_dst), dstIp, INET_ADDRSTRLEN); + + uint8_t proto = ipHeader->ip_p; + uint16_t pktLen = Utils::reverseEndian(ipHeader->ip_len); // bryanb: this could be problematic on different endianness + + LogMessage(LOG_NET, "P25, PDU -> VTUN, IP Data, srcIp = %s, dstIp = %s, pktLen = %u, proto = %02X", srcIp, dstIp, pktLen, proto); + + uint8_t ipFrame[pktLen]; + ::memset(ipFrame, 0x00U, pktLen); + ::memcpy(ipFrame, status->pduUserData + dataPktOffset, pktLen); + + Utils::dump(1U, "P25PacketData::dispatch() ipFrame", ipFrame, pktLen); + + if (!m_network->m_host->m_tun->write(ipFrame, pktLen)) { + LogError(LOG_NET, P25_PDU_STR ", failed to write IP frame to virtual tunnel, len %u", pktLen); + } } + break; + case PDUSAP::SNDCP_CTRL_DATA: + { + LogMessage(LOG_NET, P25_PDU_STR ", SNDCP_CTRL_DATA (SNDCP Control Data), blocksToFollow = %u", + status->header.getBlocksToFollow()); - dispatchToFNE(peerId); + processSNDCPControl(status); + } + break; + default: + break; + } } /* Helper to dispatch PDU user data back to the FNE network. */ @@ -389,6 +544,198 @@ void P25PacketData::dispatchToFNE(uint32_t peerId) } } +/* Helper to dispatch PDU user data back to the FNE network. */ + +void P25PacketData::dispatchUserFrameToFNE(p25::data::DataHeader& dataHeader, bool extendedAddress, uint8_t* pduUserData) +{ + uint32_t srcId = (extendedAddress) ? dataHeader.getSrcLLId() : dataHeader.getLLId(); + uint32_t dstId = dataHeader.getLLId(); + + dataHeader.setNs(m_sendSeqNo); + ++m_sendSeqNo; + if (m_sendSeqNo > 7U) + m_sendSeqNo = 0U; + + // repeat traffic to the connected peers + if (m_network->m_peers.size() > 0U) { + uint32_t i = 0U; + for (auto peer : m_network->m_peers) { + // every 2 peers flush the queue + if (i % 2U == 0U) { + m_network->m_frameQueue->flushQueue(); + } + + write_PDU_User(peer.first, nullptr, dataHeader, extendedAddress, pduUserData, true); + if (m_network->m_debug) { + LogDebug(LOG_NET, "P25, dstPeer = %u, duid = $%02X, srcId = %u, dstId = %u", + peer.first, DUID::PDU, srcId, dstId); + } + + i++; + } + m_network->m_frameQueue->flushQueue(); + } + + // repeat traffic to external peers + if (m_network->m_host->m_peerNetworks.size() > 0U) { + for (auto peer : m_network->m_host->m_peerNetworks) { + uint32_t dstPeerId = peer.second->getPeerId(); + + write_PDU_User(dstPeerId, peer.second, dataHeader, extendedAddress, pduUserData); + if (m_network->m_debug) { + LogDebug(LOG_NET, "P25, dstPeer = %u, duid = $%02X, srcId = %u, dstId = %u", + dstPeerId, DUID::PDU, srcId, dstId); + } + } + } +} + +/* Helper used to process SNDCP control data from PDU data. */ + +bool P25PacketData::processSNDCPControl(RxStatus* status) +{ + std::unique_ptr packet = SNDCPFactory::create(status->pduUserData); + if (packet == nullptr) { + LogWarning(LOG_NET, P25_PDU_STR ", undecodable SNDCP packet"); + return false; + } + + uint32_t llId = status->header.getLLId(); + + switch (packet->getPDUType()) { + case SNDCP_PDUType::ACT_TDS_CTX: + { + SNDCPCtxActRequest* isp = static_cast(packet.get()); + LogMessage(LOG_NET, P25_PDU_STR ", SNDCP context activation request, llId = %u, nsapi = %u, ipAddr = %s, nat = $%02X, dsut = $%02X, mdpco = $%02X", llId, + isp->getNSAPI(), __IP_FROM_UINT(isp->getIPAddress()).c_str(), isp->getNAT(), isp->getDSUT(), isp->getMDPCO()); + + m_arpTable[llId] = isp->getIPAddress(); + } + break; + + case SNDCP_PDUType::DEACT_TDS_CTX_REQ: + { + SNDCPCtxDeactivation* isp = static_cast(packet.get()); + LogMessage(LOG_NET, P25_PDU_STR ", SNDCP context deactivation request, llId = %u, deactType = %02X", llId, + isp->getDeactType()); + + m_arpTable.erase(llId); + } + break; + + default: + break; + } // switch (packet->getPDUType()) + + return true; +} + +/* Helper write ARP request to the network. */ + +void P25PacketData::write_PDU_ARP(uint32_t addr) +{ + if (!m_network->m_host->m_vtunEnabled) + return; + + uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH]; + ::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH); + + __SET_UINT16B(P25_PDU_ARP_CAI_TYPE, arpPacket, 0U); // Hardware Address Type + __SET_UINT16B(PDUSAP::PACKET_DATA, arpPacket, 2U); // Protocol Address Type + arpPacket[4U] = P25_PDU_ARP_HW_ADDR_LENGTH; // Hardware Address Length + arpPacket[5U] = P25_PDU_ARP_PROTO_ADDR_LENGTH; // Protocol Address Length + __SET_UINT16B(P25_PDU_ARP_REQUEST, arpPacket, 6U); // Opcode + + __SET_UINT16(WUID_FNE, arpPacket, 8U); // Sender Hardware Address + + std::string fneIPv4 = m_network->m_host->m_tun->getIPv4(); + __SET_UINT32(__IP_FROM_STR(fneIPv4), arpPacket, 11U); // Sender Protocol Address + + __SET_UINT32(addr, arpPacket, 18U); // Target Protocol Address + + Utils::dump(1U, "P25PacketData::write_PDU_ARP() arpPacket", arpPacket, P25_PDU_ARP_PCKT_LENGTH); + + LogMessage(LOG_NET, P25_PDU_STR ", ARP request, who has %s? tell %s (%u)", __IP_FROM_UINT(addr).c_str(), fneIPv4.c_str(), WUID_FNE); + + // assemble a P25 PDU frame header for transport... + data::DataHeader rspHeader = data::DataHeader(); + rspHeader.setFormat(PDUFormatType::UNCONFIRMED); + rspHeader.setMFId(MFG_STANDARD); + rspHeader.setAckNeeded(false); + rspHeader.setOutbound(true); + rspHeader.setSAP(PDUSAP::EXT_ADDR); + rspHeader.setLLId(WUID_ALL); + rspHeader.setBlocksToFollow(1U); + + rspHeader.setEXSAP(PDUSAP::ARP); + rspHeader.setSrcLLId(WUID_FNE); + + rspHeader.calculateLength(P25_PDU_ARP_PCKT_LENGTH); + uint32_t pduLength = rspHeader.getPDULength(); + + uint8_t pduUserData[pduLength]; + ::memset(pduUserData, 0x00U, pduLength); + ::memcpy(pduUserData + P25_PDU_HEADER_LENGTH_BYTES, arpPacket, P25_PDU_ARP_PCKT_LENGTH); + + dispatchUserFrameToFNE(rspHeader, true, pduUserData); +} + +/* Helper write ARP reply to the network. */ + +void P25PacketData::write_PDU_ARP_Reply(uint32_t targetAddr, uint32_t requestorLlid, uint32_t requestorAddr, uint32_t targetLlid) +{ + if (!m_network->m_host->m_vtunEnabled) + return; + + uint32_t tgtLlid = getLLIdAddress(targetAddr); + if (targetLlid != 0U) { + tgtLlid = targetLlid; // forcibly override + } + if (tgtLlid == 0U) + return; + + uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH]; + ::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH); + + __SET_UINT16B(P25_PDU_ARP_CAI_TYPE, arpPacket, 0U); // Hardware Address Type + __SET_UINT16B(PDUSAP::PACKET_DATA, arpPacket, 2U); // Protocol Address Type + arpPacket[4U] = P25_PDU_ARP_HW_ADDR_LENGTH; // Hardware Address Length + arpPacket[5U] = P25_PDU_ARP_PROTO_ADDR_LENGTH; // Protocol Address Length + __SET_UINT16B(P25_PDU_ARP_REPLY, arpPacket, 6U); // Opcode + + __SET_UINT16(tgtLlid, arpPacket, 8U); // Sender Hardware Address + __SET_UINT32(targetAddr, arpPacket, 11U); // Sender Protocol Address + + __SET_UINT16(requestorLlid, arpPacket, 15U); // Requestor Hardware Address + __SET_UINT32(requestorAddr, arpPacket, 18U); // Requestor Protocol Address + + Utils::dump(1U, "P25PacketData::write_PDU_ARP_Reply() arpPacket", arpPacket, P25_PDU_ARP_PCKT_LENGTH); + + LogMessage(LOG_NET, P25_PDU_STR ", ARP reply, %s is at %u", __IP_FROM_UINT(targetAddr).c_str(), tgtLlid); + + // assemble a P25 PDU frame header for transport... + data::DataHeader rspHeader = data::DataHeader(); + rspHeader.setFormat(PDUFormatType::UNCONFIRMED); + rspHeader.setMFId(MFG_STANDARD); + rspHeader.setAckNeeded(false); + rspHeader.setOutbound(true); + rspHeader.setSAP(PDUSAP::EXT_ADDR); + rspHeader.setLLId(WUID_ALL); + rspHeader.setBlocksToFollow(1U); + + rspHeader.setEXSAP(PDUSAP::ARP); + rspHeader.setSrcLLId(WUID_FNE); + + rspHeader.calculateLength(P25_PDU_ARP_PCKT_LENGTH); + uint32_t pduLength = rspHeader.getPDULength(); + + uint8_t pduUserData[pduLength]; + ::memset(pduUserData, 0x00U, pduLength); + ::memcpy(pduUserData + P25_PDU_HEADER_LENGTH_BYTES, arpPacket, P25_PDU_ARP_PCKT_LENGTH); + + dispatchUserFrameToFNE(rspHeader, true, pduUserData); +} + /* Helper to write user data as a P25 PDU packet. */ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNet, data::DataHeader& dataHeader, @@ -414,7 +761,7 @@ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNe writeNetwork(peerId, peerNet, dataHeader, 0U, buffer, P25_PDU_FEC_LENGTH_BYTES, pktSeq, streamId, queueOnly); ++pktSeq; - uint32_t packetLength = (dataHeader.getPacketLength() + dataHeader.getPadLength() + 4U); + uint32_t packetLength = dataHeader.getPDULength(); if (blocksToFollow > 0U) { uint32_t dataOffset = 0U; @@ -430,12 +777,11 @@ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNe ++pktSeq; dataOffset += P25_PDU_HEADER_LENGTH_BYTES; - packetLength += P25_PDU_HEADER_LENGTH_BYTES; blocksToFollow--; networkBlock++; - LogMessage(LOG_RF, P25_PDU_STR ", OSP, extended address, sap = $%02X, srcLlId = %u", + LogMessage(LOG_NET, P25_PDU_STR ", OSP, extended address, sap = $%02X, srcLlId = %u", dataHeader.getEXSAP(), dataHeader.getSrcLLId()); } @@ -443,7 +789,7 @@ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNe if ((dataHeader.getFormat() == PDUFormatType::CONFIRMED) && (dataHeader.getSAP() == PDUSAP::EXT_ADDR) && extendedAddress) { dataHeader.encodeExtAddr(pduUserData); - LogMessage(LOG_RF, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u", + LogMessage(LOG_NET, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u", dataHeader.getEXSAP(), dataHeader.getSrcLLId()); } @@ -495,3 +841,58 @@ bool P25PacketData::writeNetwork(uint32_t peerId, network::PeerNetwork* peerNet, return m_network->writePeer(peerId, { NET_FUNC::PROTOCOL, NET_SUBFUNC::PROTOCOL_SUBFUNC_P25 }, message.get(), messageLength, pktSeq, streamId, false, true); } } + +/* Helper to determine if the logical link ID has an ARP entry. */ + +bool P25PacketData::hasARPEntry(uint32_t llId) const +{ + if (llId == 0U) { + return false; + } + + // lookup ARP table entry + try { + uint32_t addr = m_arpTable.at(llId); + if (addr != 0U) { + return true; + } + else { + return false; + } + } catch (...) { + return false; + } +} + +/* Helper to get the IP address for the given logical link ID. */ + +uint32_t P25PacketData::getIPAddress(uint32_t llId) +{ + if (llId == 0U) { + return 0U; + } + + if (hasARPEntry(llId)) { + return m_arpTable[llId]; + } + + return 0U; +} + +/* Helper to get the logical link ID for the given IP address. */ + +uint32_t P25PacketData::getLLIdAddress(uint32_t addr) +{ + if (addr == 0U) { + return 0U; + } + + // lookup ARP table entry + for (auto entry : m_arpTable) { + if (entry.second == addr) { + return entry.first; + } + } + + return 0U; +} diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.h b/src/fne/network/callhandler/packetdata/P25PacketData.h index c6f73362..a067038b 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.h +++ b/src/fne/network/callhandler/packetdata/P25PacketData.h @@ -67,10 +67,30 @@ namespace network */ bool processFrame(const uint8_t* data, uint32_t len, uint32_t peerId, uint16_t pktSeq, uint32_t streamId, bool external = false); + /** + * @brief Process a data frame from the virtual IP network. + * @param data Network data buffer. + * @param len Length of data. + * @param alreaedyQueued Flag indicating the data frame being processed is already queued. + */ + void processPacketFrame(const uint8_t* data, uint32_t len, bool alreadyQueued = false); + private: FNENetwork* m_network; TagP25Data *m_tag; + static uint8_t m_sendSeqNo; + + /** + * @brief Represents a queued data frame from the VTUN. + */ + class VTUNDataFrame { + public: + uint8_t* buffer; + uint32_t bufferLen; + }; + std::deque m_dataFrames; + /** * @brief Represents the receive status of a call. */ @@ -130,6 +150,8 @@ namespace network typedef std::pair StatusMapPair; std::unordered_map m_status; + std::unordered_map m_arpTable; + bool m_debug; /** @@ -142,6 +164,34 @@ namespace network * @param peerId Peer ID. */ void dispatchToFNE(uint32_t peerId); + /** + * @brief Helper to dispatch PDU user data back to the FNE network. + * @param dataHeader Instance of a PDU data header. + * @param extendedAddress Flag indicating whether or not to extended addressing is in use. + * @param pduUserData Buffer containing user data to transmit. + */ + void dispatchUserFrameToFNE(p25::data::DataHeader& dataHeader, bool extendedAddress, uint8_t* pduUserData); + + /** + * @brief Helper used to process SNDCP control data from PDU data. + * @param status Instance of the RxStatus class. + * @returns bool True, if SNDCP control data was processed, otherwise false. + */ + bool processSNDCPControl(RxStatus* status); + + /** + * @brief Helper write ARP request to the network. + * @param addr IP Address. + */ + void write_PDU_ARP(uint32_t addr); + /** + * @brief Helper write ARP reply to the network. + * @param targetAddr Target IP Address. + * @param requestorLlid Requestor Logical Link Address. + * @param requestorAddr Requestor IP Address. + * @param targetLlId Target Logical Link Address. + */ + void write_PDU_ARP_Reply(uint32_t targetAddr, uint32_t requestorLlid, uint32_t requestorAddr, uint32_t targetLlid = 0U); /** * @brief Helper to write user data as a P25 PDU packet. @@ -167,6 +217,25 @@ namespace network */ bool writeNetwork(uint32_t peerId, network::PeerNetwork* peerNet, const p25::data::DataHeader& dataHeader, const uint8_t currentBlock, const uint8_t* data, uint32_t len, uint16_t pktSeq, uint32_t streamId, bool queueOnly = false); + + /** + * @brief Helper to determine if the logical link ID has an ARP entry. + * @param llId Logical Link Address. + * @returns bool True, if the logical link ID has an arp entry, otherwise false. + */ + bool hasARPEntry(uint32_t llId) const; + /** + * @brief Helper to get the IP address for the given logical link ID. + * @param llId Logical Link Address. + * @returns uint32_t Numerical IP address. + */ + uint32_t getIPAddress(uint32_t llId); + /** + * @brief Helper to get the logical link ID granted to the given IP address. + * @param addr Numerical IP address. + * @returns uint32_t Logical Link Address. + */ + uint32_t getLLIdAddress(uint32_t addr); }; } // namespace packetdata } // namespace callhandler diff --git a/src/host/p25/Control.cpp b/src/host/p25/Control.cpp index c7273301..bb41c502 100644 --- a/src/host/p25/Control.cpp +++ b/src/host/p25/Control.cpp @@ -1638,7 +1638,7 @@ void Control::writeRF_TDU(bool noNetwork, bool imm) m_nid.encode(data + 2U, DUID::TDU); // Add busy bits - P25Utils::addBusyBits(data + 2U, P25_TDU_FRAME_LENGTH_BITS, true, true); + P25Utils::addStatusBits(data + 2U, P25_TDU_FRAME_LENGTH_BITS, false); if (!noNetwork) m_voice->writeNetwork(data + 2U, DUID::TDU); diff --git a/src/host/p25/packet/ControlSignaling.cpp b/src/host/p25/packet/ControlSignaling.cpp index d27c95f8..8bc7f2bb 100644 --- a/src/host/p25/packet/ControlSignaling.cpp +++ b/src/host/p25/packet/ControlSignaling.cpp @@ -184,6 +184,8 @@ bool ControlSignaling::process(uint8_t* data, uint32_t len, std::unique_ptrm_rfState != RS_RF_DATA) { m_p25->m_rfState = RS_RF_DATA; } @@ -666,6 +668,7 @@ bool ControlSignaling::process(uint8_t* data, uint32_t len, std::unique_ptrwriteRF_Nulls(); } + m_inbound = false; m_p25->m_rfState = prevRfState; return true; } @@ -1201,6 +1204,7 @@ ControlSignaling::ControlSignaling(Control* p25, bool dumpTSBKData, bool debug, m_disableGrantSrcIdCheck(false), m_redundantImmediate(true), m_redundantGrant(false), + m_inbound(false), m_dumpTSBK(dumpTSBKData), m_verbose(verbose), m_debug(debug) @@ -1299,7 +1303,7 @@ void ControlSignaling::writeRF_TDULC(lc::TDULC* lc, bool noNetwork) lc->encode(data + 2U); // Add busy bits - P25Utils::addBusyBits(data + 2U, P25_TDULC_FRAME_LENGTH_BITS, true, true); + P25Utils::addStatusBits(data + 2U, P25_TDULC_FRAME_LENGTH_BITS, false); m_p25->m_rfTimeout.stop(); @@ -1338,7 +1342,7 @@ void ControlSignaling::writeNet_TDULC(lc::TDULC* lc) lc->encode(buffer + 2U); // Add busy bits - P25Utils::addBusyBits(buffer + 2U, P25_TDULC_FRAME_LENGTH_BITS, true, true); + P25Utils::addStatusBits(buffer + 2U, P25_TDULC_FRAME_LENGTH_BITS, false); m_p25->addFrame(buffer, P25_TDULC_FRAME_LENGTH_BYTES + 2U, true); @@ -1394,10 +1398,11 @@ void ControlSignaling::writeRF_TSDU_SBF(lc::TSBK* tsbk, bool noNetwork, bool for } // Add busy bits - P25Utils::addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false); + P25Utils::addStatusBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, m_inbound, true); + P25Utils::addTrunkSlotStatusBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS); // Set first busy bits to 1,1 - P25Utils::setBusyBits(data + 2U, P25_SS0_START, true, true); + P25Utils::setStatusBits(data + 2U, P25_SS0_START, true, true); if (!noNetwork) writeNetworkRF(tsbk, data + 2U, true); @@ -1455,10 +1460,11 @@ void ControlSignaling::writeNet_TSDU(lc::TSBK* tsbk) tsbk->encode(buffer + 2U); // Add busy bits - P25Utils::addBusyBits(buffer + 2U, P25_TSDU_FRAME_LENGTH_BYTES, true, false); + P25Utils::addStatusBits(buffer + 2U, P25_TSDU_FRAME_LENGTH_BYTES, false, true); + P25Utils::addTrunkSlotStatusBits(buffer + 2U, P25_TSDU_FRAME_LENGTH_BYTES); // Set first busy bits to 1,1 - P25Utils::setBusyBits(buffer + 2U, P25_SS0_START, true, true); + P25Utils::setStatusBits(buffer + 2U, P25_SS0_START, true, true); m_p25->addFrame(buffer, P25_TSDU_FRAME_LENGTH_BYTES + 2U, true); @@ -1548,10 +1554,8 @@ void ControlSignaling::writeRF_TSDU_MBF(lc::TSBK* tsbk) P25Utils::encode(tsdu, data + 2U, 114U, 720U); // Add busy bits - P25Utils::addBusyBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS, true, false); - - // Add idle bits - P25Utils::addIdleBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS, true, true); + P25Utils::addStatusBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS, m_inbound, true); + P25Utils::addTrunkSlotStatusBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS); data[0U] = modem::TAG_DATA; data[1U] = 0x00U; @@ -2855,10 +2859,10 @@ void ControlSignaling::writeNet_TSDU_From_RF(lc::TSBK* tsbk, uint8_t* data) tsbk->encode(data); // Add busy bits - P25Utils::addBusyBits(data, P25_TSDU_FRAME_LENGTH_BYTES, true, false); + P25Utils::addStatusBits(data, P25_TSDU_FRAME_LENGTH_BYTES, false); // Set first busy bits to 1,1 - P25Utils::setBusyBits(data, P25_SS0_START, true, true); + P25Utils::setStatusBits(data, P25_SS0_START, true, true); } /* Helper to automatically inhibit a source ID on a denial. */ diff --git a/src/host/p25/packet/ControlSignaling.h b/src/host/p25/packet/ControlSignaling.h index 056f0d54..62815caa 100644 --- a/src/host/p25/packet/ControlSignaling.h +++ b/src/host/p25/packet/ControlSignaling.h @@ -205,6 +205,8 @@ namespace p25 bool m_redundantImmediate; bool m_redundantGrant; + bool m_inbound; + bool m_dumpTSBK; bool m_verbose; diff --git a/src/host/p25/packet/Data.cpp b/src/host/p25/packet/Data.cpp index d6be49b4..fadd3a2d 100644 --- a/src/host/p25/packet/Data.cpp +++ b/src/host/p25/packet/Data.cpp @@ -79,6 +79,8 @@ bool Data::process(uint8_t* data, uint32_t len) // handle individual DUIDs if (duid == DUID::PDU) { + m_inbound = true; + if (m_p25->m_rfState != RS_RF_DATA) { m_rfDataHeader.reset(); m_rfExtendedAddress = false; @@ -90,8 +92,8 @@ bool Data::process(uint8_t* data, uint32_t len) m_p25->m_rfState = RS_RF_DATA; - ::memset(m_pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U); - m_pduUserDataLength = 0U; + ::memset(m_rfPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U); + m_rfPduUserDataLength = 0U; } uint32_t start = m_rfPDUCount * P25_PDU_FRAME_LENGTH_BITS; @@ -196,9 +198,9 @@ bool Data::process(uint8_t* data, uint32_t len) blocksToFollow--; // if we are using a secondary header place it in the PDU user data buffer - m_rfDataHeader.getExtAddrData(m_pduUserData + dataOffset); + m_rfDataHeader.getExtAddrData(m_rfPduUserData + dataOffset); dataOffset += P25_PDU_HEADER_LENGTH_BYTES; - m_pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES; + m_rfPduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES; } uint32_t srcId = (m_rfExtendedAddress) ? m_rfDataHeader.getSrcLLId() : m_rfDataHeader.getLLId(); @@ -214,7 +216,7 @@ bool Data::process(uint8_t* data, uint32_t len) // if the primary header has a header offset ensure data if offset by that amount if (m_rfDataHeader.getHeaderOffset() > 0U) { offset += m_rfDataHeader.getHeaderOffset() * 8; - m_pduUserDataLength -= m_rfDataHeader.getHeaderOffset(); + m_rfPduUserDataLength -= m_rfDataHeader.getHeaderOffset(); } // decode data blocks @@ -255,9 +257,9 @@ bool Data::process(uint8_t* data, uint32_t len) } } - m_rfData[i].getData(m_pduUserData + dataOffset); + m_rfData[i].getData(m_rfPduUserData + dataOffset); dataOffset += (m_rfDataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; - m_pduUserDataLength = dataOffset; + m_rfPduUserDataLength = dataOffset; // only send data blocks across the network, if we're not an AMBT, // an RSP or a registration service @@ -277,9 +279,9 @@ bool Data::process(uint8_t* data, uint32_t len) // to prevent data block offset errors fill the bad block with 0's uint8_t blankBuf[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES]; ::memset(blankBuf, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); - ::memcpy(m_pduUserData + dataOffset, blankBuf, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); + ::memcpy(m_rfPduUserData + dataOffset, blankBuf, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); dataOffset += P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; - m_pduUserDataLength = dataOffset; + m_rfPduUserDataLength = dataOffset; } else { LogWarning(LOG_RF, P25_PDU_STR ", unfixable PDU data (1/2 rate or CRC), block %u", i); @@ -287,9 +289,9 @@ bool Data::process(uint8_t* data, uint32_t len) // to prevent data block offset errors fill the bad block with 0's uint8_t blankBuf[P25_PDU_UNCONFIRMED_LENGTH_BYTES]; ::memset(blankBuf, 0x00U, P25_PDU_UNCONFIRMED_LENGTH_BYTES); - ::memcpy(m_pduUserData + dataOffset, blankBuf, P25_PDU_UNCONFIRMED_LENGTH_BYTES); + ::memcpy(m_rfPduUserData + dataOffset, blankBuf, P25_PDU_UNCONFIRMED_LENGTH_BYTES); dataOffset += P25_PDU_UNCONFIRMED_LENGTH_BYTES; - m_pduUserDataLength = dataOffset; + m_rfPduUserDataLength = dataOffset; } if (m_dumpPDUData) { @@ -301,14 +303,14 @@ bool Data::process(uint8_t* data, uint32_t len) } if (m_rfDataHeader.getBlocksToFollow() > 0U) { - bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength); + bool crcRet = edac::CRC::checkCRC32(m_rfPduUserData, m_rfPduUserDataLength); if (!crcRet) { - LogWarning(LOG_RF, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", m_rfDataHeader.getBlocksToFollow(), m_pduUserDataLength); + LogWarning(LOG_RF, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", m_rfDataHeader.getBlocksToFollow(), m_rfPduUserDataLength); } } if (m_dumpPDUData && m_rfDataBlockCnt > 0U) { - Utils::dump(1U, "PDU Packet", m_pduUserData, m_pduUserDataLength); + Utils::dump(1U, "PDU Packet", m_rfPduUserData, m_rfPduUserDataLength); } if (m_rfDataBlockCnt < blocksToFollow) { @@ -401,9 +403,9 @@ bool Data::process(uint8_t* data, uint32_t len) case PDUSAP::ARP: { /* bryanb: quick and dirty ARP logging */ - uint8_t arpPacket[22U]; - ::memset(arpPacket, 0x00U, 22U); - ::memcpy(arpPacket, m_pduUserData + 12U, 22U); + uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH]; + ::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH); + ::memcpy(arpPacket, m_rfPduUserData + P25_PDU_HEADER_LENGTH_BYTES, P25_PDU_ARP_PCKT_LENGTH); uint16_t opcode = __GET_UINT16B(arpPacket, 6U); uint32_t srcHWAddr = __GET_UINT16(arpPacket, 8U); @@ -429,7 +431,7 @@ bool Data::process(uint8_t* data, uint32_t len) m_rfDataHeader.getBlocksToFollow()); } - processSNDCPControl(); + processSNDCPControl(m_rfPduUserData); } break; case PDUSAP::CONV_DATA_REG: @@ -439,7 +441,7 @@ bool Data::process(uint8_t* data, uint32_t len) m_rfDataHeader.getBlocksToFollow()); } - processConvDataReg(); + processConvDataReg(m_rfPduUserData); } break; case PDUSAP::TRUNK_CTRL: @@ -473,12 +475,13 @@ bool Data::process(uint8_t* data, uint32_t len) m_rfDataBlockCnt = 0U; m_rfPDUCount = 0U; m_rfPDUBits = 0U; - m_pduUserDataLength = 0U; + m_rfPduUserDataLength = 0U; m_p25->m_rfState = m_prevRfState; } // switch (m_rfDataHeader.getSAP()) } + m_inbound = false; return true; } else { @@ -609,7 +612,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) m_netDataOffset = 0U; m_netDataBlockCnt = 0U; m_netPDUCount = 0U; - m_pduUserDataLength = 0U; + m_netPduUserDataLength = 0U; } return true; @@ -621,9 +624,6 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) m_netPDUCount++; m_netDataBlockCnt++; - uint32_t srcId = (m_netExtendedAddress) ? m_netDataHeader.getSrcLLId() : m_netDataHeader.getLLId(); - uint32_t dstId = m_netDataHeader.getLLId(); - if (m_netDataBlockCnt >= m_netDataHeader.getBlocksToFollow()) { uint32_t blocksToFollow = m_netDataHeader.getBlocksToFollow(); uint32_t offset = 0U; @@ -660,9 +660,9 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) blocksToFollow--; // if we are using a secondary header place it in the PDU user data buffer - m_netDataHeader.getExtAddrData(m_pduUserData + dataOffset); + m_netDataHeader.getExtAddrData(m_netPduUserData + dataOffset); dataOffset += P25_PDU_HEADER_LENGTH_BYTES; - m_pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES; + m_netPduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES; } m_netDataBlockCnt = 0U; @@ -703,9 +703,9 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) m_netData[i].getLastBlock()); } - m_netData[i].getData(m_pduUserData + dataOffset); + m_netData[i].getData(m_netPduUserData + dataOffset); dataOffset += (m_netDataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; - m_pduUserDataLength = dataOffset; + m_netPduUserDataLength = dataOffset; m_netDataBlockCnt++; } @@ -724,24 +724,51 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) } if (m_netDataHeader.getBlocksToFollow() > 0U) { - bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength); + bool crcRet = edac::CRC::checkCRC32(m_netPduUserData, m_netPduUserDataLength); if (!crcRet) { - LogWarning(LOG_NET, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", m_netDataHeader.getBlocksToFollow(), m_pduUserDataLength); + LogWarning(LOG_NET, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", m_netDataHeader.getBlocksToFollow(), m_netPduUserDataLength); } } if (m_dumpPDUData && m_netDataBlockCnt > 0U) { - Utils::dump(1U, "PDU Packet", m_pduUserData, m_pduUserDataLength); + Utils::dump(1U, "PDU Packet", m_netPduUserData, m_netPduUserDataLength); } if (m_netDataBlockCnt < blocksToFollow) { LogWarning(LOG_NET, P25_PDU_STR ", incomplete PDU (%d / %d blocks)", m_netDataBlockCnt, blocksToFollow); } + uint32_t srcId = (m_netExtendedAddress) ? m_netDataHeader.getSrcLLId() : m_netDataHeader.getLLId(); + uint32_t dstId = m_netDataHeader.getLLId(); + uint8_t sap = (m_netExtendedAddress) ? m_netDataHeader.getEXSAP() : m_netDataHeader.getSAP(); // handle standard P25 service access points switch (sap) { + case PDUSAP::ARP: + { + /* bryanb: quick and dirty ARP logging */ + uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH]; + ::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH); + ::memcpy(arpPacket, m_netPduUserData + P25_PDU_HEADER_LENGTH_BYTES, P25_PDU_ARP_PCKT_LENGTH); + + uint16_t opcode = __GET_UINT16B(arpPacket, 6U); + uint32_t srcHWAddr = __GET_UINT16(arpPacket, 8U); + uint32_t srcProtoAddr = __GET_UINT32(arpPacket, 11U); + uint32_t tgtHWAddr = __GET_UINT16(arpPacket, 15U); + uint32_t tgtProtoAddr = __GET_UINT32(arpPacket, 18U); + + if (m_verbose) { + if (opcode == P25_PDU_ARP_REQUEST) { + LogMessage(LOG_NET, P25_PDU_STR ", ARP request, who has %s? tell %s (%u)", __IP_FROM_UINT(tgtProtoAddr).c_str(), __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr); + } else if (opcode == P25_PDU_ARP_REPLY) { + LogMessage(LOG_NET, P25_PDU_STR ", ARP reply, %s is at %u", __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr); + } + } + + writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on + } + break; default: ::ActivityLog("P25", false, "Net data transmission from %u to %u, %u blocks", srcId, dstId, m_netDataHeader.getBlocksToFollow()); @@ -760,7 +787,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) m_netDataOffset = 0U; m_netDataBlockCnt = 0U; m_netPDUCount = 0U; - m_pduUserDataLength = 0U; + m_netPduUserDataLength = 0U; m_p25->m_netState = RS_NET_IDLE; } @@ -819,7 +846,7 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, if (blocksToFollow > 0U) { uint32_t dataOffset = 0U; - uint32_t packetLength = (dataHeader.getPacketLength() + dataHeader.getPadLength() + 4U); + uint32_t packetLength = dataHeader.getPDULength(); // generate the second PDU header if ((dataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (dataHeader.getSAP() == PDUSAP::EXT_ADDR) && extendedAddress) { @@ -833,7 +860,6 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, offset += P25_PDU_FEC_LENGTH_BITS; dataOffset += P25_PDU_HEADER_LENGTH_BYTES; - packetLength += P25_PDU_HEADER_LENGTH_BYTES; blocksToFollow--; @@ -1091,14 +1117,17 @@ Data::Data(Control* p25, bool dumpPDUData, bool repeatPDU, bool debug, bool verb m_retryPDUData(nullptr), m_retryPDUBitLength(0U), m_retryCount(0U), - m_pduUserData(nullptr), - m_pduUserDataLength(0U), + m_rfPduUserData(nullptr), + m_rfPduUserDataLength(0U), + m_netPduUserData(nullptr), + m_netPduUserDataLength(0U), m_fneRegTable(), m_convRegQueueTable(), m_convRegTimerTable(), m_sndcpStateTable(), m_sndcpReadyTimers(), m_sndcpStandbyTimers(), + m_inbound(false), m_dumpPDUData(dumpPDUData), m_repeatPDU(repeatPDU), m_verbose(verbose), @@ -1114,8 +1143,10 @@ Data::Data(Control* p25, bool dumpPDUData, bool repeatPDU, bool debug, bool verb m_netPDU = new uint8_t[P25_PDU_FRAME_LENGTH_BYTES + 2U]; ::memset(m_netPDU, 0x00U, P25_PDU_FRAME_LENGTH_BYTES + 2U); - m_pduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U]; - ::memset(m_pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U); + m_rfPduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U]; + ::memset(m_rfPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U); + m_netPduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U]; + ::memset(m_netPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U); m_fneRegTable.clear(); @@ -1139,19 +1170,20 @@ Data::~Data() if (m_retryPDUData != nullptr) delete m_retryPDUData; - delete[] m_pduUserData; + delete[] m_rfPduUserData; + delete[] m_netPduUserData; } /* Helper used to process conventional data registration from PDU data. */ -bool Data::processConvDataReg() +bool Data::processConvDataReg(const uint8_t* pduUserData) { - uint8_t regType = (m_pduUserData[0] >> 4) & 0x0F; + uint8_t regType = (pduUserData[0U] >> 4) & 0x0F; switch (regType) { case PDURegType::CONNECT: { - uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U]; - uint32_t ipAddr = (m_pduUserData[8U] << 24) + (m_pduUserData[9U] << 16) + (m_pduUserData[10U] << 8) + m_pduUserData[11U]; + uint32_t llId = (pduUserData[1U] << 16) + (pduUserData[2U] << 8) + pduUserData[3U]; + uint32_t ipAddr = (pduUserData[8U] << 24) + (pduUserData[9U] << 16) + (pduUserData[10U] << 8) + pduUserData[11U]; if (m_verbose) { LogMessage(LOG_RF, P25_PDU_STR ", CONNECT (Registration Request Connect), llId = %u, ipAddr = %s", llId, __IP_FROM_UINT(ipAddr).c_str()); @@ -1168,7 +1200,7 @@ bool Data::processConvDataReg() break; case PDURegType::DISCONNECT: { - uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U]; + uint32_t llId = (pduUserData[1U] << 16) + (pduUserData[2U] << 8) + pduUserData[3U]; if (m_verbose) { LogMessage(LOG_RF, P25_PDU_STR ", DISCONNECT (Registration Request Disconnect), llId = %u", llId); @@ -1199,16 +1231,16 @@ bool Data::processConvDataReg() /* Helper used to process SNDCP control data from PDU data. */ -bool Data::processSNDCPControl() +bool Data::processSNDCPControl(const uint8_t* pduUserData) { if (!m_p25->m_sndcpSupport) { return false; } - uint8_t pduUserData[P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES]; - ::memset(pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES); + uint8_t txPduUserData[P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES]; + ::memset(txPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES); - std::unique_ptr packet = SNDCPFactory::create(m_pduUserData); + std::unique_ptr packet = SNDCPFactory::create(pduUserData); if (packet == nullptr) { LogWarning(LOG_RF, P25_PDU_STR ", undecodable SNDCP packet"); return false; @@ -1242,10 +1274,10 @@ bool Data::processSNDCPControl() osp->setNSAPI(DEFAULT_NSAPI); osp->setRejectCode(SNDCPRejectReason::SU_NOT_PROVISIONED); - osp->encode(pduUserData); + osp->encode(txPduUserData); rspHeader.calculateLength(2U); - writeRF_PDU_User(rspHeader, false, pduUserData); + writeRF_PDU_User(rspHeader, false, txPduUserData); return true; } @@ -1257,10 +1289,10 @@ bool Data::processSNDCPControl() osp->setNSAPI(DEFAULT_NSAPI); osp->setRejectCode(SNDCPRejectReason::STATIC_IP_ALLOCATION_UNSUPPORTED); - osp->encode(pduUserData); + osp->encode(txPduUserData); rspHeader.calculateLength(2U); - writeRF_PDU_User(rspHeader, false, pduUserData); + writeRF_PDU_User(rspHeader, false, txPduUserData); sndcpReset(llId, true); } @@ -1272,10 +1304,10 @@ bool Data::processSNDCPControl() osp->setNSAPI(DEFAULT_NSAPI); osp->setRejectCode(SNDCPRejectReason::DYN_IP_ALLOCATION_UNSUPPORTED); - osp->encode(pduUserData); + osp->encode(txPduUserData); rspHeader.calculateLength(2U); - writeRF_PDU_User(rspHeader, false, pduUserData); + writeRF_PDU_User(rspHeader, false, txPduUserData); sndcpReset(llId, true); @@ -1290,10 +1322,10 @@ bool Data::processSNDCPControl() osp->setMTU(SNDCP_MTU_510); osp->setMDPCO(isp->getMDPCO()); - osp->encode(pduUserData); + osp->encode(txPduUserData); rspHeader.calculateLength(13U); - writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData); + writeRF_PDU_User(rspHeader, rspHeader, false, txPduUserData); m_sndcpStateTable[llId] = SNDCPState::STANDBY; m_sndcpReadyTimers[llId].stop(); @@ -1308,10 +1340,10 @@ bool Data::processSNDCPControl() osp->setNSAPI(DEFAULT_NSAPI); osp->setRejectCode(SNDCPRejectReason::ANY_REASON); - osp->encode(pduUserData); + osp->encode(txPduUserData); rspHeader.calculateLength(2U); - writeRF_PDU_User(rspHeader, false, pduUserData); + writeRF_PDU_User(rspHeader, false, txPduUserData); sndcpReset(llId, true); } @@ -1401,11 +1433,12 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls, boo // Regenerate NID m_p25->m_nid.encode(data + 2U, DUID::PDU); - // Add busy bits - P25Utils::addBusyBits(data + 2U, newBitLength, true, false); + // Add status bits + P25Utils::addStatusBits(data + 2U, newBitLength, false); + P25Utils::addIdleStatusBits(data + 2U, newBitLength); - // Add idle bits - P25Utils::addIdleBits(data + 2U, newBitLength, true, true); + // Set first busy bits to 1,1 + P25Utils::setStatusBits(data + 2U, P25_SS0_START, true, true); if (m_p25->m_duplex) { data[0U] = modem::TAG_DATA; @@ -1451,7 +1484,7 @@ void Data::writeNet_PDU_Buffered() // generate the second PDU header if ((m_netDataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (m_netDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_netExtendedAddress) { - m_netDataHeader.encodeExtAddr(m_pduUserData, true); + m_netDataHeader.encodeExtAddr(m_netPduUserData, true); ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); m_netDataHeader.encodeExtAddr(block); @@ -1471,7 +1504,7 @@ void Data::writeNet_PDU_Buffered() // are we processing extended address data from the first block? if ((m_netDataHeader.getFormat() == PDUFormatType::CONFIRMED) && (m_netDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_netExtendedAddress) { - m_netDataHeader.encodeExtAddr(m_pduUserData); + m_netDataHeader.encodeExtAddr(m_netPduUserData); if (m_verbose) { LogMessage(LOG_NET, P25_PDU_STR ", OSP, extended address, sap = $%02X, srcLlId = %u", @@ -1479,13 +1512,17 @@ void Data::writeNet_PDU_Buffered() } } - edac::CRC::addCRC32(m_pduUserData, m_pduUserDataLength); + edac::CRC::addCRC32(m_netPduUserData, m_netPduUserDataLength); + + if (m_dumpPDUData) { + Utils::dump("OSP PDU User Data (NET)", m_netPduUserData, m_netPduUserDataLength); + } // generate the PDU data for (uint32_t i = 0U; i < blocksToFollow; i++) { m_netData[i].setFormat(m_netDataHeader); m_netData[i].setSerialNo(i); - m_netData[i].setData(m_pduUserData + dataOffset); + m_netData[i].setData(m_netPduUserData + dataOffset); if (m_verbose) { LogMessage(LOG_NET, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u", @@ -1536,7 +1573,7 @@ void Data::writeRF_PDU_Buffered() // generate the second PDU header if ((m_rfDataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (m_rfDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_rfExtendedAddress) { - m_rfDataHeader.encodeExtAddr(m_pduUserData, true); + m_rfDataHeader.encodeExtAddr(m_rfPduUserData, true); ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); m_rfDataHeader.encodeExtAddr(block); @@ -1556,7 +1593,7 @@ void Data::writeRF_PDU_Buffered() // are we processing extended address data from the first block? if ((m_rfDataHeader.getFormat() == PDUFormatType::CONFIRMED) && (m_rfDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_rfExtendedAddress) { - m_rfDataHeader.encodeExtAddr(m_pduUserData); + m_rfDataHeader.encodeExtAddr(m_rfPduUserData); if (m_verbose) { LogMessage(LOG_RF, P25_PDU_STR ", OSP, extended address, sap = $%02X, srcLlId = %u", @@ -1564,13 +1601,17 @@ void Data::writeRF_PDU_Buffered() } } - edac::CRC::addCRC32(m_pduUserData, m_pduUserDataLength); + edac::CRC::addCRC32(m_rfPduUserData, m_rfPduUserDataLength); + + if (m_dumpPDUData) { + Utils::dump("OSP PDU User Data (RF)", m_rfPduUserData, m_rfPduUserDataLength); + } // generate the PDU data for (uint32_t i = 0U; i < blocksToFollow; i++) { m_rfData[i].setFormat(m_rfDataHeader); m_rfData[i].setSerialNo(i); - m_rfData[i].setData(m_pduUserData + dataOffset); + m_rfData[i].setData(m_rfPduUserData + dataOffset); if (m_verbose) { LogMessage(LOG_RF, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u", diff --git a/src/host/p25/packet/Data.h b/src/host/p25/packet/Data.h index c82bbb21..111bcb20 100644 --- a/src/host/p25/packet/Data.h +++ b/src/host/p25/packet/Data.h @@ -140,8 +140,10 @@ namespace p25 uint32_t m_retryPDUBitLength; uint8_t m_retryCount; - uint8_t* m_pduUserData; - uint32_t m_pduUserDataLength; + uint8_t* m_rfPduUserData; + uint32_t m_rfPduUserDataLength; + uint8_t* m_netPduUserData; + uint32_t m_netPduUserDataLength; std::unordered_map m_fneRegTable; @@ -152,6 +154,8 @@ namespace p25 std::unordered_map m_sndcpReadyTimers; std::unordered_map m_sndcpStandbyTimers; + bool m_inbound; + bool m_dumpPDUData; bool m_repeatPDU; @@ -174,14 +178,16 @@ namespace p25 /** * @brief Helper used to process conventional data registration from PDU data. - * @returns bool True, if SNDCP control data was processed, otherwise false. + * @param pduUserData Buffer containing user data to transmit. + * @returns bool True, if conventional data registration data was processed, otherwise false. */ - bool processConvDataReg(); + bool processConvDataReg(const uint8_t* pduUserData); /** * @brief Helper used to process SNDCP control data from PDU data. + * @param pduUserData Buffer containing user data to transmit. * @returns bool True, if SNDCP control data was processed, otherwise false. */ - bool processSNDCPControl(); + bool processSNDCPControl(const uint8_t* pduUserData); /** * @brief Write data processed from RF to the network. diff --git a/src/host/p25/packet/Voice.cpp b/src/host/p25/packet/Voice.cpp index 270b5d76..c519b059 100644 --- a/src/host/p25/packet/Voice.cpp +++ b/src/host/p25/packet/Voice.cpp @@ -58,6 +58,8 @@ void Voice::resetRF() m_rfUndecodableLC = 0U; m_vocLDU1Count = 0U; m_roamLDU1Count = 0U; + + m_inbound = false; } /* Resets the data states for the network. */ @@ -106,6 +108,8 @@ bool Voice::process(uint8_t* data, uint32_t len) if (m_p25->m_rfState == RS_RF_LISTENING || m_p25->m_rfState == RS_RF_AUDIO) { resetRF(); + m_inbound = true; + lc::LC lc = lc::LC(); bool ret = lc.decodeHDU(data + 2U); if (!ret) { @@ -192,9 +196,12 @@ bool Voice::process(uint8_t* data, uint32_t len) lc::LC lc = lc::LC(); bool ret = lc.decodeLDU1(data + 2U); if (!ret) { + m_inbound = false; return false; } + m_inbound = true; + rsValue = lc.getRS(); uint32_t srcId = lc.getSrcId(); @@ -495,7 +502,7 @@ bool Voice::process(uint8_t* data, uint32_t len) m_rfLC.encodeHDU(buffer + 2U); // Add busy bits - P25Utils::addBusyBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false, true); + P25Utils::addStatusBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, m_inbound); writeNetwork(buffer, DUID::HDU); @@ -637,6 +644,8 @@ bool Voice::process(uint8_t* data, uint32_t len) } } + m_inbound = true; + rsValue = m_rfLC.getRS(); alreadyDecoded = false; @@ -715,7 +724,7 @@ bool Voice::process(uint8_t* data, uint32_t len) m_rfFrames++; // add busy bits - P25Utils::addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + P25Utils::addStatusBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, m_inbound); writeNetwork(data + 2U, DUID::LDU1, frameType); @@ -773,6 +782,8 @@ bool Voice::process(uint8_t* data, uint32_t len) m_rfLastLDU2 = m_rfLC; } + m_inbound = true; + // generate Sync Sync::addP25Sync(data + 2U); @@ -821,7 +832,7 @@ bool Voice::process(uint8_t* data, uint32_t len) m_rfFrames++; // add busy bits - P25Utils::addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + P25Utils::addStatusBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, m_inbound); writeNetwork(data + 2U, DUID::LDU2); @@ -868,6 +879,8 @@ bool Voice::process(uint8_t* data, uint32_t len) resetRF(); } + m_inbound = true; + m_lastRejectId = 0U; ::ActivityLog("P25", true, "RF VSELP voice transmission"); @@ -908,7 +921,7 @@ bool Voice::process(uint8_t* data, uint32_t len) m_rfLC.encodeHDU(buffer + 2U); // Add busy bits - P25Utils::addBusyBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false, true); + P25Utils::addStatusBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, m_inbound); writeNetwork(buffer, DUID::HDU); @@ -942,6 +955,8 @@ bool Voice::process(uint8_t* data, uint32_t len) if (m_p25->m_rfState == RS_RF_AUDIO) { m_rfFrames++; + m_inbound = true; + // generate Sync Sync::addP25Sync(data + 2U); @@ -949,7 +964,7 @@ bool Voice::process(uint8_t* data, uint32_t len) m_p25->m_nid.encode(data + 2U, DUID::VSELP1); // add busy bits - P25Utils::addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + P25Utils::addStatusBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, m_inbound); writeNetwork(data + 2U, DUID::VSELP1); @@ -982,6 +997,8 @@ bool Voice::process(uint8_t* data, uint32_t len) else if (m_p25->m_rfState == RS_RF_AUDIO) { m_rfFrames++; + m_inbound = true; + // generate Sync Sync::addP25Sync(data + 2U); @@ -989,7 +1006,7 @@ bool Voice::process(uint8_t* data, uint32_t len) m_p25->m_nid.encode(data + 2U, DUID::VSELP2); // add busy bits - P25Utils::addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + P25Utils::addStatusBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, m_inbound); writeNetwork(data + 2U, DUID::VSELP2); @@ -1063,6 +1080,7 @@ bool Voice::process(uint8_t* data, uint32_t len) m_p25->writeRF_ControlData(); } + m_inbound = false; m_p25->m_rfState = RS_RF_LISTENING; return true; } @@ -1371,6 +1389,7 @@ Voice::Voice(Control* p25, bool debug, bool verbose) : m_silenceThreshold(DEFAULT_SILENCE_THRESHOLD), m_vocLDU1Count(0U), m_roamLDU1Count(0U), + m_inbound(false), m_verbose(verbose), m_debug(debug) { @@ -1465,7 +1484,7 @@ void Voice::writeNet_TDU() m_p25->m_nid.encode(buffer + 2U, DUID::TDU); // Add busy bits - P25Utils::addBusyBits(buffer + 2U, P25_TDU_FRAME_LENGTH_BITS, true, true); + P25Utils::addStatusBits(buffer + 2U, P25_TDU_FRAME_LENGTH_BITS, false); m_p25->addFrame(buffer, P25_TDU_FRAME_LENGTH_BYTES + 2U, true); @@ -1733,7 +1752,7 @@ void Voice::writeNet_LDU1() m_netLC.encodeHDU(buffer + 2U); // Add busy bits - P25Utils::addBusyBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false, true); + P25Utils::addStatusBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false); buffer[0U] = modem::TAG_DATA; buffer[1U] = 0x00U; @@ -1844,7 +1863,7 @@ void Voice::writeNet_LDU1() m_netLSD.encode(buffer + 2U); // Add busy bits - P25Utils::addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + P25Utils::addStatusBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false); buffer[0U] = modem::TAG_DATA; buffer[1U] = 0x00U; @@ -1934,7 +1953,7 @@ void Voice::writeNet_LDU2() m_netLSD.encode(buffer + 2U); // Add busy bits - P25Utils::addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + P25Utils::addStatusBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false); buffer[0U] = modem::TAG_DATA; buffer[1U] = 0x00U; diff --git a/src/host/p25/packet/Voice.h b/src/host/p25/packet/Voice.h index e4a360f9..d8caab0a 100644 --- a/src/host/p25/packet/Voice.h +++ b/src/host/p25/packet/Voice.h @@ -124,6 +124,8 @@ namespace p25 uint8_t m_vocLDU1Count; uint8_t m_roamLDU1Count; + bool m_inbound; + bool m_verbose; bool m_debug; diff --git a/src/host/setup/SetupApplication.h b/src/host/setup/SetupApplication.h index 62d6ef0f..9e8702b2 100644 --- a/src/host/setup/SetupApplication.h +++ b/src/host/setup/SetupApplication.h @@ -63,7 +63,7 @@ protected: nid->encode(data + 2U, DUID::TDU); // Add busy bits - p25::P25Utils::addBusyBits(data + 2U, P25_TDU_FRAME_LENGTH_BITS, true, true); + p25::P25Utils::addStatusBits(data + 2U, P25_TDU_FRAME_LENGTH_BITS, false); data[0U] = modem::TAG_EOT; data[1U] = 0x00U;