From 2f0c30168565ffade9b22ec76771108e435a1b28 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Thu, 4 Dec 2025 22:30:48 -0500 Subject: [PATCH] implement a very preliminary DMR to VTUN IP dispatcher based on the P25 implementation (while I am sure this is probably going to be quite iffy, I am quite sure the VTUN TAP interface method we are using *is* the right direction; even though the P25 implementation is iffy, with iteration over time I suspect it is the correct approach, as such I have implemented a DMR equiviliant based on the P25 implementation); --- src/common/dmr/DMRDefines.h | 27 +- src/fne/HostFNE.cpp | 9 +- .../callhandler/packetdata/DMRPacketData.cpp | 513 ++++++++++++++++++ .../callhandler/packetdata/DMRPacketData.h | 87 +++ 4 files changed, 630 insertions(+), 6 deletions(-) diff --git a/src/common/dmr/DMRDefines.h b/src/common/dmr/DMRDefines.h index 187912dd..d8e240fd 100644 --- a/src/common/dmr/DMRDefines.h +++ b/src/common/dmr/DMRDefines.h @@ -120,6 +120,12 @@ namespace dmr const uint32_t DMR_PDU_CONFIRMED_HR_DATA_LENGTH_BYTES = 10U; const uint32_t DMR_PDU_CONFIRMED_UNCODED_DATA_LENGTH_BYTES = 22U; + const uint32_t DMR_PDU_ARP_PCKT_LENGTH = 22U; + const uint32_t DMR_PDU_ARP_HW_ADDR_LENGTH = 3U; + const uint32_t DMR_PDU_ARP_PROTO_ADDR_LENGTH = 4U; + + const uint8_t DMR_PDU_ARP_CAI_TYPE = 0x21U; + const uint32_t MI_LENGTH_BYTES = 4U; // This was guessed based on OTA data captures -- the message indicator seems to be the same length as a source/destination address const uint32_t RAW_AMBE_LENGTH_BYTES = 9U; /** @} */ @@ -149,6 +155,7 @@ namespace dmr const uint16_t DMR_LOGICAL_CH_ABSOLUTE = 0xFFFU; + const uint32_t WUID_IPI = 0xFFFEC3U; //!< IP Interface Working Unit ID const uint32_t WUID_SUPLI = 0xFFFEC4U; //!< Supplementary Data Service Working Unit ID const uint32_t WUID_SDMI = 0xFFFEC5U; //!< UDT Short Data Service Working Unit ID const uint32_t WUID_REGI = 0xFFFEC6U; //!< Registration Working Unit ID @@ -179,6 +186,22 @@ namespace dmr }; }; + /** @brief Service Access Point */ + namespace PDUSAP { + /** @brief Service Access Point */ + enum : uint8_t { + UDT = 0x00U, //!< Unified Data Transport Header + + PACKET_DATA = 0x04U, //!< IP based Packet Data + + ARP = 0x05U, //!< ARP + + PROP_PACKET_DATA = 0x09U, //!< Proprietary Packet Data + + SHORT_DATA = 0x0AU //!< Defined Short Data + }; + } + /** @brief Data Response Class */ namespace PDUResponseClass { /** @brief Data Response Class */ @@ -187,7 +210,7 @@ namespace dmr NACK = 0x01U, //!< Negative Acknowledge ACK_RETRY = 0x02U //!< Acknowlege Retry }; - }; + } /** @brief Data Response Type */ namespace PDUResponseType { @@ -200,7 +223,7 @@ namespace dmr NACK_MEMORY_FULL = 0x02U, //!< Memory Full NACK_UNDELIVERABLE = 0x04U //!< Undeliverable }; - }; + } /** @brief ARP Request */ const uint8_t DMR_PDU_ARP_REQUEST = 0x01U; diff --git a/src/fne/HostFNE.cpp b/src/fne/HostFNE.cpp index 1534a5f1..0a8f1915 100644 --- a/src/fne/HostFNE.cpp +++ b/src/fne/HostFNE.cpp @@ -49,6 +49,7 @@ using namespace lookups; #define IDLE_WARMUP_MS 5U #define DEFAULT_MTU_SIZE 496 +#define MAX_MTU_SIZE 65535 // --------------------------------------------------------------------------- // Public Class Members @@ -258,7 +259,7 @@ int HostFNE::run() if (m_vtunEnabled) { switch (m_packetDataMode) { case PacketDataMode::DMR: - // TODO: not supported yet + m_network->dmrTrafficHandler()->packetData()->clock(ms); break; case PacketDataMode::PROJECT25: @@ -998,14 +999,14 @@ void* HostFNE::threadVirtualNetworking(void* arg) uint32_t ms = stopWatch.elapsed(); stopWatch.start(); - uint8_t packet[DEFAULT_MTU_SIZE]; - ::memset(packet, 0x00U, DEFAULT_MTU_SIZE); + uint8_t packet[MAX_MTU_SIZE]; + ::memset(packet, 0x00U, MAX_MTU_SIZE); ssize_t len = fne->m_tun->read(packet); if (len > 0) { switch (fne->m_packetDataMode) { case PacketDataMode::DMR: - // TODO: not supported yet + fne->m_network->dmrTrafficHandler()->packetData()->processPacketFrame(packet, (uint32_t)len); break; case PacketDataMode::PROJECT25: diff --git a/src/fne/network/callhandler/packetdata/DMRPacketData.cpp b/src/fne/network/callhandler/packetdata/DMRPacketData.cpp index 7079b7a4..db4f26d4 100644 --- a/src/fne/network/callhandler/packetdata/DMRPacketData.cpp +++ b/src/fne/network/callhandler/packetdata/DMRPacketData.cpp @@ -9,6 +9,7 @@ */ #include "fne/Defines.h" #include "common/dmr/data/DataBlock.h" +#include "common/dmr/data/Assembler.h" #include "common/dmr/SlotType.h" #include "common/dmr/Sync.h" #include "common/edac/CRC.h" @@ -30,11 +31,19 @@ using namespace dmr::defines; #include #include +#if !defined(_WIN32) +#include +#endif // !defined(_WIN32) + // --------------------------------------------------------------------------- // Constants // --------------------------------------------------------------------------- const uint8_t DATA_CALL_COLL_TIMEOUT = 60U; +const uint8_t MAX_PKT_RETRY_CNT = 2U; + +const uint32_t INTERPACKET_DELAY = 100U; // milliseconds +const uint32_t ARP_RETRY_MS = 5000U; // milliseconds // --------------------------------------------------------------------------- // Public Class Members @@ -46,6 +55,10 @@ DMRPacketData::DMRPacketData(FNENetwork* network, TagDMRData* tag, bool debug) : m_network(network), m_tag(tag), m_status(), + m_queuedFrames(), + m_arpTable(), + m_readyForNextPkt(), + m_suSendSeq(), m_debug(debug) { assert(network != nullptr); @@ -268,6 +281,164 @@ bool DMRPacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee return true; } +/* Process a data frame from the virtual IP network. */ + +void DMRPacketData::processPacketFrame(const uint8_t* data, uint32_t len, bool alreadyQueued) +{ + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + +#if !defined(_WIN32) + // validate minimum IPv4 header size + if (len < sizeof(struct ip)) { + LogError(LOG_DMR, "DMR, packet too short for IP header"); + return; + } + + // check IP version (must be IPv4) + if ((data[0] & 0xF0) != 0x40) { + LogError(LOG_DMR, "DMR, non-IPv4 packet received"); + return; + } + + // validate Internet Header Length + uint8_t ihl = (data[0] & 0x0F) * 4; // IHL in 32-bit words, convert to bytes + if (len < ihl || ihl < 20U) { + LogError(LOG_DMR, "DMR, invalid IP header length"); + return; + } + + 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); + + // validate IP total length field against actual received length + if (pktLen > len) { + LogError(LOG_DMR, "DMR, IP packet length mismatch"); + return; + } + + if (pktLen < ihl) { + LogError(LOG_DMR, "DMR, IP packet length less than header length"); + return; + } + + uint32_t dstId = getRadioIdAddress(Utils::reverseEndian(ipHeader->ip_dst.s_addr)); + uint32_t srcProtoAddr = Utils::reverseEndian(ipHeader->ip_src.s_addr); + uint32_t tgtProtoAddr = Utils::reverseEndian(ipHeader->ip_dst.s_addr); + + std::string srcIpStr = __IP_FROM_UINT(srcProtoAddr); + std::string tgtIpStr = __IP_FROM_UINT(tgtProtoAddr); + + LogInfoEx(LOG_DMR, "VTUN -> PDU IP Data, srcIp = %s (%u), dstIp = %s (%u), pktLen = %u, proto = %02X", + srcIpStr.c_str(), DMRDEF::WUID_IPI, tgtIpStr.c_str(), dstId, pktLen, proto); + + // assemble a DMR PDU frame header for transport... + dmr::data::DataHeader* pktHeader = new dmr::data::DataHeader(); + pktHeader->setDPF(DPF::CONFIRMED_DATA); + pktHeader->setA(true); + pktHeader->setSAP(PDUSAP::PACKET_DATA); + pktHeader->setSrcId(DMRDEF::WUID_IPI); + pktHeader->setDstId(dstId); + pktHeader->setGI(false); + + // calculate blocks needed based on data rate + // using Rate 1 (24 bytes per block) for simplicity + uint32_t blocksNeeded = (pktLen + 23U) / 24U; + pktHeader->setBlocksToFollow(blocksNeeded); + pktHeader->setFullMesage(true); // Full message + + uint32_t pduLength = pktLen; + + DECLARE_UINT8_ARRAY(pduUserData, pduLength); + ::memcpy(pduUserData, data, pktLen); + + // queue frame for dispatch + QueuedDataFrame* qf = new QueuedDataFrame(); + qf->retryCnt = 0U; + qf->extendRetry = false; + qf->timestamp = now + INTERPACKET_DELAY; + + qf->header = pktHeader; + qf->dstId = dstId; + qf->tgtProtoAddr = tgtProtoAddr; + + qf->userData = new uint8_t[pduLength]; + ::memcpy(qf->userData, pduUserData, pduLength); + qf->userDataLen = pduLength; + + m_queuedFrames.push_back(qf); +#endif // !defined(_WIN32) +} + +/* Helper to write a PDU acknowledge response. */ + +void DMRPacketData::write_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint8_t ackStatus, uint32_t srcId, uint32_t dstId) +{ + if (ackClass == PDUResponseClass::ACK && ackType != PDUResponseType::ACK) + ackType = PDUResponseType::ACK; + + dmr::data::DataHeader rspHeader = dmr::data::DataHeader(); + rspHeader.setDPF(DPF::RESPONSE); + rspHeader.setSAP(PDUSAP::PACKET_DATA); + rspHeader.setResponseClass(ackClass); + rspHeader.setResponseType(ackType); + rspHeader.setResponseStatus(ackStatus); + rspHeader.setSrcId(srcId); + rspHeader.setDstId(dstId); + rspHeader.setGI(false); + rspHeader.setBlocksToFollow(0U); + + dispatchUserFrameToFNE(rspHeader, nullptr); +} + +/* Updates the timer by the passed number of milliseconds. */ + +void DMRPacketData::clock(uint32_t ms) +{ + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + if (m_queuedFrames.size() == 0U) { + return; + } + + // transmit queued data frames + auto& frame = m_queuedFrames[0]; + if (frame != nullptr) { + if (now >= frame->timestamp) { + // check if we have an ARP entry for the destination + if (!hasARPEntry(frame->dstId)) { + if (frame->retryCnt < MAX_PKT_RETRY_CNT || frame->extendRetry) { + // send ARP request + write_PDU_ARP(frame->tgtProtoAddr); + frame->timestamp = now + ARP_RETRY_MS; + frame->retryCnt++; + } else { + LogWarning(LOG_DMR, "DMR, failed to resolve ARP for dstId %u", frame->dstId); + delete frame->header; + delete[] frame->userData; + delete frame; + m_queuedFrames.pop_front(); + } + } else { + // transmit the PDU frame + dispatchUserFrameToFNE(*frame->header, frame->userData); + + delete frame->header; + delete[] frame->userData; + delete frame; + m_queuedFrames.pop_front(); + } + } + } +} + /* Helper to cleanup any call's left in a dangling state without any further updates. */ void DMRPacketData::cleanupStale() @@ -332,6 +503,126 @@ void DMRPacketData::dispatch(uint32_t peerId, dmr::data::NetData& dmrData, const if (m_network->m_dumpPacketData) { Utils::dump(1U, "DMR, ISP PDU Packet", status->pduUserData, status->pduDataOffset); } + + // handle service access points + uint8_t sap = status->header.getSAP(); + switch (sap) { + case PDUSAP::ARP: + { +#if !defined(_WIN32) + // is the host virtual tunneling enabled? + if (!m_network->m_host->m_vtunEnabled) + return; + + uint32_t fneIPv4 = __IP_FROM_STR(m_network->m_host->m_tun->getIPv4()); + + if (status->pduDataOffset < DMR_PDU_ARP_PCKT_LENGTH) { + LogError(LOG_DMR, "DMR, ARP packet too short, %u < %u", status->pduDataOffset, DMR_PDU_ARP_PCKT_LENGTH); + return; + } + + uint8_t arpPacket[DMR_PDU_ARP_PCKT_LENGTH]; + ::memset(arpPacket, 0x00U, DMR_PDU_ARP_PCKT_LENGTH); + ::memcpy(arpPacket, status->pduUserData, DMR_PDU_ARP_PCKT_LENGTH); + + uint16_t opcode = GET_UINT16(arpPacket, 6U); + uint32_t srcHWAddr = GET_UINT24(arpPacket, 8U); + uint32_t srcProtoAddr = GET_UINT32(arpPacket, 11U); + uint32_t tgtProtoAddr = GET_UINT32(arpPacket, 18U); + + if (opcode == DMR_PDU_ARP_REQUEST) { + LogInfoEx(LOG_DMR, "DMR, ARP request, who has %s? tell %s (%u)", __IP_FROM_UINT(tgtProtoAddr).c_str(), __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr); + m_arpTable[srcHWAddr] = srcProtoAddr; // update ARP table + if (tgtProtoAddr == fneIPv4) { + write_PDU_ARP_Reply(fneIPv4, srcHWAddr, srcProtoAddr, DMRDEF::WUID_ALLL); + } + } else if (opcode == DMR_PDU_ARP_REPLY) { + LogInfoEx(LOG_DMR, "DMR, ARP reply, %s is at %u", __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr); + m_arpTable[srcHWAddr] = srcProtoAddr; // update ARP table + } +#endif // !defined(_WIN32) + } + break; + + case PDUSAP::PACKET_DATA: + { +#if !defined(_WIN32) + // is the host virtual tunneling enabled? + if (!m_network->m_host->m_vtunEnabled) + return; + + uint32_t srcId = status->header.getSrcId(); + uint32_t dstId = status->header.getDstId(); + + // validate minimum IP header size + if (status->pduDataOffset < sizeof(struct ip)) { + LogError(LOG_DMR, "DMR, IP packet too short for IP header, len %u", status->pduDataOffset); + return; + } + + struct ip* ipHeader = (struct ip*)(status->pduUserData); + + // verify IPv4 version + if ((status->pduUserData[0] & 0xF0) != 0x40) { + LogError(LOG_DMR, "DMR, non-IPv4 packet received, version %u", (status->pduUserData[0] & 0xF0) >> 4); + return; + } + + // validate IP header length + uint8_t ihl = (status->pduUserData[0] & 0x0F) * 4; + if (ihl < sizeof(struct ip) || ihl > status->pduDataOffset) { + LogError(LOG_DMR, "DMR, invalid IP header length, ihl %u, pduLen %u", ihl, status->pduDataOffset); + return; + } + + 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); + + // reflect broadcast messages back to the CAI network + bool handled = false; + if (status->header.getDstId() == DMRDEF::WUID_ALL) { + uint32_t tgtProtoAddr = Utils::reverseEndian(ipHeader->ip_dst.s_addr); + uint32_t targetId = getRadioIdAddress(tgtProtoAddr); + if (targetId != 0U) { + LogInfoEx(LOG_DMR, "DMR, reflecting broadcast IP packet, srcIp = %s (%u), dstIp = %s (%u)", + srcIp, srcId, dstIp, targetId); + // TODO: reflect packet back to CAI + handled = true; + } + } + + // transmit packet to IP network + LogInfoEx(LOG_DMR, "PDU -> VTUN, IP Data, srcIp = %s (%u), dstIp = %s (%u), pktLen = %u, proto = %02X", + srcIp, srcId, dstIp, dstId, pktLen, proto); + + DECLARE_UINT8_ARRAY(ipFrame, pktLen); + ::memcpy(ipFrame, status->pduUserData, pktLen); + if (!m_network->m_host->m_tun->write(ipFrame, pktLen)) { + LogError(LOG_DMR, "DMR, failed to write IP frame to virtual tunnel, len %u", pktLen); + } + + // if the packet is unhandled and sent off to VTUN; ack the packet so the sender knows we received it + if (!handled) { + if (status->header.getA()) { + write_PDU_Ack_Response(PDUResponseClass::ACK, PDUResponseType::ACK, + status->header.getNs(), srcId, dstId); + } + } +#endif // !defined(_WIN32) + } + break; + + default: + // unknown SAP, just log the packet + LogWarning(LOG_DMR, "DMR, unhandled SAP $%02X, blocks %u, len %u", sap, status->header.getBlocksToFollow(), status->pduDataOffset); + break; + } } } @@ -397,3 +688,225 @@ void DMRPacketData::dispatchToFNE(uint32_t peerId, dmr::data::NetData& dmrData, } } } + +/* Helper to dispatch PDU user data back to the local FNE network. */ + +void DMRPacketData::dispatchUserFrameToFNE(dmr::data::DataHeader& dataHeader, uint8_t* pduUserData) +{ + uint32_t srcId = dataHeader.getSrcId(); + uint32_t dstId = dataHeader.getDstId(); + + // update the sequence number + m_suSendSeq[srcId]++; + if (m_suSendSeq[srcId] >= 8U) { + m_suSendSeq[srcId] = 0U; + } + + dataHeader.setNs(m_suSendSeq[srcId]); + + // encode and transmit DMR PDU frames to all connected peers + uint32_t streamId = m_network->createStreamId(); + uint16_t pktSeq = 0U; + + if (pduUserData == nullptr) + pktSeq = RTP_END_OF_CALL_SEQ; + + // use lambda to capture context and write network frames + auto blockWriter = [&](const void* context, const uint8_t currentBlock, const uint8_t* data, uint32_t len, bool lastBlock) -> void { + if (data == nullptr) + return; + + dmr::data::NetData dmrData; + dmrData.setSlotNo(1U); // PDU data is slot-agnostic for VTUN + dmrData.setSrcId(srcId); + dmrData.setDstId(dstId); + dmrData.setFLCO(FLCO::PRIVATE); + dmrData.setN(0U); + dmrData.setSeqNo(0U); + dmrData.setDataType(DataType::DATA_HEADER); + + // create DMR network message + uint32_t messageLength = 0U; + UInt8Array message = m_network->createDMR_Message(messageLength, streamId, dmrData); + if (message != nullptr) { + // copy the DMR frame data + ::memcpy(message.get() + 20U, data, len); + + // repeat traffic to the connected peers + if (m_network->m_peers.size() > 0U) { + for (auto peer : m_network->m_peers) { + m_network->writePeer(peer.first, m_network->m_peerId, { NET_FUNC::PROTOCOL, NET_SUBFUNC::PROTOCOL_SUBFUNC_DMR }, + message.get(), messageLength, pktSeq, streamId); + } + } + } + + pktSeq++; + }; + + // determine data type based on header + DataType::E dataType = DataType::RATE_1_DATA; // Rate 1 for most data + + // assemble and transmit the PDU + dmr::data::Assembler assembler; + assembler.setBlockWriter(blockWriter); + assembler.assemble(dataHeader, dataType, pduUserData, nullptr, nullptr); +} + +/* Helper write ARP request to the network. */ + +void DMRPacketData::write_PDU_ARP(uint32_t addr) +{ +#if !defined(_WIN32) + if (!m_network->m_host->m_vtunEnabled) + return; + + uint8_t arpPacket[DMR_PDU_ARP_PCKT_LENGTH]; + ::memset(arpPacket, 0x00U, DMR_PDU_ARP_PCKT_LENGTH); + + SET_UINT16(DMR_PDU_ARP_CAI_TYPE, arpPacket, 0U); // Hardware Address Type + SET_UINT16(PDUSAP::PACKET_DATA, arpPacket, 2U); // Protocol Address Type + arpPacket[4U] = DMR_PDU_ARP_HW_ADDR_LENGTH; // Hardware Address Length + arpPacket[5U] = DMR_PDU_ARP_PROTO_ADDR_LENGTH; // Protocol Address Length + SET_UINT16(DMR_PDU_ARP_REQUEST, arpPacket, 6U); // Opcode + + SET_UINT24(DMRDEF::WUID_ALLL, 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 + + LogInfoEx(LOG_DMR, "DMR, ARP request, who has %s? tell %s (%u)", __IP_FROM_UINT(addr).c_str(), fneIPv4.c_str(), DMRDEF::WUID_ALLL); + + // assemble a DMR PDU frame header for transport... + dmr::data::DataHeader rspHeader = dmr::data::DataHeader(); + rspHeader.setDPF(DPF::UNCONFIRMED_DATA); + rspHeader.setA(false); + rspHeader.setSAP(PDUSAP::ARP); + rspHeader.setSrcId(DMRDEF::WUID_ALLL); + rspHeader.setDstId(DMRDEF::WUID_ALL); + rspHeader.setGI(true); + rspHeader.setBlocksToFollow(1U); + + dispatchUserFrameToFNE(rspHeader, arpPacket); +#endif // !defined(_WIN32) +} + +/* Helper write ARP reply to the network. */ + +void DMRPacketData::write_PDU_ARP_Reply(uint32_t targetAddr, uint32_t requestorId, uint32_t requestorAddr, uint32_t targetId) +{ +#if !defined(_WIN32) + if (!m_network->m_host->m_vtunEnabled) + return; + + uint8_t arpPacket[DMR_PDU_ARP_PCKT_LENGTH]; + ::memset(arpPacket, 0x00U, DMR_PDU_ARP_PCKT_LENGTH); + + SET_UINT16(DMR_PDU_ARP_CAI_TYPE, arpPacket, 0U); // Hardware Address Type + SET_UINT16(PDUSAP::PACKET_DATA, arpPacket, 2U); // Protocol Address Type + arpPacket[4U] = DMR_PDU_ARP_HW_ADDR_LENGTH; // Hardware Address Length + arpPacket[5U] = DMR_PDU_ARP_PROTO_ADDR_LENGTH; // Protocol Address Length + SET_UINT16(DMR_PDU_ARP_REPLY, arpPacket, 6U); // Opcode + + SET_UINT24(targetId, arpPacket, 8U); // Sender Hardware Address + SET_UINT32(targetAddr, arpPacket, 11U); // Sender Protocol Address + + SET_UINT24(requestorId, arpPacket, 15U); // Target Hardware Address + SET_UINT32(requestorAddr, arpPacket, 18U); // Target Protocol Address + + LogInfoEx(LOG_DMR, "DMR, ARP reply, %s is at %u", __IP_FROM_UINT(targetAddr).c_str(), targetId); + + // assemble a DMR PDU frame header for transport... + dmr::data::DataHeader rspHeader = dmr::data::DataHeader(); + rspHeader.setDPF(DPF::UNCONFIRMED_DATA); + rspHeader.setA(false); + rspHeader.setSAP(PDUSAP::ARP); + rspHeader.setSrcId(targetId); + rspHeader.setDstId(requestorId); + rspHeader.setGI(false); + rspHeader.setBlocksToFollow(1U); + + dispatchUserFrameToFNE(rspHeader, arpPacket); +#endif // !defined(_WIN32) +} + +/* Helper to determine if the radio ID has an ARP entry. */ + +bool DMRPacketData::hasARPEntry(uint32_t id) const +{ + if (id == 0U) { + return false; + } + + // lookup ARP table entry + try { + uint32_t addr = m_arpTable.at(id); + if (addr != 0U) { + return true; + } + else { + return false; + } + } catch (...) { + return false; + } +} + +/* Helper to get the IP address for the given radio ID. */ + +uint32_t DMRPacketData::getIPAddress(uint32_t id) +{ + if (id == 0U) { + return 0U; + } + + if (hasARPEntry(id)) { + return m_arpTable[id]; + } else { + // do we have a static entry for this ID? + lookups::RadioId rid = m_network->m_ridLookup->find(id); + if (!rid.radioDefault()) { + if (rid.radioEnabled()) { + std::string addr = rid.radioIPAddress(); + uint32_t ipAddr = __IP_FROM_STR(addr); + return ipAddr; + } + } + } + + return 0U; +} + +/* Helper to get the radio ID for the given IP address. */ + +uint32_t DMRPacketData::getRadioIdAddress(uint32_t addr) +{ + if (addr == 0U) { + return 0U; + } + + for (auto entry : m_arpTable) { + if (entry.second == addr) { + return entry.first; + } + } + + // check if we have an entry in the RID lookup + std::string ipAddr = __IP_FROM_UINT(addr); + std::unordered_map ridTable = m_network->m_ridLookup->table(); + auto it = std::find_if(ridTable.begin(), ridTable.end(), [&](std::pair x) { + if (x.second.radioIPAddress() == ipAddr) { + if (x.second.radioEnabled() && !x.second.radioDefault()) + return true; + } + return false; + }); + if (it != ridTable.end()) { + m_arpTable[it->first] = addr; + return it->first; + } + + return 0U; +} diff --git a/src/fne/network/callhandler/packetdata/DMRPacketData.h b/src/fne/network/callhandler/packetdata/DMRPacketData.h index a484fe7a..50524927 100644 --- a/src/fne/network/callhandler/packetdata/DMRPacketData.h +++ b/src/fne/network/callhandler/packetdata/DMRPacketData.h @@ -68,6 +68,30 @@ namespace network */ bool processFrame(const uint8_t* data, uint32_t len, uint32_t peerId, uint16_t pktSeq, uint32_t streamId, bool fromUpstream = false); + /** + * @brief Process a data frame from the virtual IP network. + * @param data Network data buffer. + * @param len Length of data. + * @param alreadyQueued Flag indicating the data frame being processed is already queued. + */ + void processPacketFrame(const uint8_t* data, uint32_t len, bool alreadyQueued = false); + + /** + * @brief Helper to write a PDU acknowledge response. + * @param ackClass Acknowledgement Class. + * @param ackType Acknowledgement Type. + * @param ackStatus Acknowledgement Status. + * @param srcId Source Radio ID. + * @param dstId Destination Radio ID. + */ + void write_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint8_t ackStatus, uint32_t srcId, uint32_t dstId); + + /** + * @brief Updates the timer by the passed number of milliseconds. + * @param ms Number of milliseconds. + */ + void clock(uint32_t ms); + /** * @brief Helper to cleanup any call's left in a dangling state without any further updates. */ @@ -77,6 +101,24 @@ namespace network FNENetwork* m_network; TagDMRData *m_tag; + /** + * @brief Represents a queued data frame from the VTUN. + */ + class QueuedDataFrame { + public: + dmr::data::DataHeader* header; //!< Instance of a PDU data header. + uint32_t dstId; //!< Destination Radio 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; + /** * @brief Represents the receive status of a call. */ @@ -131,6 +173,12 @@ namespace network typedef std::pair StatusMapPair; concurrent::unordered_map m_status; + typedef std::pair ArpTablePair; + std::unordered_map m_arpTable; + typedef std::pair ReadyForNextPktPair; + std::unordered_map m_readyForNextPkt; + std::unordered_map m_suSendSeq; + bool m_debug; /** @@ -152,6 +200,45 @@ namespace network * @param streamId Stream ID. */ void dispatchToFNE(uint32_t peerId, dmr::data::NetData& dmrData, const uint8_t* data, uint32_t len, uint8_t seqNo, uint16_t pktSeq, uint32_t streamId); + /** + * @brief Helper to dispatch PDU user data back to the local FNE network. (Will not transmit to neighbor FNE peers.) + * @param dataHeader Instance of a PDU data header. + * @param pduUserData Buffer containing user data to transmit. + */ + void dispatchUserFrameToFNE(dmr::data::DataHeader& dataHeader, uint8_t* pduUserData); + + /** + * @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 requestorId Requestor Radio ID. + * @param requestorAddr Requestor IP Address. + * @param targetId Target Radio ID. + */ + void write_PDU_ARP_Reply(uint32_t targetAddr, uint32_t requestorId, uint32_t requestorAddr, uint32_t targetId = 0U); + + /** + * @brief Helper to determine if the radio ID has an ARP entry. + * @param id Radio ID. + * @returns bool True, if ARP entry exists, otherwise false. + */ + bool hasARPEntry(uint32_t id) const; + /** + * @brief Helper to get the IP address for the given radio ID. + * @param id Radio ID. + * @returns uint32_t IP address. + */ + uint32_t getIPAddress(uint32_t id); + /** + * @brief Helper to get the radio ID for the given IP address. + * @param addr IP Address. + * @returns uint32_t Radio ID. + */ + uint32_t getRadioIdAddress(uint32_t addr); }; } // namespace packetdata } // namespace callhandler