[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

@@ -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
+        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

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

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.

src/common/network/viface/VIFace.cpp

@@ -19,6 +19,7 @@ using namespace network::viface;
 #include <sstream>
 #include <fstream>
 #include <iomanip>
+#include <random>
 
 #include <cassert>
 #include <cstring>
@@ -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<uint8_t> 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 << ":";
         }

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.
              *

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 */

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, ss0Pos, true);              // 1
-        WRITE_BIT(data, ss1Pos, false); // 0
+        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);
+        if (inbound) {
-        WRITE_BIT(data, ss1Pos, b2);
+            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, ss0Pos, true);              // 1
-        WRITE_BIT(data, ss1Pos, b2);
+        WRITE_BIT(data, ss1Pos, true);              // 1
     }
 }
 

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 inbound Flag indicating inbound channel is busy.
-         * @param b2 Status Bit 2
+         * @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.

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) {

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.

src/fne/HostFNE.cpp

@@ -21,6 +21,7 @@
 #include "FNEMain.h"
 
 using namespace network;
+using namespace network::viface;
 using namespace lookups;
 
 #include <cstdio>
@@ -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<bool>(false);
+    if (vtunEnabled) {
+        m_vtunEnabled = vtunEnabled;
+
+        std::string vtunName = vtunConf["interfaceName"].as<std::string>("fne0");
+        std::string ipv4Address = vtunConf["address"].as<std::string>("192.168.1.254");
+        std::string ipv4Netmask = vtunConf["netmask"].as<std::string>("255.255.255.0");
+        std::string ipv4Broadcast = vtunConf["broadcast"].as<std::string>("192.168.1.255");
+        std::string packetDataModeStr = vtunConf["digitalMode"].as<std::string>("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)

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.

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",

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.
              */

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 <cassert>
 #include <chrono>
 
+#include <netinet/ip.h>
+
+// ---------------------------------------------------------------------------
+//  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<sndcp::SNDCPPacket> 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<SNDCPCtxActRequest*>(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<SNDCPCtxDeactivation*>(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;
+}

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<VTUNDataFrame> m_dataFrames;
+
                 /**
                  * @brief Represents the receive status of a call.
                  */
@@ -130,6 +150,8 @@ namespace network
                 typedef std::pair<const uint32_t, RxStatus*> StatusMapPair;
                 std::unordered_map<uint32_t, RxStatus*> m_status;
 
+                std::unordered_map<uint32_t, uint32_t> 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

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);

src/host/p25/packet/ControlSignaling.cpp

@@ -184,6 +184,8 @@ bool ControlSignaling::process(uint8_t* data, uint32_t len, std::unique_ptr<lc::
 
     // handle individual DUIDs
     if (duid == DUID::TSDU) {
+        m_inbound = true;
+
         if (m_p25->m_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_ptr<lc::
             m_p25->writeRF_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);
+        P25Utils::addStatusBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS, m_inbound, true);
-
+        P25Utils::addTrunkSlotStatusBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS);
-        // Add idle bits
-        P25Utils::addIdleBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS, true, true);
 
         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. */

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;

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);
+            ::memset(m_rfPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
-            m_pduUserDataLength = 0U;
+            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];
+                        uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH];
-                        ::memset(arpPacket, 0x00U, 22U);
+                        ::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH);
-                        ::memcpy(arpPacket, m_pduUserData + 12U, 22U);
+                        ::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_rfPduUserData(nullptr),
-    m_pduUserDataLength(0U),
+    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];
+    m_rfPduUserData = 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);
+    ::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 llId = (pduUserData[1U] << 16) + (pduUserData[2U] << 8) + pduUserData[3U];
-        uint32_t ipAddr = (m_pduUserData[8U] << 24) + (m_pduUserData[9U] << 16) + (m_pduUserData[10U] << 8) + m_pduUserData[11U];
+        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];
+    uint8_t txPduUserData[P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES];
-    ::memset(pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES);
+    ::memset(txPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES);
 
-    std::unique_ptr<sndcp::SNDCPPacket> packet = SNDCPFactory::create(m_pduUserData);
+    std::unique_ptr<sndcp::SNDCPPacket> 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
+    // Add status bits
-    P25Utils::addBusyBits(data + 2U, newBitLength, true, false);
+    P25Utils::addStatusBits(data + 2U, newBitLength, false);
+    P25Utils::addIdleStatusBits(data + 2U, newBitLength);
 
-    // Add idle bits
+    // Set first busy bits to 1,1
-    P25Utils::addIdleBits(data + 2U, newBitLength, true, true);
+    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",

src/host/p25/packet/Data.h

@@ -140,8 +140,10 @@ namespace p25
             uint32_t m_retryPDUBitLength;
             uint8_t m_retryCount;
 
-            uint8_t* m_pduUserData;
+            uint8_t* m_rfPduUserData;
-            uint32_t m_pduUserDataLength;
+            uint32_t m_rfPduUserDataLength;
+            uint8_t* m_netPduUserData;
+            uint32_t m_netPduUserDataLength;
 
             std::unordered_map<uint32_t, uint32_t> m_fneRegTable;
 
@@ -152,6 +154,8 @@ namespace p25
             std::unordered_map<uint32_t, Timer> m_sndcpReadyTimers;
             std::unordered_map<uint32_t, Timer> 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.

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;

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;
 

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;