From 3060769f9bdb0cef2a1fb02bad4ce26c95fd9def Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Thu, 8 Oct 2026 11:51:26 -0400 Subject: [PATCH] add better handling for ARP and data packet routing; --- .../p25/data/ConventionalDataService.cpp | 16 + src/common/p25/data/ConventionalDataService.h | 7 + src/common/p25/data/DataHeader.cpp | 10 + src/common/p25/data/DataHeader.h | 7 + src/common/p25/data/DataRouting.cpp | 213 +++++++++ src/common/p25/data/DataRouting.h | 320 +++++++++++++ src/common/p25/data/PacketScheduler.cpp | 88 ++++ src/common/p25/data/PacketScheduler.h | 115 +++++ .../callhandler/packetdata/P25PacketData.cpp | 431 +++++++++--------- .../callhandler/packetdata/P25PacketData.h | 28 +- tests/p25/P25_PacketDataState_Tests.cpp | 230 ++++++++-- 11 files changed, 1184 insertions(+), 281 deletions(-) create mode 100644 src/common/p25/data/DataRouting.cpp create mode 100644 src/common/p25/data/DataRouting.h create mode 100644 src/common/p25/data/PacketScheduler.cpp create mode 100644 src/common/p25/data/PacketScheduler.h diff --git a/src/common/p25/data/ConventionalDataService.cpp b/src/common/p25/data/ConventionalDataService.cpp index ba96be20..b7bbdf45 100644 --- a/src/common/p25/data/ConventionalDataService.cpp +++ b/src/common/p25/data/ConventionalDataService.cpp @@ -165,6 +165,22 @@ bool ConventionalDataService::isRegistered(uint32_t llId) const return m_registrations.find(llId) != m_registrations.end(); } +/* Converts a registration deny reason to a string representation. */ + +std::string ConventionalDataService::denyReasonToString(RegistrationDenyReason reason) +{ + switch (reason) { + case RegistrationDenyReason::MALFORMED: return "MALFORMED"; + case RegistrationDenyReason::IDENTITY_MISMATCH: return "IDENTITY_MISMATCH"; + case RegistrationDenyReason::NOT_PROVISIONED: return "NOT_PROVISIONED"; + case RegistrationDenyReason::DISABLED: return "DISABLED"; + case RegistrationDenyReason::UNSUPPORTED_OPTIONS: return "UNSUPPORTED_OPTIONS"; + case RegistrationDenyReason::ADDRESS_UNAVAILABLE: return "ADDRESS_UNAVAILABLE"; + case RegistrationDenyReason::ADDRESS_CONFLICT: return "ADDRESS_CONFLICT"; + default: return "UNKNOWN"; + } +} + // --------------------------------------------------------------------------- // Private Class Members // --------------------------------------------------------------------------- diff --git a/src/common/p25/data/ConventionalDataService.h b/src/common/p25/data/ConventionalDataService.h index 3e85ec1e..49a506e0 100644 --- a/src/common/p25/data/ConventionalDataService.h +++ b/src/common/p25/data/ConventionalDataService.h @@ -195,6 +195,13 @@ namespace p25 */ size_t registrationCount() const noexcept { return m_registrations.size(); } + /** + * @brief Converts a registration deny reason to a string representation. + * @param reason The registration deny reason. + * @return A string representation of the registration deny reason. + */ + static std::string denyReasonToString(RegistrationDenyReason reason); + private: /** * @brief Represents the state of a registration. diff --git a/src/common/p25/data/DataHeader.cpp b/src/common/p25/data/DataHeader.cpp index d7bd4443..2c0e2f2c 100644 --- a/src/common/p25/data/DataHeader.cpp +++ b/src/common/p25/data/DataHeader.cpp @@ -112,6 +112,16 @@ DataHeader::~DataHeader() } } +/* Equals operator. */ + +DataHeader& DataHeader::operator=(const DataHeader& header) +{ + if (&header != this) + copy(header); + + return *this; +} + /* Decodes P25 PDU data header. */ bool DataHeader::decode(const uint8_t* data, bool noTrellis) diff --git a/src/common/p25/data/DataHeader.h b/src/common/p25/data/DataHeader.h index 0070dfb4..b82cc4f3 100644 --- a/src/common/p25/data/DataHeader.h +++ b/src/common/p25/data/DataHeader.h @@ -53,6 +53,13 @@ namespace p25 */ ~DataHeader(); + /** + * @brief Equals operator. + * @param header Instance of DataHeader class to copy from. + * @return Reference to this instance. + */ + DataHeader& operator=(const DataHeader& header); + /** * @brief Decodes P25 PDU data header. * @param[in] data Buffer containing a PDU data header to decode. diff --git a/src/common/p25/data/DataRouting.cpp b/src/common/p25/data/DataRouting.cpp new file mode 100644 index 00000000..5bc86ef7 --- /dev/null +++ b/src/common/p25/data/DataRouting.cpp @@ -0,0 +1,213 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Common Library + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2026 Bryan Biedenkapp, N2PLL + * + */ +#include "common/p25/data/DataRouting.h" + +using namespace p25::data; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/* Observes a route neighbor with the given link-layer ID and IP address. */ + +void RouteNeighborCache::observe(uint32_t llId, uint32_t ipAddress, uint64_t nowMs) +{ + if (llId == 0U || ipAddress == 0U) + return; + + RouteNeighbor& neighbor = m_neighbors[llId]; + neighbor.llId = llId; + neighbor.ipAddress = ipAddress; + neighbor.lastSeen = nowMs; +} + +/* Erases a route neighbor with the given link-layer ID. */ + +bool RouteNeighborCache::erase(uint32_t llId) +{ + return m_neighbors.erase(llId) > 0U; +} + +/* Clears all observed route neighbors. */ + +void RouteNeighborCache::clear() +{ + m_neighbors.clear(); +} + +/* Finds a route neighbor by its link-layer ID. */ + +const RouteNeighbor* RouteNeighborCache::findByLLId(uint32_t llId) const +{ + auto it = m_neighbors.find(llId); + return it == m_neighbors.end() ? nullptr : &it->second; +} + +/* Finds a route neighbor by its IP address. */ + +const RouteNeighbor* RouteNeighborCache::findByIPAddress(uint32_t ipAddress) const +{ + for (const auto& entry : m_neighbors) { + if (entry.second.ipAddress == ipAddress) + return &entry.second; + } + + return nullptr; +} + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/* Initializes a new instance of the DataDataLocationRegistry class. */ + +DataLocationRegistry::DataLocationRegistry(uint64_t staleAfterMs, + bool learnFromInbound, UnknownLocationPolicy unknownLocationPolicy) : + m_staleAfterMs(staleAfterMs), + m_learnFromInbound(learnFromInbound), + m_unknownLocationPolicy(unknownLocationPolicy), + m_conventional(), + m_trunked(), + m_groupPeers() +{ + /* stub */ +} + +/* Updates the conventional location for a given link-layer ID. */ + +bool DataLocationRegistry::updateConventional(uint32_t llId, const ConventionalLocation& location) +{ + if (llId == 0U || location.peerId == 0U) + return false; + + m_conventional[llId] = location; + return true; +} + +/* Updates the trunked data route for a given link-layer ID. */ + +bool DataLocationRegistry::updateTrunked(uint32_t llId, const DataRoute& route) +{ + if (llId == 0U || route.peerId == 0U || route.mode != AccessMode::TRUNKED) + return false; + + m_trunked[llId] = route; + return true; +} + +/* Erases the location or route for a given link-layer ID and access mode. */ + +bool DataLocationRegistry::erase(uint32_t llId, AccessMode mode) +{ + if (mode == AccessMode::TRUNKED) + return m_trunked.erase(llId) > 0U; + return m_conventional.erase(llId) > 0U; +} + +/* Clears all subscriber and group route state. */ + +void DataLocationRegistry::clear() +{ + m_conventional.clear(); + m_trunked.clear(); + m_groupPeers.clear(); +} + +/* Expires stale conventional and trunked route state based on the current time. */ + +void DataLocationRegistry::expire(uint64_t nowMs) +{ + for (auto it = m_conventional.begin(); it != m_conventional.end();) { + if (stale(it->second.lastSeen, nowMs)) + it = m_conventional.erase(it); + else + ++it; + } + + for (auto it = m_trunked.begin(); it != m_trunked.end();) { + if (stale(it->second.lastSeen, nowMs)) + it = m_trunked.erase(it); + else + ++it; + } +} + +/* Resolves the data route for a given link-layer ID and access mode. */ + +DataRoute DataLocationRegistry::resolve(uint32_t llId, AccessMode mode, uint64_t nowMs) const +{ + DataRoute result; + result.mode = mode; + + // check if the requested access mode is trunked and resolve accordingly + if (mode == AccessMode::TRUNKED) { + auto it = m_trunked.find(llId); + if (it == m_trunked.end() || stale(it->second.lastSeen, nowMs)) + return result; + + result = it->second; + result.valid = true; + + return result; + } + + auto it = m_conventional.find(llId); + if (it == m_conventional.end() || stale(it->second.lastSeen, nowMs)) + return result; + + result.peerId = it->second.peerId; + result.channelId = it->second.channelId; + result.channelNo = it->second.channelNo; + result.lastSeen = it->second.lastSeen; + result.valid = true; + + return result; +} + +/* Sets the group peers for group-based routing. */ + +void DataLocationRegistry::setGroupPeers(const std::vector& peerIds) +{ + m_groupPeers.clear(); + for (uint32_t peerId : peerIds) { + if (peerId != 0U) + m_groupPeers.insert(peerId); + } +} + +/* Resolves the data routes for a group of available peers. */ + +std::vector DataLocationRegistry::resolveGroup(const std::vector& availablePeerIds) const +{ + std::vector routes; + for (uint32_t peerId : availablePeerIds) { + if (peerId == 0U || (!m_groupPeers.empty() && m_groupPeers.count(peerId) == 0U)) + continue; + + DataRoute route; + route.mode = AccessMode::CONVENTIONAL; + route.peerId = peerId; + route.valid = true; + routes.push_back(route); + } + + return routes; +} + +// --------------------------------------------------------------------------- +// Private Class Members +// --------------------------------------------------------------------------- + +/* Checks if a location is considered stale based on the last seen timestamp and the current time. */ + +bool DataLocationRegistry::stale(uint64_t lastSeen, uint64_t nowMs) const +{ + return m_staleAfterMs != 0U && nowMs != 0U && nowMs >= lastSeen && nowMs - lastSeen >= m_staleAfterMs; +} diff --git a/src/common/p25/data/DataRouting.h b/src/common/p25/data/DataRouting.h new file mode 100644 index 00000000..0017d643 --- /dev/null +++ b/src/common/p25/data/DataRouting.h @@ -0,0 +1,320 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Common Library + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2026 Bryan Biedenkapp, N2PLL + * + */ +/** + * @file DataRouting.h + * @ingroup p25_pdu + * @file DataRouting.cpp + * @ingroup p25_pdu + */ +#if !defined(__P25_DATA__DATAROUTING_H__) +#define __P25_DATA__DATAROUTING_H__ + +#include "common/Defines.h" +#include "common/p25/data/PacketDataState.h" + +#include +#include +#include +#include + +namespace p25 +{ + namespace data + { + // --------------------------------------------------------------------------- + // Constants + // --------------------------------------------------------------------------- + + /** + * @addtogroup p25_pdu + * @{ + */ + + /** + * @brief Access mode used to reach a P25 packet data subscriber. + */ + enum class AccessMode : uint8_t { + CONVENTIONAL, //!< Conventional access mode. + TRUNKED //!< Trunked access mode. + }; + + /** + * @brief Policy applied when no current subscriber location is known. + */ + enum class UnknownLocationPolicy : uint8_t { + QUEUE, //!< Queue the packet until location is known. + ARP, //!< Attempt ARP resolution. + DROP //!< Drop the packet if location is unknown. + }; + /* @} */ + + // --------------------------------------------------------------------------- + // Struct Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Last observed conventional location for a subscriber. + * @ingroup p25_pdu + */ + struct DVM_COMMON_API ConventionalLocation { + /** + * @brief Peer ID of the subscriber. + */ + uint32_t peerId = 0U; + /** + * @brief Channel ID of the subscriber. + */ + uint8_t channelId = 0U; + /** + * @brief Channel number of the subscriber. + */ + uint16_t channelNo = 0U; + /** + * @brief Timestamp of the last observation. + */ + uint64_t lastSeen = 0U; + }; + + // --------------------------------------------------------------------------- + // Struct Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Resolved RF route for a P25 packet data subscriber. + * @ingroup p25_pdu + */ + struct DVM_COMMON_API DataRoute { + /** + * @brief Access mode used to reach the subscriber. + */ + AccessMode mode = AccessMode::CONVENTIONAL; + /** + * @brief Peer ID of the subscriber. + */ + uint32_t peerId = 0U; + /** + * @brief Channel ID of the subscriber. + */ + uint8_t channelId = 0U; + /** + * @brief Channel number of the subscriber. + */ + uint16_t channelNo = 0U; + /** + * @brief Grant ID associated with the subscriber. + */ + uint32_t grantId = 0U; + /** + * @brief Timestamp of the last observation. + */ + uint64_t lastSeen = 0U; + /** + * @brief Indicates if the route is valid. + */ + bool valid = false; + }; + + // --------------------------------------------------------------------------- + // Struct Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Observed ARP neighbor information without authorization semantics. + * @ingroup p25_pdu + */ + struct DVM_COMMON_API RouteNeighbor { + /** + * @brief Link-layer ID of the neighbor. + */ + uint32_t llId = 0U; + /** + * @brief IP address of the neighbor. + */ + uint32_t ipAddress = 0U; + /** + * @brief Timestamp of the last observation. + */ + uint64_t lastSeen = 0U; + }; + + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Stores ARP observations separately from authorized bindings. + * @ingroup p25_pdu + */ + class DVM_COMMON_API RouteNeighborCache { + public: + /** + * @brief Observes a route neighbor with the given link-layer ID and IP address. + * @param llId Link-layer ID of the neighbor. + * @param ipAddress IP address of the neighbor. + * @param nowMs Timestamp of the observation in milliseconds. + */ + void observe(uint32_t llId, uint32_t ipAddress, uint64_t nowMs = 0U); + /** + * @brief Erases a route neighbor with the given link-layer ID. + * @param llId Link-layer ID of the neighbor to erase. + * @return True if the neighbor was found and erased, false otherwise. + */ + bool erase(uint32_t llId); + /** + * @brief Clears all observed route neighbors. + */ + void clear(); + + /** + * @brief Finds a route neighbor by its link-layer ID. + * @param llId Link-layer ID of the neighbor to find. + * @return Pointer to the route neighbor if found, nullptr otherwise. + */ + const RouteNeighbor* findByLLId(uint32_t llId) const; + /** + * @brief Finds a route neighbor by its IP address. + * @param ipAddress IP address of the neighbor to find. + * @return Pointer to the route neighbor if found, nullptr otherwise. + */ + const RouteNeighbor* findByIPAddress(uint32_t ipAddress) const; + /** + * @brief Returns the number of observed route neighbors. + * @return Number of observed route neighbors. + */ + size_t size() const noexcept { return m_neighbors.size(); } + + private: + std::unordered_map m_neighbors; + }; + + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Resolves conventional locations and future trunked data routes. + * @ingroup p25_pdu + */ + class DVM_COMMON_API DataLocationRegistry { + public: + /** + * @brief Initializes a new instance of the LocationRegistry class. + * @param staleAfterMs Duration in milliseconds after which locations are considered stale. + * @param learnFromInbound Whether to learn locations from inbound traffic. + * @param unknownLocationPolicy Policy for handling unknown locations. + */ + explicit DataLocationRegistry(uint64_t staleAfterMs = 0U, bool learnFromInbound = true, + UnknownLocationPolicy unknownLocationPolicy = UnknownLocationPolicy::ARP); + + /** + * @brief Updates the conventional location for a given link-layer ID. + * @param llId Link-layer ID of the neighbor. + * @param location Conventional location to update. + * @return True if the location was updated, false otherwise. + */ + bool updateConventional(uint32_t llId, const ConventionalLocation& location); + /** + * @brief Updates the trunked data route for a given link-layer ID. + * @param llId Link-layer ID of the neighbor. + * @param route Trunked data route to update. + * @return True if the route was updated, false otherwise. + */ + bool updateTrunked(uint32_t llId, const DataRoute& route); + /** + * @brief Erases the location or route for a given link-layer ID and access mode. + * @param llId Link-layer ID of the neighbor. + * @param mode Access mode (conventional or trunked). + * @return True if the entry was erased, false otherwise. + */ + bool erase(uint32_t llId, AccessMode mode); + /** + * @brief Clears all subscriber and group route state. + */ + void clear(); + /** + * @brief Expires stale conventional and trunked route state based on the current time. + * @param nowMs Current time in milliseconds. + */ + void expire(uint64_t nowMs); + + /** + * @brief Resolves the data route for a given link-layer ID and access mode. + * @param llId Link-layer ID of the neighbor. + * @param mode Access mode (conventional or trunked). + * @param nowMs Current time in milliseconds. + * @return Resolved data route. + */ + DataRoute resolve(uint32_t llId, AccessMode mode, uint64_t nowMs = 0U) const; + + /** + * @brief Sets the group peers for group-based routing. + * @param peerIds Vector of peer link-layer IDs. + */ + void setGroupPeers(const std::vector& peerIds); + /** + * @brief Resolves the data routes for a group of available peers. + * @param availablePeerIds Vector of available peer link-layer IDs. + * @return Vector of resolved data routes for the available peers. + */ + std::vector resolveGroup(const std::vector& availablePeerIds) const; + + /** + * @brief Sets whether to learn from inbound data routes. + * @param enabled True to enable learning from inbound routes, false to disable. + */ + void setLearnFromInbound(bool enabled) noexcept { m_learnFromInbound = enabled; } + /** + * @brief Checks whether learning from inbound data routes is enabled. + * @return True if learning from inbound routes is enabled, false otherwise. + */ + bool learnFromInbound() const noexcept { return m_learnFromInbound; } + /** + * @brief Sets the policy for handling unknown locations. + * @param policy The unknown location policy to set. + */ + void setUnknownLocationPolicy(UnknownLocationPolicy policy) noexcept { m_unknownLocationPolicy = policy; } + /** + * @brief Retrieves the current policy for handling unknown locations. + * @return The current unknown location policy. + */ + UnknownLocationPolicy unknownLocationPolicy() const noexcept { return m_unknownLocationPolicy; } + + /** + * @brief Retrieves the count of conventional locations. + * @return The number of conventional locations. + */ + size_t conventionalCount() const noexcept { return m_conventional.size(); } + /** + * @brief Retrieves the count of trunked locations. + * @return The number of trunked locations. + */ + size_t trunkedCount() const noexcept { return m_trunked.size(); } + + private: + uint64_t m_staleAfterMs; + bool m_learnFromInbound; + UnknownLocationPolicy m_unknownLocationPolicy; + std::unordered_map m_conventional; + std::unordered_map m_trunked; + std::unordered_set m_groupPeers; + + /** + * @brief Checks if a location is considered stale based on the last seen timestamp and the current time. + * @param lastSeen The timestamp when the location was last seen. + * @param nowMs The current time in milliseconds. + * @return True if the location is stale, false otherwise. + */ + bool stale(uint64_t lastSeen, uint64_t nowMs) const; + }; + + } // namespace data +} // namespace p25 + +#endif // __P25_DATA__DATAROUTING_H__ diff --git a/src/common/p25/data/PacketScheduler.cpp b/src/common/p25/data/PacketScheduler.cpp new file mode 100644 index 00000000..184985ac --- /dev/null +++ b/src/common/p25/data/PacketScheduler.cpp @@ -0,0 +1,88 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Common Library + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2026 Bryan Biedenkapp, N2PLL + * + */ +#include "common/p25/data/PacketScheduler.h" + +#include + +using namespace p25::data; + +// --------------------------------------------------------------------------- +// Public Class Members +// --------------------------------------------------------------------------- + +/* Initializes a new instance of the PacketScheduler class. */ + +PacketScheduler::PacketScheduler(uint32_t maxFrames, uint32_t maxBytes) : + m_maxFrames(maxFrames), + m_maxBytes(maxBytes), + m_byteCount(0U), + m_packets() +{ + /* stub */ +} + +/* Enqueues a scheduled packet into the scheduler. */ + +uint32_t PacketScheduler::enqueue(ScheduledDataPacket&& packet) +{ + uint32_t dropped = 0U; + uint32_t packetBytes = uint32_t(packet.userData.size()); + if (m_maxBytes != 0U && packetBytes > m_maxBytes) + return 1U; + + while (!m_packets.empty() && + ((m_maxFrames != 0U && m_packets.size() >= m_maxFrames) || + (m_maxBytes != 0U && m_byteCount + packetBytes > m_maxBytes))) { + pop(); + dropped++; + } + + m_byteCount += packetBytes; + m_packets.emplace_back(std::move(packet)); + return dropped; +} + +/* Gets the next queued packet. */ + +ScheduledDataPacket* PacketScheduler::front() +{ + return m_packets.empty() ? nullptr : &m_packets.front(); +} + +/* Removes the next queued packet. */ + +void PacketScheduler::pop() +{ + if (m_packets.empty()) + return; + + uint32_t packetBytes = uint32_t(m_packets.front().userData.size()); + m_byteCount = packetBytes <= m_byteCount ? m_byteCount - packetBytes : 0U; + m_packets.pop_front(); +} + +/* Moves the next queued packet to the back of the queue. */ + +void PacketScheduler::rotate() +{ + if (m_packets.size() < 2U) + return; + + m_packets.emplace_back(std::move(m_packets.front())); + m_packets.pop_front(); +} + +/* Clears all queued packets from the scheduler. */ + +void PacketScheduler::clear() +{ + m_packets.clear(); + m_byteCount = 0U; +} diff --git a/src/common/p25/data/PacketScheduler.h b/src/common/p25/data/PacketScheduler.h new file mode 100644 index 00000000..9bc3dd2a --- /dev/null +++ b/src/common/p25/data/PacketScheduler.h @@ -0,0 +1,115 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Digital Voice Modem - Common Library + * GPLv2 Open Source. Use is subject to license terms. + * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. + * + * Copyright (C) 2026 Bryan Biedenkapp, N2PLL + * + */ +/** + * @file PacketScheduler.h + * @ingroup p25_pdu + * @file PacketScheduler.cpp + * @ingroup p25_pdu + */ +#if !defined(__P25_DATA__PACKETSCHEDULER_H__) +#define __P25_DATA__PACKETSCHEDULER_H__ + +#include "common/Defines.h" +#include "common/p25/data/DataHeader.h" + +#include +#include +#include + +namespace p25 +{ + namespace data + { + // --------------------------------------------------------------------------- + // Struct Declaration + // --------------------------------------------------------------------------- + + /** + * @brief One queued packet data downlink. + * @ingroup p25_pdu + */ + struct DVM_COMMON_API ScheduledDataPacket { + DataHeader header; //!< Header information for the P25 packet. + std::vector userData; //!< User data payload for the P25 packet. + uint32_t llId = 0U; //!< Link-layer ID associated with the scheduled packet. + uint32_t targetIPAddress = 0U; //!< Target IP address for the scheduled packet. + uint64_t dueAt = 0U; //!< Timestamp indicating when the packet is due. + uint8_t retryCount = 0U; //!< Number of retry attempts for the packet. + bool extendedRetry = false; //!< Indicates if extended retry is enabled. + }; + + // --------------------------------------------------------------------------- + // Class Declaration + // --------------------------------------------------------------------------- + + /** + * @brief Owns bounded queued packet data downlinks. + * @ingroup p25_pdu + */ + class DVM_COMMON_API PacketScheduler { + public: + /** + * @brief Initializes a new instance of the PacketScheduler class. + * @param maxFrames The maximum number of frames the scheduler can hold. + * @param maxBytes The maximum number of bytes the scheduler can hold. + */ + PacketScheduler(uint32_t maxFrames = 0U, uint32_t maxBytes = 0U); + + /** + * @brief Enqueues a scheduled packet into the scheduler. + * @param packet The scheduled packet to enqueue. + * @return The number of oldest frames dropped to make room. + */ + uint32_t enqueue(ScheduledDataPacket&& packet); + + /** + * @brief Checks if the scheduler is empty. + * @return True if the scheduler is empty, false otherwise. + */ + bool empty() const noexcept { return m_packets.empty(); } + /** + * @brief Gets the number of packets currently in the scheduler. + * @return The number of packets in the scheduler. + */ + size_t size() const noexcept { return m_packets.size(); } + /** + * @brief Gets the total byte count of packets currently in the scheduler. + * @return The total byte count of packets in the scheduler. + */ + uint32_t byteCount() const noexcept { return m_byteCount; } + + /** + * @brief Gets the next queued packet. + * @return A pointer to the next scheduled packet, or nullptr if the scheduler is empty. + */ + ScheduledDataPacket* front(); + /** + * @brief Removes the next queued packet. + */ + void pop(); + /** + * @brief Moves the next queued packet to the back of the queue. + */ + void rotate(); + /** + * @brief Clears all queued packets from the scheduler. + */ + void clear(); + + private: + uint32_t m_maxFrames; + uint32_t m_maxBytes; + uint32_t m_byteCount; + std::deque m_packets; + }; + } // namespace data +} // namespace p25 + +#endif // __P25_DATA__PACKETSCHEDULER_H__ diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.cpp b/src/fne/network/callhandler/packetdata/P25PacketData.cpp index 295cc536..d2ca6db6 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.cpp +++ b/src/fne/network/callhandler/packetdata/P25PacketData.cpp @@ -29,12 +29,14 @@ using namespace network::callhandler; using namespace network::callhandler::packetdata; using namespace p25; using namespace p25::defines; +using namespace p25::data; using namespace p25::kmm; using namespace p25::sndcp; +#include #include #include -#include +#include #if !defined(_WIN32) #include @@ -50,6 +52,7 @@ const uint8_t MAX_PKT_RETRY_CNT = 2U; const uint32_t INTERPACKET_DELAY = 100U; // milliseconds const uint32_t ARP_RETRY_MS = 5000U; // milliseconds const uint32_t SUBSCRIBER_READY_RETRY_MS = 1000U; // milliseconds +const uint32_t CONVENTIONAL_LOCATION_MAX_AGE_MS = 300000U; // 5 minutes // --------------------------------------------------------------------------- // Public Class Members @@ -61,11 +64,11 @@ P25PacketData::P25PacketData(TrafficNetwork* network, TagP25Data* tag, bool debu m_network(network), m_tag(tag), m_assembler(nullptr), - m_queuedFrames(), - m_queuedFrameBytes(0U), + m_packetScheduler(network->m_vtunQueueMaxFrames, network->m_vtunQueueMaxBytes), m_status(), - m_arpTable(), + m_neighborCache(), m_bindingRegistry(), + m_locationRegistry(CONVENTIONAL_LOCATION_MAX_AGE_MS), m_dataLinkManager(), m_conventionalDataService(m_bindingRegistry, m_dataLinkManager), m_scepService(m_bindingRegistry), @@ -98,18 +101,6 @@ P25PacketData::P25PacketData(TrafficNetwork* network, TagP25Data* tag, bool debu P25PacketData::~P25PacketData() { - while (m_queuedFrames.size() > 0U) { - QueuedDataFrame* frame = m_queuedFrames[0U]; - m_queuedFrames.pop_front(); - if (frame != nullptr) { - if (frame->userData != nullptr) - delete[] frame->userData; - if (frame->header != nullptr) - delete frame->header; - delete frame; - } - } - if (m_assembler != nullptr) delete m_assembler; } @@ -307,13 +298,13 @@ void P25PacketData::processPacketFrame(const uint8_t* data, uint32_t len, bool a uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); #if !defined(_WIN32) - p25::data::IPDecodeResult decoded = m_scepService.decode(data, len); - if (decoded.result != p25::data::ConvergenceResult::OK) { + IPDecodeResult decoded = m_scepService.decode(data, len); + if (decoded.result != ConvergenceResult::OK) { LogError(LOG_P25, "VTUN SCEP packet rejected, length = %u, reason = %u", len, uint8_t(decoded.result)); return; } - const p25::data::IPv4Packet& ipPacket = decoded.packet; + const IPv4Packet& ipPacket = decoded.packet; uint32_t networkSrcAddr = htonl(ipPacket.sourceAddress); uint32_t networkDstAddr = htonl(ipPacket.destinationAddress); @@ -333,8 +324,8 @@ void P25PacketData::processPacketFrame(const uint8_t* data, uint32_t len, bool a // lazily materialize provisioned static SCEP bindings before routing getLLIdAddress(ipPacket.destinationAddress); - p25::data::IPEncodeResult route = m_scepService.encode(data, len); - if (route.result != p25::data::ConvergenceResult::OK) { + IPEncodeResult route = m_scepService.encode(data, len); + if (route.result != ConvergenceResult::OK) { LogWarning(LOG_P25, "VTUN SCEP route rejected, dstIp = %s, reason = %u", __IP_FROM_UINT(ipPacket.destinationAddress).c_str(), uint8_t(route.result)); return; @@ -352,18 +343,18 @@ void P25PacketData::processPacketFrame(const uint8_t* data, uint32_t len, bool a llId, (llId == 0U) ? " (UNRESOLVED - will retry with ARP)" : ""); // assemble a P25 PDU frame header for transport... - data::DataHeader* pktHeader = new data::DataHeader(); - bool confirmed = route.delivery == p25::data::DataDeliveryMode::CONFIRMED; - pktHeader->setFormat(confirmed ? PDUFormatType::CONFIRMED : PDUFormatType::UNCONFIRMED); - pktHeader->setMFId(MFG_STANDARD); - pktHeader->setAckNeeded(confirmed); - pktHeader->setOutbound(true); - pktHeader->setSAP(PDUSAP::PACKET_DATA); - pktHeader->setLLId(llId); - pktHeader->setBlocksToFollow(1U); - - pktHeader->calculateLength(pktLen); - uint32_t pduLength = pktHeader->getPDULength(); + data::DataHeader pktHeader; + bool confirmed = route.delivery == DataDeliveryMode::CONFIRMED; + pktHeader.setFormat(confirmed ? PDUFormatType::CONFIRMED : PDUFormatType::UNCONFIRMED); + pktHeader.setMFId(MFG_STANDARD); + pktHeader.setAckNeeded(confirmed); + pktHeader.setOutbound(true); + pktHeader.setSAP(PDUSAP::PACKET_DATA); + pktHeader.setLLId(llId); + pktHeader.setBlocksToFollow(1U); + + pktHeader.calculateLength(pktLen); + uint32_t pduLength = pktHeader.getPDULength(); if (pduLength < pktLen) { LogWarning(LOG_P25, "VTUN, data truncated!"); pktLen = pduLength; // don't overflow the buffer @@ -375,60 +366,18 @@ void P25PacketData::processPacketFrame(const uint8_t* data, uint32_t len, bool a Utils::dump(1U, "P25, P25PacketData::processPacketFrame(), pduUserData", pduUserData, pduLength); //#endif - if (pduLength > m_network->m_vtunQueueMaxBytes) { - LogWarning(LOG_P25, "VTUN queue drop, frame too large for queue cap, frameBytes = %u, capBytes = %u", - pduLength, m_network->m_vtunQueueMaxBytes); - return; - } - - uint32_t droppedFrames = 0U; - while (m_queuedFrames.size() >= m_network->m_vtunQueueMaxFrames || - (m_queuedFrameBytes + pduLength) > m_network->m_vtunQueueMaxBytes) { - if (m_queuedFrames.size() == 0U) { - break; - } - - QueuedDataFrame* oldFrame = m_queuedFrames[0U]; - m_queuedFrames.pop_front(); - if (oldFrame != nullptr) { - if (oldFrame->userDataLen <= m_queuedFrameBytes) { - m_queuedFrameBytes -= oldFrame->userDataLen; - } - else { - m_queuedFrameBytes = 0U; - } - - if (oldFrame->userData != nullptr) - delete[] oldFrame->userData; - if (oldFrame->header != nullptr) - delete oldFrame->header; - delete oldFrame; - } - - droppedFrames++; - } + ScheduledDataPacket packet; + packet.header = pktHeader; + packet.llId = llId; + packet.targetIPAddress = tgtProtoAddr; + packet.dueAt = now + INTERPACKET_DELAY; + packet.userData.assign(pduUserData, pduUserData + pduLength); + uint32_t droppedFrames = m_packetScheduler.enqueue(std::move(packet)); if (droppedFrames > 0U) { - LogWarning(LOG_P25, "VTUN queue cap reached, dropped %u oldest frame(s), queuedFrames = %u, queuedBytes = %u", - droppedFrames, (uint32_t)m_queuedFrames.size(), m_queuedFrameBytes); + LogWarning(LOG_P25, "VTUN queue cap reached, dropped %u frame(s), queuedFrames = %u, queuedBytes = %u", + droppedFrames, uint32_t(m_packetScheduler.size()), m_packetScheduler.byteCount()); } - - // queue frame for dispatch - QueuedDataFrame* qf = new QueuedDataFrame(); - qf->retryCnt = 0U; - qf->extendRetry = false; - qf->timestamp = now + INTERPACKET_DELAY; - - qf->header = pktHeader; - qf->llId = llId; - qf->tgtProtoAddr = tgtProtoAddr; - - qf->userData = new uint8_t[pduLength]; - ::memcpy(qf->userData, pduUserData, pduLength); - qf->userDataLen = pduLength; - - m_queuedFrames.push_back(qf); - m_queuedFrameBytes += pduLength; #endif // !defined(_WIN32) } @@ -505,98 +454,65 @@ void P25PacketData::clock(uint32_t ms) #if !defined(_WIN32) uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); m_conventionalDataService.expire(now); + m_locationRegistry.expire(now); - if (m_queuedFrames.size() == 0U) { + if (m_packetScheduler.empty()) { return; } - // transmit queued data frames - bool processed = false; - - auto& frame = m_queuedFrames[0]; - if (frame != nullptr) { - if (now > frame->timestamp) { - processed = true; - - if (frame->retryCnt >= MAX_PKT_RETRY_CNT && !frame->extendRetry) { - LogWarning(LOG_P25, P25_PDU_STR ", max packet retry count exceeded, dropping packet, dstIp = %s", __IP_FROM_UINT(frame->tgtProtoAddr).c_str()); - goto pkt_clock_abort; - } - - if (frame->retryCnt >= (MAX_PKT_RETRY_CNT * 2U) && frame->extendRetry) { - LogWarning(LOG_P25, P25_PDU_STR ", max packet retry count exceeded, dropping packet, dstIp = %s", __IP_FROM_UINT(frame->tgtProtoAddr).c_str()); - m_dataLinkManager.setReady(frame->llId, true); // force ready for next packet - goto pkt_clock_abort; - } - - std::string tgtIpStr = __IP_FROM_UINT(frame->tgtProtoAddr); - - // extract protocol for logging - uint8_t proto = 0x00; - if (frame->userDataLen >= 20U) - proto = frame->userData[9U]; + ScheduledDataPacket* packet = m_packetScheduler.front(); + if (packet == nullptr || now <= packet->dueAt) + return; - LogInfoEx(LOG_P25, "VTUN -> PDU IP Data (queued), dstIp = %s (%u), userDataLen = %u, proto = %02X%s, retries = %u", - tgtIpStr.c_str(), frame->llId, frame->userDataLen, proto, (proto == 0x01) ? " (ICMP)" : "", frame->retryCnt); + std::string targetIP = __IP_FROM_UINT(packet->targetIPAddress); + uint8_t protocol = packet->userData.size() >= 20U ? packet->userData[9U] : 0U; + if (packet->retryCount >= (packet->extendedRetry ? MAX_PKT_RETRY_CNT * 2U : MAX_PKT_RETRY_CNT)) { + LogWarning(LOG_P25, P25_PDU_STR ", max packet retry count exceeded, dropping packet, dstIp = %s", + targetIP.c_str()); + m_dataLinkManager.setReady(packet->llId, true); + m_packetScheduler.pop(); + return; + } - // do we have a valid target address? - if (frame->llId == 0U) { - frame->llId = getLLIdAddress(frame->tgtProtoAddr); - if (frame->llId == 0U) { - LogWarning(LOG_P25, P25_PDU_STR ", no ARP entry for, dstIp = %s", tgtIpStr.c_str()); - write_PDU_ARP(frame->tgtProtoAddr); + if (packet->llId != WUID_ALL) { + DataRoute route = m_locationRegistry.resolve(packet->llId, AccessMode::CONVENTIONAL, now); + if (!route.valid) { + LogWarning(LOG_P25, P25_PDU_STR ", subscriber location unknown, dstIp = %s (%u)", + targetIP.c_str(), packet->llId); - processed = false; - frame->timestamp = now + ARP_RETRY_MS; - frame->retryCnt++; - goto pkt_clock_abort; - } - else { - frame->header->setLLId(frame->llId); - } + UnknownLocationPolicy policy = m_locationRegistry.unknownLocationPolicy(); + if (policy == UnknownLocationPolicy::DROP) { + m_packetScheduler.pop(); + return; } + if (policy == UnknownLocationPolicy::ARP) + write_PDU_ARP(packet->targetIPAddress); - // is the SU ready for the next packet? - if (m_dataLinkManager.hasState(frame->llId)) { - if (!m_dataLinkManager.isReady(frame->llId)) { - LogWarning(LOG_P25, P25_PDU_STR ", subscriber not ready, dstIp = %s (%u), proto = %02X%s, will retry in %ums", - tgtIpStr.c_str(), frame->llId, proto, (proto == 0x01) ? " (ICMP)" : "", SUBSCRIBER_READY_RETRY_MS); - processed = false; - frame->timestamp = now + SUBSCRIBER_READY_RETRY_MS; - frame->extendRetry = true; - frame->retryCnt++; - goto pkt_clock_abort; - } - } - - m_dataLinkManager.setReady(frame->llId, false); - //LogDebugEx(LOG_P25, "P25PacketData::clock()", "dispatching queued PDU to llId %u (proto = %02X)", frame->llId, proto); - dispatchUserFrameToFNE(*frame->header, false, false, frame->userData); + packet->dueAt = now + ARP_RETRY_MS; + packet->retryCount++; + m_packetScheduler.rotate(); + return; } } -pkt_clock_abort: - m_queuedFrames.pop_front(); - if (frame != nullptr) { - if (frame->userDataLen <= m_queuedFrameBytes) { - m_queuedFrameBytes -= frame->userDataLen; - } - else { - m_queuedFrameBytes = 0U; - } + if (m_dataLinkManager.hasState(packet->llId) && + !m_dataLinkManager.isReady(packet->llId)) { + LogWarning(LOG_P25, P25_PDU_STR ", subscriber not ready, dstIp = %s (%u), proto = %02X%s, will retry in %ums", + targetIP.c_str(), packet->llId, protocol, (protocol == 0x01U) ? " (ICMP)" : "", + SUBSCRIBER_READY_RETRY_MS); + packet->dueAt = now + SUBSCRIBER_READY_RETRY_MS; + packet->extendedRetry = true; + packet->retryCount++; + m_packetScheduler.rotate(); + return; } - if (processed) { - if (frame->userData != nullptr) - delete[] frame->userData; - if (frame->header != nullptr) - delete frame->header; - delete frame; - } else { - // requeue packet - m_queuedFrames.push_back(frame); - m_queuedFrameBytes += frame->userDataLen; - } + LogInfoEx(LOG_P25, "VTUN -> PDU IP Data (queued), dstIp = %s (%u), userDataLen = %u, proto = %02X%s, retries = %u", + targetIP.c_str(), packet->llId, uint32_t(packet->userData.size()), protocol, + (protocol == 0x01U) ? " (ICMP)" : "", packet->retryCount); + m_dataLinkManager.setReady(packet->llId, false); + dispatchUserFrameToFNE(packet->header, false, false, packet->userData.data()); + m_packetScheduler.pop(); #endif // !defined(_WIN32) } @@ -712,6 +628,21 @@ void P25PacketData::dispatch(uint32_t peerId) if (status->assembler.getAuxiliaryES()) sap = status->assembler.dataHeader.getEXSAP(); + // The peer is the only channel identity carried at this boundary. Preserve + // unknown channel fields as zero until peer metadata supplies them. A + // registration establishes location only after the binding is accepted. + if (sap != PDUSAP::CONV_DATA_REG && m_locationRegistry.learnFromInbound()) { + uint32_t inboundLlId = status->assembler.getExtendedAddress() ? + status->assembler.dataHeader.getSrcLLId() : status->assembler.dataHeader.getLLId(); + if (inboundLlId != 0U && inboundLlId != WUID_ALL) { + ConventionalLocation location; + location.peerId = peerId; + location.lastSeen = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + m_locationRegistry.updateConventional(inboundLlId, location); + } + } + // handle standard P25 service access points switch (sap) { case PDUSAP::ARP: @@ -751,7 +682,7 @@ void P25PacketData::dispatch(uint32_t peerId) if (fneIPv4 == srcProtoAddr) { LogWarning(LOG_P25, P25_PDU_STR ", ARP reply, %u is trying to masquerade as us...", srcHWAddr); } else { - m_arpTable[srcHWAddr] = srcProtoAddr; + m_neighborCache.observe(srcHWAddr, srcProtoAddr); // is the SU ready for the next packet? m_dataLinkManager.setReady(srcHWAddr, true); @@ -776,21 +707,21 @@ void P25PacketData::dispatch(uint32_t peerId) if (!status->assembler.getExtendedAddress()) dstLlId = WUID_FNE; - p25::data::IPDecodeResult decoded = m_scepService.decode(status->pduUserData, + IPDecodeResult decoded = m_scepService.decode(status->pduUserData, status->pduUserDataLength); - if (decoded.result != p25::data::ConvergenceResult::OK) { + if (decoded.result != ConvergenceResult::OK) { LogError(LOG_P25, P25_PDU_STR ", SCEP uplink decode rejected, length = %u, reason = %u", status->pduUserDataLength, uint8_t(decoded.result)); break; } - const p25::data::IPv4Packet& ipPacket = decoded.packet; + const IPv4Packet& ipPacket = decoded.packet; // provisioned static entries become typed bindings on first use -- ARP // observations are intentionally not consulted for authorization getIPAddress(srcLlId); - p25::data::ConvergenceResult authorization = + ConvergenceResult authorization = m_scepService.authorizeUplink(srcLlId, ipPacket); - if (authorization != p25::data::ConvergenceResult::OK) { + if (authorization != ConvergenceResult::OK) { LogWarning(LOG_P25, P25_PDU_STR ", SCEP uplink unauthorized, llId = %u, srcIp = %s, reason = %u", srcLlId, __IP_FROM_UINT(ipPacket.sourceAddress).c_str(), uint8_t(authorization)); write_PDU_Ack_Response(PDUAckClass::NACK, PDUAckType::NACK_INVL_USER, @@ -823,12 +754,12 @@ void P25PacketData::dispatch(uint32_t peerId) bool synchronize = status->assembler.dataHeader.getSynchronize(); uint8_t expectedNs = 0U; - uint32_t packetFingerprint = p25::data::DataLinkManager::fingerprint( + uint32_t packetFingerprint = DataLinkManager::fingerprint( status->pduUserData, pktLen); - p25::data::ReceiveSequenceResult sequenceResult = + ReceiveSequenceResult sequenceResult = m_dataLinkManager.acceptReceiveSequence(srcLlId, receivedNs, synchronize, packetFingerprint, expectedNs); - if (sequenceResult == p25::data::ReceiveSequenceResult::DUPLICATE) { + if (sequenceResult == ReceiveSequenceResult::DUPLICATE) { // re-ACK a duplicate, but never deliver the IP datagram twice if (status->assembler.getExtendedAddress()) { write_PDU_Ack_Response(PDUAckClass::ACK, PDUAckType::ACK, receivedNs, @@ -839,7 +770,7 @@ void P25PacketData::dispatch(uint32_t peerId) } break; } - if (sequenceResult == p25::data::ReceiveSequenceResult::OUT_OF_SEQUENCE) { + if (sequenceResult == ReceiveSequenceResult::OUT_OF_SEQUENCE) { // out of sequence - send NACK_OUT_OF_SEQ LogWarning(LOG_P25, P25_PDU_STR ", NACK_OUT_OF_SEQ, llId %u, expected N(S) %u or %u, received N(S) = %u", srcLlId, expectedNs, (expectedNs + 1) % 8, receivedNs); @@ -942,6 +873,36 @@ void P25PacketData::dispatchToFNE(uint32_t peerId) uint32_t srcId = (status->assembler.getExtendedAddress()) ? status->assembler.dataHeader.getSrcLLId() : status->assembler.dataHeader.getLLId(); uint32_t dstId = status->assembler.dataHeader.getLLId(); + std::vector routes; + bool groupDelivery = dstId == WUID_ALL; + if (groupDelivery) { + std::vector availablePeers; + for (const auto& peer : m_network->m_peers) + availablePeers.push_back(peer.first); + + for (const auto& peer : m_network->m_host->m_peerNetworks) { + if (peer.second->isEnabled()) + availablePeers.push_back(peer.second->getPeerId()); + } + + routes = m_locationRegistry.resolveGroup(availablePeers); + } + else { + uint64_t now = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + DataRoute route = m_locationRegistry.resolve(dstId, AccessMode::CONVENTIONAL, now); + if (!route.valid) { + LogWarning(LOG_P25, P25_PDU_STR ", no RF route for relayed packet, llId = %u", dstId); + return; + } + + routes.push_back(route); + } + + auto selectedPeer = [&routes](uint32_t candidatePeerId) { + return std::any_of(routes.cbegin(), routes.cend(), [candidatePeerId](const DataRoute& route) { return route.peerId == candidatePeerId; }); + }; + /* ** MASTER TRAFFIC */ @@ -949,7 +910,7 @@ void P25PacketData::dispatchToFNE(uint32_t peerId) // repeat traffic to the connected peers if (m_network->m_peers.size() > 0U) { for (auto peer : m_network->m_peers) { - if (peerId != peer.first) { + if (peerId != peer.first && selectedPeer(peer.first)) { write_PDU_User(peer.first, peerId, nullptr, status->assembler.dataHeader, status->assembler.getExtendedAddress(), status->assembler.getAuxiliaryES(), status->pduUserData); if (m_network->m_debug) { @@ -971,7 +932,7 @@ void P25PacketData::dispatchToFNE(uint32_t peerId) // don't try to repeat traffic to the source peer...if this traffic // is coming from a neighbor FNE peer - if (dstPeerId != peerId) { + if (dstPeerId != peerId && selectedPeer(dstPeerId)) { // skip peer if it isn't enabled if (!peer.second->isEnabled()) { continue; @@ -990,32 +951,59 @@ void P25PacketData::dispatchToFNE(uint32_t peerId) /* Helper to dispatch PDU user data back to the local FNE network. (Will not transmit to neighbor FNE peers.) */ -void P25PacketData::dispatchUserFrameToFNE(p25::data::DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData) +void P25PacketData::dispatchUserFrameToFNE(DataHeader& dataHeader, bool extendedAddress, bool auxiliaryES, uint8_t* pduUserData) { uint32_t srcId = (extendedAddress) ? dataHeader.getSrcLLId() : dataHeader.getLLId(); uint32_t dstId = dataHeader.getLLId(); - // update V(S) independently for each logical link + std::vector routes; + if (dstId == WUID_ALL) { + std::vector availablePeers; + for (const auto& peer : m_network->m_peers) + availablePeers.push_back(peer.first); + + routes = m_locationRegistry.resolveGroup(availablePeers); + } + else { + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + DataRoute route = m_locationRegistry.resolve(dstId, AccessMode::CONVENTIONAL, now); + if (route.valid) + routes.push_back(route); + } + + if (routes.empty()) { + LogWarning(LOG_P25, P25_PDU_STR ", no RF route for downlink, llId = %u", dstId); + return; + } + + // update V(S) only after a usable RF route has been selected bool synchronize = false; uint8_t sendSequence = m_dataLinkManager.nextSendSequence(srcId, synchronize); - if (synchronize) { + if (synchronize) dataHeader.setSynchronize(true); - } dataHeader.setNs(sendSequence); - /* - ** MASTER TRAFFIC - */ - - // repeat traffic to the connected peers - if (m_network->m_peers.size() > 0U) { - for (auto peer : m_network->m_peers) { - write_PDU_User(peer.first, m_network->m_peerId, nullptr, dataHeader, extendedAddress, auxiliaryES, pduUserData); - if (m_network->m_debug) { - LogDebug(LOG_P25, "dstPeer = %u, duid = $%02X, srcId = %u, dstId = %u", - peer.first, DUID::PDU, srcId, dstId); + // dispatch the user frame to each resolved RF route + for (const DataRoute& route : routes) { + auto localPeer = m_network->m_peers.find(route.peerId); + if (localPeer != m_network->m_peers.end()) { + write_PDU_User(route.peerId, m_network->m_peerId, nullptr, dataHeader, + extendedAddress, auxiliaryES, pduUserData); + } + else { + for (const auto& peer : m_network->m_host->m_peerNetworks) { + if (peer.second->getPeerId() == route.peerId && peer.second->isEnabled()) { + write_PDU_User(route.peerId, m_network->m_peerId, peer.second, + dataHeader, extendedAddress, auxiliaryES, pduUserData); + break; + } } } + + if (m_network->m_debug) { + LogDebug(LOG_P25, "dstPeer = %u, duid = $%02X, srcId = %u, dstId = %u", + route.peerId, DUID::PDU, srcId, dstId); + } } } @@ -1028,14 +1016,14 @@ bool P25PacketData::processConvDataReg(RxStatus* status) return false; } - p25::data::ConventionalRegistration registration; - if (!p25::data::ConventionalRegistration::decode(status->pduUserData, + ConventionalRegistration registration; + if (!ConventionalRegistration::decode(status->pduUserData, status->pduUserDataLength, registration)) { LogError(LOG_P25, P25_PDU_STR ", invalid conventional registration payload"); return false; } - p25::data::ConventionalRegistrationProvisioning provisioning; + ConventionalRegistrationProvisioning provisioning; lookups::RadioId rid = m_network->m_ridLookup->find(registration.llId); provisioning.known = !rid.radioDefault(); provisioning.enabled = provisioning.known && rid.radioEnabled(); @@ -1045,18 +1033,26 @@ bool P25PacketData::processConvDataReg(RxStatus* status) uint32_t headerLlId = status->assembler.getExtendedAddress() ? status->assembler.dataHeader.getSrcLLId() : status->assembler.dataHeader.getLLId(); - p25::data::ConventionalRegistrationResult result = m_conventionalDataService.process( + ConventionalRegistrationResult result = m_conventionalDataService.process( status->pduUserData, status->pduUserDataLength, headerLlId, provisioning); - if (result.decision == p25::data::RegistrationDecision::DISCONNECTED) { - m_arpTable.erase(registration.llId); + if (result.decision == RegistrationDecision::DISCONNECTED) { + m_neighborCache.erase(registration.llId); + m_locationRegistry.erase(registration.llId, AccessMode::CONVENTIONAL); LogInfoEx(LOG_P25, P25_PDU_STR ", DISCONNECT, llId = %u", registration.llId); return true; } + if (result.decision == RegistrationDecision::ACCEPT) { + ConventionalLocation location; + location.peerId = status->peerId; + location.lastSeen = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + m_locationRegistry.updateConventional(registration.llId, location); + } + // only the FNE emits the registration decision -- the host merely validates // and forwards the request, so an ACCEPT always reflects installed state - uint8_t responseData[p25::data::ConventionalRegistration::LENGTH]; + uint8_t responseData[ConventionalRegistration::LENGTH]; if (!result.response.encode(responseData, sizeof(responseData))) return false; @@ -1070,14 +1066,14 @@ bool P25PacketData::processConvDataReg(RxStatus* status) responseHeader.calculateLength(sizeof(responseData)); dispatchUserFrameToFNE(responseHeader, false, false, responseData); - if (result.decision == p25::data::RegistrationDecision::ACCEPT) { + if (result.decision == RegistrationDecision::ACCEPT) { LogInfoEx(LOG_P25, P25_PDU_STR ", registration ACCEPT, llId = %u, ipAddr = %s", registration.llId, __IP_FROM_UINT(result.response.ipAddress).c_str()); return true; } - LogWarning(LOG_P25, P25_PDU_STR ", registration DENY, llId = %u, reason = %u", - registration.llId, uint8_t(result.denyReason)); + LogWarning(LOG_P25, P25_PDU_STR ", registration DENY, llId = %u, reason = %u (%s)", + registration.llId, uint8_t(result.denyReason), ConventionalDataService::denyReasonToString(result.denyReason).c_str()); return false; } @@ -1190,7 +1186,7 @@ bool P25PacketData::processSNDCPControl(RxStatus* status) rspHeader.setBlocksToFollow(1U); rspHeader.calculateLength(13U); - m_arpTable[llId] = staticIP; + m_neighborCache.observe(llId, staticIP); m_dataLinkManager.setReady(llId, true); dispatchUserFrameToFNE(rspHeader, false, false, txPduUserData); @@ -1255,7 +1251,7 @@ bool P25PacketData::processSNDCPControl(RxStatus* status) rspHeader.setBlocksToFollow(1U); rspHeader.calculateLength(13U); - m_arpTable[llId] = dynamicIP; + m_neighborCache.observe(llId, dynamicIP); m_dataLinkManager.setReady(llId, true); dispatchUserFrameToFNE(rspHeader, false, false, txPduUserData); @@ -1301,7 +1297,7 @@ bool P25PacketData::processSNDCPControl(RxStatus* status) LogInfoEx(LOG_P25, P25_PDU_STR ", SNDCP context deactivation request, llId = %u, deactType = %02X", llId, isp->getDeactType()); - m_arpTable.erase(llId); + m_neighborCache.erase(llId); m_dataLinkManager.erase(llId); // send ACK response @@ -1449,7 +1445,7 @@ void P25PacketData::write_PDU_User(uint32_t peerId, uint32_t srcPeerId, network: /* Write data processed to the network. */ -bool P25PacketData::writeNetwork(uint32_t peerId, uint32_t srcPeerId, network::PeerNetwork* peerNet, const p25::data::DataHeader& dataHeader, const uint8_t currentBlock, +bool P25PacketData::writeNetwork(uint32_t peerId, uint32_t srcPeerId, network::PeerNetwork* peerNet, const DataHeader& dataHeader, const uint8_t currentBlock, const uint8_t *data, uint32_t len, uint16_t pktSeq, uint32_t streamId) { assert(data != nullptr); @@ -1475,18 +1471,8 @@ bool P25PacketData::hasARPEntry(uint32_t llId) const 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; - } + const RouteNeighbor* neighbor = m_neighborCache.findByLLId(llId); + return neighbor != nullptr && neighbor->ipAddress != 0U; } /* Helper to get the IP address for the given logical link ID. */ @@ -1497,7 +1483,7 @@ uint32_t P25PacketData::getIPAddress(uint32_t llId) return 0U; } - const p25::data::IPBinding* binding = m_bindingRegistry.findByLLId(llId); + const IPBinding* binding = m_bindingRegistry.findByLLId(llId); if (binding != nullptr && binding->authorized) { return binding->ipAddress; } @@ -1522,7 +1508,7 @@ uint32_t P25PacketData::getLLIdAddress(uint32_t addr) return 0U; } - const p25::data::IPBinding* binding = m_bindingRegistry.findByIPAddress(addr); + const IPBinding* binding = m_bindingRegistry.findByIPAddress(addr); if (binding != nullptr && binding->authorized) return binding->link.llId; @@ -1561,18 +1547,13 @@ uint32_t P25PacketData::allocateIPAddress(uint32_t llId) nextIP = m_network->m_sndcpStartAddr; } - // build set of already-allocated IPs to ensure uniqueness - std::unordered_set allocatedIPs; - for (const auto& entry : m_arpTable) { - allocatedIPs.insert(entry.second); - } - // find next available IP not already in use uint32_t candidateIP = nextIP; const uint32_t poolSize = m_network->m_sndcpEndAddr - m_network->m_sndcpStartAddr + 1U; uint32_t attempts = 0U; - while (allocatedIPs.find(candidateIP) != allocatedIPs.end() && attempts < poolSize) { + while ((m_neighborCache.findByIPAddress(candidateIP) != nullptr || + m_bindingRegistry.findByIPAddress(candidateIP) != nullptr) && attempts < poolSize) { candidateIP++; // wrap around if we exceed the end address @@ -1598,7 +1579,7 @@ uint32_t P25PacketData::allocateIPAddress(uint32_t llId) nextIP = m_network->m_sndcpStartAddr; } - m_arpTable[llId] = allocatedIP; + m_neighborCache.observe(llId, allocatedIP); LogInfoEx(LOG_P25, P25_PDU_STR ", SNDCP allocated dynamic IP %s to llId = %u (pool: %s - %s)", __IP_FROM_UINT(allocatedIP).c_str(), llId, __IP_FROM_UINT(m_network->m_sndcpStartAddr).c_str(), __IP_FROM_UINT(m_network->m_sndcpEndAddr).c_str()); diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.h b/src/fne/network/callhandler/packetdata/P25PacketData.h index 89200649..509e6a07 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.h +++ b/src/fne/network/callhandler/packetdata/P25PacketData.h @@ -18,21 +18,20 @@ #include "fne/Defines.h" #include "common/Clock.h" -#include "common/concurrent/deque.h" #include "common/concurrent/unordered_map.h" #include "common/p25/P25Defines.h" #include "common/p25/data/Assembler.h" #include "common/p25/data/ConventionalDataService.h" #include "common/p25/data/IPConvergenceService.h" #include "common/p25/data/PacketDataState.h" +#include "common/p25/data/DataRouting.h" +#include "common/p25/data/PacketScheduler.h" #include "common/p25/data/DataHeader.h" #include "common/p25/data/DataBlock.h" #include "network/TrafficNetwork.h" #include "network/PeerNetwork.h" #include "network/callhandler/TagP25Data.h" -#include - namespace network { namespace callhandler @@ -137,24 +136,7 @@ namespace network uint32_t streamId; //!< Stream ID. }; - /** - * @brief Represents a queued data frame from the VTUN. - */ - class QueuedDataFrame { - public: - p25::data::DataHeader* header; //!< Instance of a PDU data header. - uint32_t llId; //!< Logical Link ID - uint32_t tgtProtoAddr; //!< Target Protocol Address - - uint8_t* userData; //!< Raw data buffer - uint32_t userDataLen; //!< Length of raw data buffer - - uint64_t timestamp; //!< Timestamp in milliseconds - uint8_t retryCnt; //!< Packet Retry Counter - bool extendRetry; //!< Flag indicating whether or not to extend the retry count for this packet. - }; - concurrent::deque m_queuedFrames; - uint32_t m_queuedFrameBytes; + p25::data::PacketScheduler m_packetScheduler; /** * @brief Represents the receive status of a call. @@ -229,9 +211,9 @@ namespace network typedef std::pair StatusMapPair; concurrent::unordered_map m_status; - typedef std::pair ArpTablePair; - std::unordered_map m_arpTable; + p25::data::RouteNeighborCache m_neighborCache; p25::data::DataBindingRegistry m_bindingRegistry; + p25::data::DataLocationRegistry m_locationRegistry; p25::data::DataLinkManager m_dataLinkManager; p25::data::ConventionalDataService m_conventionalDataService; p25::data::SCEPService m_scepService; diff --git a/tests/p25/P25_PacketDataState_Tests.cpp b/tests/p25/P25_PacketDataState_Tests.cpp index 5f550912..a326f232 100644 --- a/tests/p25/P25_PacketDataState_Tests.cpp +++ b/tests/p25/P25_PacketDataState_Tests.cpp @@ -14,6 +14,8 @@ #include "common/p25/data/IPConvergenceService.h" #include "common/p25/data/IPv4Packet.h" #include "common/p25/data/PacketDataState.h" +#include "common/p25/data/DataRouting.h" +#include "common/p25/data/PacketScheduler.h" using namespace p25::data; using namespace p25::defines; @@ -21,41 +23,54 @@ using namespace p25::defines; #include #include +#include #include -namespace -{ -std::array registrationPacket( - uint8_t type, uint32_t llId, uint32_t ipAddress, uint8_t options = 0U) -{ - ConventionalRegistration registration; - registration.type = type; - registration.options = options; - registration.llId = llId; - registration.ipAddress = ipAddress; - std::array data {}; - REQUIRE(registration.encode(data.data(), data.size())); - return data; -} // namespace - -std::vector ipv4Packet(uint32_t source, uint32_t destination, - uint16_t length = 20U) -{ - std::vector packet(length, 0U); - packet[0U] = 0x45U; - packet[2U] = uint8_t(length >> 8U); - packet[3U] = uint8_t(length); - packet[9U] = 17U; - packet[12U] = uint8_t(source >> 24U); - packet[13U] = uint8_t(source >> 16U); - packet[14U] = uint8_t(source >> 8U); - packet[15U] = uint8_t(source); - packet[16U] = uint8_t(destination >> 24U); - packet[17U] = uint8_t(destination >> 16U); - packet[18U] = uint8_t(destination >> 8U); - packet[19U] = uint8_t(destination); - return packet; -} +namespace { + /** + * @brief Creates an encoded conventional registration packet. + * @param type The type of the conventional registration packet. + * @param llId The logical link ID. + * @param ipAddress The IP address. + * @param options Optional registration options. + * @return Encoded conventional registration packet as a byte array. + */ + std::array registrationPacket(uint8_t type, uint32_t llId, uint32_t ipAddress, uint8_t options = 0U) + { + ConventionalRegistration registration; + registration.type = type; + registration.options = options; + registration.llId = llId; + registration.ipAddress = ipAddress; + std::array data {}; + REQUIRE(registration.encode(data.data(), data.size())); + return data; + } + + /** + * @brief Creates an IPv4 packet with the specified source and destination addresses. + * @param source The source IP address. + * @param destination The destination IP address. + * @param length The total length of the IPv4 packet (default is 20 bytes). + * @return Encoded IPv4 packet as a byte vector. + */ + std::vector ipv4Packet(uint32_t source, uint32_t destination, uint16_t length = 20U) + { + std::vector packet(length, 0U); + packet[0U] = 0x45U; + packet[2U] = uint8_t(length >> 8U); + packet[3U] = uint8_t(length); + packet[9U] = 17U; + packet[12U] = uint8_t(source >> 24U); + packet[13U] = uint8_t(source >> 16U); + packet[14U] = uint8_t(source >> 8U); + packet[15U] = uint8_t(source); + packet[16U] = uint8_t(destination >> 24U); + packet[17U] = uint8_t(destination >> 16U); + packet[18U] = uint8_t(destination >> 8U); + packet[19U] = uint8_t(destination); + return packet; + } } TEST_CASE("P25 conventional registration codec is length safe", "[p25][packet-data][conventional][security]") @@ -351,3 +366,152 @@ TEST_CASE("P25 SCEP enforces binding, IP ownership, MTU, and delivery policy", CHECK(noBinding.authorizeUplink(100U, decoded.packet) == ConvergenceResult::NO_BINDING); CHECK(noBinding.encode(packet.data(), packet.size()).result == ConvergenceResult::NO_BINDING); } + +TEST_CASE("P25 conventional locations move, expire, and resolve targeted routes", + "[p25][packet-data][location][routing]") +{ + DataLocationRegistry locations(100U); + ConventionalLocation location; + location.peerId = 10U; + location.channelId = 1U; + location.channelNo = 101U; + location.lastSeen = 1000U; + REQUIRE(locations.updateConventional(1001U, location)); + + DataRoute route = locations.resolve(1001U, AccessMode::CONVENTIONAL, 1050U); + REQUIRE(route.valid); + CHECK(route.peerId == 10U); + CHECK(route.channelId == 1U); + CHECK(route.channelNo == 101U); + + // Later inbound traffic moves the subscriber to peer B. + location.peerId = 20U; + location.channelId = 2U; + location.channelNo = 202U; + location.lastSeen = 1060U; + REQUIRE(locations.updateConventional(1001U, location)); + route = locations.resolve(1001U, AccessMode::CONVENTIONAL, 1070U); + REQUIRE(route.valid); + CHECK(route.peerId == 20U); + + locations.expire(1160U); + CHECK_FALSE(locations.resolve(1001U, AccessMode::CONVENTIONAL, 1160U).valid); +} + +TEST_CASE("P25 group and trunked route state coexist with conventional locations", + "[p25][packet-data][location][routing]") +{ + DataLocationRegistry locations; + ConventionalLocation conventional; + conventional.peerId = 20U; + conventional.lastSeen = 1U; + REQUIRE(locations.updateConventional(1001U, conventional)); + + DataRoute trunked; + trunked.mode = AccessMode::TRUNKED; + trunked.peerId = 30U; + trunked.channelId = 4U; + trunked.channelNo = 404U; + trunked.grantId = 77U; + trunked.lastSeen = 1U; + REQUIRE(locations.updateTrunked(2002U, trunked)); + + CHECK(locations.resolve(1001U, AccessMode::CONVENTIONAL).peerId == 20U); + CHECK(locations.resolve(2002U, AccessMode::TRUNKED).peerId == 30U); + CHECK_FALSE(locations.resolve(2002U, AccessMode::CONVENTIONAL).valid); + + locations.setGroupPeers({ 20U, 40U }); + std::vector routes = locations.resolveGroup({ 10U, 20U, 30U, 40U }); + REQUIRE(routes.size() == 2U); + CHECK(routes[0U].peerId == 20U); + CHECK(routes[1U].peerId == 40U); +} + +TEST_CASE("P25 unknown conventional location behavior is policy controlled", + "[p25][packet-data][location][policy]") +{ + DataLocationRegistry locations(100U, false, UnknownLocationPolicy::QUEUE); + CHECK_FALSE(locations.learnFromInbound()); + CHECK(locations.unknownLocationPolicy() == UnknownLocationPolicy::QUEUE); + + locations.setLearnFromInbound(true); + locations.setUnknownLocationPolicy(UnknownLocationPolicy::ARP); + CHECK(locations.learnFromInbound()); + CHECK(locations.unknownLocationPolicy() == UnknownLocationPolicy::ARP); + + locations.setUnknownLocationPolicy(UnknownLocationPolicy::DROP); + CHECK(locations.unknownLocationPolicy() == UnknownLocationPolicy::DROP); +} + +TEST_CASE("P25 neighbor observations do not create bindings or routes", + "[p25][packet-data][neighbor][security]") +{ + RouteNeighborCache neighbors; + DataBindingRegistry bindings; + DataLocationRegistry locations; + + neighbors.observe(1001U, 0x0A000001U, 10U); + REQUIRE(neighbors.findByLLId(1001U) != nullptr); + CHECK(neighbors.findByIPAddress(0x0A000001U)->llId == 1001U); + CHECK(bindings.findByLLId(1001U) == nullptr); + CHECK_FALSE(locations.resolve(1001U, AccessMode::CONVENTIONAL).valid); +} + +TEST_CASE("P25 packet scheduler bounds and rotates queued downlinks", + "[p25][packet-data][scheduler]") +{ + PacketScheduler scheduler(2U, 8U); + ScheduledDataPacket first; + first.llId = 1U; + first.userData.assign(4U, 0x11U); + CHECK(scheduler.enqueue(std::move(first)) == 0U); + + ScheduledDataPacket second; + second.llId = 2U; + second.userData.assign(4U, 0x22U); + CHECK(scheduler.enqueue(std::move(second)) == 0U); + REQUIRE(scheduler.front() != nullptr); + CHECK(scheduler.front()->llId == 1U); + + scheduler.rotate(); + CHECK(scheduler.front()->llId == 2U); + + ScheduledDataPacket third; + third.llId = 3U; + third.userData.assign(4U, 0x33U); + CHECK(scheduler.enqueue(std::move(third)) == 1U); + CHECK(scheduler.size() == 2U); + CHECK(scheduler.byteCount() == 8U); +} + +TEST_CASE("P25 scheduled packet header assignment owns an independent copy", + "[p25][packet-data][scheduler][memory]") +{ + PacketScheduler scheduler(1U, 64U); + std::array originalMI { + 0x01U, 0x23U, 0x45U, 0x67U, 0x89U, 0xABU, 0xCDU, 0xEFU, 0x10U + }; + + { + DataHeader source; + source.setLLId(1001U); + source.setSAP(PDUSAP::PACKET_DATA); + source.setMI(originalMI.data()); + + ScheduledDataPacket packet; + packet.header = source; + packet.userData.assign(20U, 0x55U); + REQUIRE(scheduler.enqueue(std::move(packet)) == 0U); + } + + REQUIRE(scheduler.front() != nullptr); + CHECK(scheduler.front()->header.getLLId() == 1001U); + CHECK(scheduler.front()->header.getSAP() == PDUSAP::PACKET_DATA); + + std::array copiedMI {}; + scheduler.front()->header.getMI(copiedMI.data()); + CHECK(copiedMI == originalMI); + + scheduler.clear(); + CHECK(scheduler.empty()); +}