diff --git a/src/common/network/BaseNetwork.cpp b/src/common/network/BaseNetwork.cpp index 30534260..d0ccf1af 100644 --- a/src/common/network/BaseNetwork.cpp +++ b/src/common/network/BaseNetwork.cpp @@ -23,6 +23,12 @@ using namespace network::frame; #include +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +#define NET_RING_BUF_SIZE 4098U + // --------------------------------------------------------------------------- // Public Class Members // --------------------------------------------------------------------------- @@ -43,9 +49,9 @@ BaseNetwork::BaseNetwork(uint32_t peerId, bool duplex, bool debug, bool slot1, b m_debug(debug), m_socket(nullptr), m_frameQueue(nullptr), - m_rxDMRData(4000U, "DMR Net Buffer"), - m_rxP25Data(4000U, "P25 Net Buffer"), - m_rxNXDNData(4000U, "NXDN Net Buffer"), + m_rxDMRData(NET_RING_BUF_SIZE, "DMR Net Buffer"), + m_rxP25Data(NET_RING_BUF_SIZE, "P25 Net Buffer"), + m_rxNXDNData(NET_RING_BUF_SIZE, "NXDN Net Buffer"), m_random(), m_dmrStreamId(nullptr), m_p25StreamId(0U), diff --git a/src/common/network/RTPExtensionHeader.h b/src/common/network/RTPExtensionHeader.h index c31ea2b1..c789afb3 100644 --- a/src/common/network/RTPExtensionHeader.h +++ b/src/common/network/RTPExtensionHeader.h @@ -44,6 +44,7 @@ namespace network * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ * | Payload Type | Payload Length | * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * 4 bytes * \endcode */ class HOST_SW_API RTPExtensionHeader { diff --git a/src/common/network/RTPFNEHeader.h b/src/common/network/RTPFNEHeader.h index 8e0d8066..03191b27 100644 --- a/src/common/network/RTPFNEHeader.h +++ b/src/common/network/RTPFNEHeader.h @@ -129,6 +129,7 @@ namespace network * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ * | Message Length | * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * 20 bytes (16 bytes without RTP Extension Header) * \endcode */ class HOST_SW_API RTPFNEHeader : public RTPExtensionHeader { diff --git a/src/common/network/RTPHeader.h b/src/common/network/RTPHeader.h index 87d8eac6..f03495a5 100644 --- a/src/common/network/RTPHeader.h +++ b/src/common/network/RTPHeader.h @@ -50,6 +50,7 @@ namespace network * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ * | SSRC | * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * 12 bytes * \endcode * @ingroup network_core */ diff --git a/src/fne/HostFNE.cpp b/src/fne/HostFNE.cpp index a9827986..e5df4944 100644 --- a/src/fne/HostFNE.cpp +++ b/src/fne/HostFNE.cpp @@ -857,7 +857,13 @@ void* HostFNE::threadVirtualNetworking(void* arg) #endif // _GNU_SOURCE if (fne->m_tun != nullptr) { + StopWatch stopWatch; + stopWatch.start(); + while (!g_killed) { + uint32_t ms = stopWatch.elapsed(); + stopWatch.start(); + uint8_t packet[DEFAULT_MTU_SIZE]; ::memset(packet, 0x00U, DEFAULT_MTU_SIZE); @@ -869,11 +875,22 @@ void* HostFNE::threadVirtualNetworking(void* arg) break; case PacketDataMode::PROJECT25: - fne->m_network->p25TrafficHandler()->processPacketFrame(packet, DEFAULT_MTU_SIZE); + fne->m_network->p25TrafficHandler()->packetData()->processPacketFrame(packet, DEFAULT_MTU_SIZE); break; } } + // clock traffic handler + switch (fne->m_packetDataMode) { + case PacketDataMode::DMR: + // TODO: not supported yet + break; + + case PacketDataMode::PROJECT25: + fne->m_network->p25TrafficHandler()->packetData()->clock(ms); + break; + } + Thread::sleep(5U); } } diff --git a/src/fne/network/callhandler/TagDMRData.h b/src/fne/network/callhandler/TagDMRData.h index 7d90afad..c49a87dd 100644 --- a/src/fne/network/callhandler/TagDMRData.h +++ b/src/fne/network/callhandler/TagDMRData.h @@ -103,6 +103,12 @@ namespace network */ void write_Call_Alrt(uint32_t peerId, uint8_t slot, uint32_t srcId, uint32_t dstId); + /** + * @brief Gets instance of the DMRPacketData class. + * @returns DMRPacketData* Instance of the DMRPacketData class. + */ + packetdata::DMRPacketData* packetData() { return m_packetData; } + private: FNENetwork* m_network; @@ -143,7 +149,7 @@ namespace network std::unordered_map m_status; friend class packetdata::DMRPacketData; - packetdata::DMRPacketData *m_packetData; + packetdata::DMRPacketData* m_packetData; bool m_debug; diff --git a/src/fne/network/callhandler/TagP25Data.cpp b/src/fne/network/callhandler/TagP25Data.cpp index eb62b7e3..da1b358f 100644 --- a/src/fne/network/callhandler/TagP25Data.cpp +++ b/src/fne/network/callhandler/TagP25Data.cpp @@ -434,13 +434,6 @@ 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() diff --git a/src/fne/network/callhandler/TagP25Data.h b/src/fne/network/callhandler/TagP25Data.h index 429c6400..a1da1ad3 100644 --- a/src/fne/network/callhandler/TagP25Data.h +++ b/src/fne/network/callhandler/TagP25Data.h @@ -79,13 +79,6 @@ 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. */ @@ -132,6 +125,12 @@ namespace network */ void write_TSDU_U_Reg_Cmd(uint32_t peerId, uint32_t dstId); + /** + * @brief Gets instance of the P25PacketData class. + * @returns P25PacketData* Instance of the P25PacketData class. + */ + packetdata::P25PacketData* packetData() { return m_packetData; } + private: FNENetwork* m_network; diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.cpp b/src/fne/network/callhandler/packetdata/P25PacketData.cpp index d6fff700..22732955 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.cpp +++ b/src/fne/network/callhandler/packetdata/P25PacketData.cpp @@ -58,6 +58,8 @@ P25PacketData::P25PacketData(FNENetwork* network, TagP25Data* tag, bool debug) : m_dataFrames(), m_status(), m_arpTable(), + m_readyForPkt(), + m_suNotReadyTimeout(), m_debug(debug) { assert(network != nullptr); @@ -119,9 +121,9 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee return false; } - LogMessage(LOG_NET, P25_PDU_STR ", peerId = %u, ack = %u, outbound = %u, fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, n = %u, seqNo = %u, hdrOffset = %u, llId = %u", + LogMessage(LOG_NET, P25_PDU_STR ", peerId = %u, ack = %u, outbound = %u, fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, S = %u, n = %u, seqNo = %u, hdrOffset = %u, llId = %u", peerId, status->header.getAckNeeded(), status->header.getOutbound(), status->header.getFormat(), status->header.getSAP(), status->header.getFullMessage(), - status->header.getBlocksToFollow(), status->header.getPadLength(), status->header.getPacketLength(), status->header.getNs(), status->header.getFSN(), + status->header.getBlocksToFollow(), status->header.getPadLength(), status->header.getPacketLength(), status->header.getSynchronize(), status->header.getNs(), status->header.getFSN(), status->header.getHeaderOffset(), status->header.getLLId()); // make sure we don't get a PDU with more blocks then we support @@ -134,8 +136,8 @@ 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) { + // is this a response header? + if (status->header.getFormat() == PDUFormatType::RSP) { dispatch(peerId); delete status; @@ -143,6 +145,11 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee return true; } + if (status->header.getSAP() != PDUSAP::EXT_ADDR && + status->header.getFormat() != PDUFormatType::UNCONFIRMED) { + m_readyForPkt[status->llId] = true; + } + LogMessage(LOG_NET, "P25, Data Call Start, peer = %u, llId = %u, streamId = %u, external = %u", peerId, status->llId, streamId, external); return true; } else { @@ -190,6 +197,10 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee status->dataBlockCnt = 0U; + // process all blocks in the data stream + status->pduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U]; + ::memset(status->pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U); + // process second header if we're using enhanced addressing if (status->header.getSAP() == PDUSAP::EXT_ADDR && status->header.getFormat() == PDUFormatType::UNCONFIRMED) { @@ -211,6 +222,8 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee status->header.getEXSAP(), status->header.getSrcLLId()); status->extendedAddress = true; + status->llId = status->header.getSrcLLId(); + m_readyForPkt[status->llId] = true; offset += P25_PDU_FEC_LENGTH_BYTES; blocksToFollow--; @@ -221,10 +234,6 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee status->pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES; } - // process all blocks in the data stream - status->pduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U]; - ::memset(status->pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U); - // decode data blocks for (uint32_t i = 0U; i < blocksToFollow; i++) { ::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES); @@ -336,14 +345,60 @@ void P25PacketData::processPacketFrame(const uint8_t* data, uint32_t len, bool a #if DEBUG_P25_PDU_DATA Utils::dump(1U, "P25PacketData::processPacketFrame() packet", data, pktLen); #endif + + VTUNDataFrame dataFrame; + dataFrame.buffer = new uint8_t[len]; + ::memcpy(dataFrame.buffer, data, len); + dataFrame.bufferLen = len; + dataFrame.pktLen = pktLen; + uint32_t dstLlId = getLLIdAddress(Utils::reverseEndian(ipHeader->ip_dst.s_addr)); + + dataFrame.srcHWAddr = WUID_FNE; + dataFrame.srcProtoAddr = Utils::reverseEndian(ipHeader->ip_src.s_addr); + dataFrame.tgtHWAddr = dstLlId; + dataFrame.tgtProtoAddr = 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 { + + m_dataFrames.push_back(dataFrame); +#endif // !defined(_WIN32) +} + +/* Updates the timer by the passed number of milliseconds. */ + +void P25PacketData::clock(uint32_t ms) +{ + // transmit queued data frames + if (m_dataFrames.size() > 0) { + auto& dataFrame = m_dataFrames[0]; + + if (dataFrame.tgtHWAddr == 0U) { + uint32_t dstLlId = getLLIdAddress(dataFrame.tgtProtoAddr); + if (dstLlId == 0U) + return; + + dataFrame.tgtHWAddr = dstLlId; + } + + // don't allow another packet to go out if we haven't acked the previous + if (!m_readyForPkt[dataFrame.tgtHWAddr]) { + m_suNotReadyTimeout[dataFrame.tgtHWAddr].clock(ms); + if (m_suNotReadyTimeout[dataFrame.tgtHWAddr].isRunning() && m_suNotReadyTimeout[dataFrame.tgtHWAddr].hasExpired()) { + m_suNotReadyTimeout[dataFrame.tgtHWAddr].stop(); + m_readyForPkt[dataFrame.tgtHWAddr] = true; + } + + return; + } + + m_readyForPkt[dataFrame.tgtHWAddr] = false; + m_suNotReadyTimeout[dataFrame.tgtHWAddr] = Timer(1000U, 5U, 0U); + m_suNotReadyTimeout[dataFrame.tgtHWAddr].start(); + // assemble a P25 PDU frame header for transport... data::DataHeader rspHeader = data::DataHeader(); rspHeader.setFormat(PDUFormatType::CONFIRMED); @@ -351,27 +406,27 @@ void P25PacketData::processPacketFrame(const uint8_t* data, uint32_t len, bool a rspHeader.setAckNeeded(true); rspHeader.setOutbound(true); rspHeader.setSAP(PDUSAP::EXT_ADDR); - rspHeader.setLLId(dstLlId); + rspHeader.setLLId(dataFrame.tgtHWAddr); rspHeader.setBlocksToFollow(1U); rspHeader.setEXSAP(PDUSAP::PACKET_DATA); rspHeader.setSrcLLId(WUID_FNE); - rspHeader.calculateLength(pktLen); + rspHeader.calculateLength(dataFrame.pktLen); uint32_t pduLength = rspHeader.getPDULength(); UInt8Array __pduUserData = std::make_unique(pduLength); uint8_t* pduUserData = __pduUserData.get(); ::memset(pduUserData, 0x00U, pduLength); - ::memcpy(pduUserData + 4U, data, pktLen); + ::memcpy(pduUserData + 4U, dataFrame.buffer, dataFrame.pktLen); #if DEBUG_P25_PDU_DATA - Utils::dump(1U, "P25PacketData::processPacketFrame() pduUserData", pduUserData, pduLength); + Utils::dump(1U, "P25PacketData::clock() pduUserData", pduUserData, pduLength); #endif dispatchUserFrameToFNE(rspHeader, true, pduUserData); - } - Thread::sleep(1750); -#endif // !defined(_WIN32) + delete[] dataFrame.buffer; + m_dataFrames.pop_front(); + } } // --------------------------------------------------------------------------- @@ -384,10 +439,11 @@ void P25PacketData::dispatch(uint32_t peerId) { RxStatus* status = m_status[peerId]; + bool crcValid = false; if (status->header.getBlocksToFollow() > 0U) { - bool crcRet = edac::CRC::checkCRC32(status->pduUserData, status->pduUserDataLength); - if (!crcRet) { - LogWarning(LOG_NET, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", status->header.getBlocksToFollow(), status->pduUserDataLength); + crcValid = edac::CRC::checkCRC32(status->pduUserData, status->pduUserDataLength); + if (!crcValid) { + LogError(LOG_NET, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", status->header.getBlocksToFollow(), status->pduUserDataLength); } } @@ -395,6 +451,18 @@ void P25PacketData::dispatch(uint32_t peerId) Utils::dump(1U, "PDU Packet", status->pduUserData, status->pduUserDataLength); } + if (status->header.getFormat() == PDUFormatType::RSP) { + LogMessage(LOG_NET, P25_PDU_STR ", ISP, response, fmt = $%02X, rspClass = $%02X, rspType = $%02X, rspStatus = $%02X, llId = %u, srcLlId = %u", + status->header.getFormat(), status->header.getResponseClass(), status->header.getResponseType(), status->header.getResponseStatus(), + status->header.getLLId(), status->header.getSrcLLId()); +/* + if (status->header.getResponseClass() == PDUAckClass::ACK && status->header.getResponseType() == PDUAckType::ACK) { + m_readyForPkt[status->header.getSrcLLId()] = true; + } +*/ + return; + } + uint8_t sap = (status->extendedAddress) ? status->header.getEXSAP() : status->header.getSAP(); // don't dispatch SNDCP control, conventional data registration or ARP @@ -438,6 +506,8 @@ void P25PacketData::dispatch(uint32_t peerId) } else { m_arpTable[srcHWAddr] = srcProtoAddr; } + + m_readyForPkt[srcHWAddr] = true; } #else break; @@ -451,6 +521,11 @@ void P25PacketData::dispatch(uint32_t peerId) if (!m_network->m_host->m_vtunEnabled) break; + if (!crcValid) { + write_PDU_Ack_Response(PDUAckClass::NACK, PDUAckType::NACK_PACKET_CRC, 0U, (status->extendedAddress) ? status->header.getSrcLLId() : status->header.getLLId()); + break; + } + int dataPktOffset = 0U; if (status->header.getFormat() == PDUFormatType::CONFIRMED && status->extendedAddress) dataPktOffset = 4U; @@ -480,6 +555,9 @@ void P25PacketData::dispatch(uint32_t peerId) 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); } + + write_PDU_Ack_Response(PDUAckClass::ACK, PDUAckType::ACK, 0U, (status->extendedAddress) ? status->header.getSrcLLId() : status->header.getLLId()); + m_readyForPkt[status->header.getSrcLLId()] = true; #endif // !defined(_WIN32) } break; @@ -562,11 +640,13 @@ void P25PacketData::dispatchUserFrameToFNE(p25::data::DataHeader& dataHeader, bo uint32_t srcId = (extendedAddress) ? dataHeader.getSrcLLId() : dataHeader.getLLId(); uint32_t dstId = dataHeader.getLLId(); + dataHeader.setSynchronize(true); +/* 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; @@ -751,13 +831,38 @@ void P25PacketData::write_PDU_ARP_Reply(uint32_t targetAddr, uint32_t requestorL dispatchUserFrameToFNE(rspHeader, true, pduUserData); } +/* Helper to write a PDU acknowledge response. */ + +void P25PacketData::write_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint8_t ackStatus, uint32_t llId, uint32_t srcLlId) +{ + if (ackClass == PDUAckClass::ACK && ackType != PDUAckType::ACK) + return; + + data::DataHeader rspHeader = data::DataHeader(); + rspHeader.setFormat(PDUFormatType::RSP); + rspHeader.setMFId(MFG_STANDARD); + rspHeader.setOutbound(true); + rspHeader.setResponseClass(ackClass); + rspHeader.setResponseType(ackType); + rspHeader.setResponseStatus(ackStatus); + rspHeader.setLLId(llId); + if (srcLlId > 0U) { + rspHeader.setSrcLLId(srcLlId); + rspHeader.setFullMessage(false); + } + else { + rspHeader.setFullMessage(true); + } + rspHeader.setBlocksToFollow(0U); + + dispatchUserFrameToFNE(rspHeader, srcLlId > 0U, nullptr); +} + /* Helper to write user data as a P25 PDU packet. */ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNet, data::DataHeader& dataHeader, bool extendedAddress, uint8_t* pduUserData, bool queueOnly) { - assert(pduUserData != nullptr); - uint32_t streamId = m_network->createStreamId(); uint16_t pktSeq = 0U; @@ -766,16 +871,19 @@ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNe uint32_t blocksToFollow = dataHeader.getBlocksToFollow(); - LogMessage(LOG_NET, P25_PDU_STR ", OSP, peerId = %u, ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u", + LogMessage(LOG_NET, P25_PDU_STR ", OSP, peerId = %u, ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, S = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u", peerId, dataHeader.getAckNeeded(), dataHeader.getOutbound(), dataHeader.getFormat(), dataHeader.getMFId(), dataHeader.getSAP(), dataHeader.getFullMessage(), - dataHeader.getBlocksToFollow(), dataHeader.getPadLength(), dataHeader.getPacketLength(), dataHeader.getNs(), dataHeader.getFSN(), dataHeader.getLastFragment(), + dataHeader.getBlocksToFollow(), dataHeader.getPadLength(), dataHeader.getPacketLength(), dataHeader.getSynchronize(), dataHeader.getNs(), dataHeader.getFSN(), dataHeader.getLastFragment(), dataHeader.getHeaderOffset(), dataHeader.getLLId()); // generate the PDU header and 1/2 rate Trellis dataHeader.encode(buffer); writeNetwork(peerId, peerNet, dataHeader, 0U, buffer, P25_PDU_FEC_LENGTH_BYTES, pktSeq, streamId, queueOnly); - ++pktSeq; + if (pduUserData == nullptr) + return; + + ++pktSeq; uint32_t packetLength = dataHeader.getPDULength(); if (blocksToFollow > 0U) { diff --git a/src/fne/network/callhandler/packetdata/P25PacketData.h b/src/fne/network/callhandler/packetdata/P25PacketData.h index a067038b..018a1ea5 100644 --- a/src/fne/network/callhandler/packetdata/P25PacketData.h +++ b/src/fne/network/callhandler/packetdata/P25PacketData.h @@ -75,6 +75,12 @@ namespace network */ void processPacketFrame(const uint8_t* data, uint32_t len, bool alreadyQueued = false); + /** + * @brief Updates the timer by the passed number of milliseconds. + * @param ms Number of milliseconds. + */ + void clock(uint32_t ms); + private: FNENetwork* m_network; TagP25Data *m_tag; @@ -86,8 +92,15 @@ namespace network */ class VTUNDataFrame { public: + uint32_t srcHWAddr; + uint32_t srcProtoAddr; + uint32_t tgtHWAddr; + uint32_t tgtProtoAddr; + uint8_t* buffer; uint32_t bufferLen; + + uint16_t pktLen; }; std::deque m_dataFrames; @@ -151,6 +164,8 @@ namespace network std::unordered_map m_status; std::unordered_map m_arpTable; + std::unordered_map m_readyForPkt; + std::unordered_map m_suNotReadyTimeout; bool m_debug; @@ -193,6 +208,16 @@ namespace network */ void write_PDU_ARP_Reply(uint32_t targetAddr, uint32_t requestorLlid, uint32_t requestorAddr, uint32_t targetLlid = 0U); + /** + * @brief Helper to write a PDU acknowledge response. + * @param ackClass Acknowledgement Class. + * @param ackType Acknowledgement Type. + * @param ackStatus + * @param llId Logical Link ID. + * @param srcLlId Source Logical Link ID. + */ + void write_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint8_t ackStatus, uint32_t llId, uint32_t srcLlId = 0U); + /** * @brief Helper to write user data as a P25 PDU packet. * @param peerId Peer ID. diff --git a/src/host/network/Network.cpp b/src/host/network/Network.cpp index 751a87a3..01c423e6 100644 --- a/src/host/network/Network.cpp +++ b/src/host/network/Network.cpp @@ -280,6 +280,8 @@ void Network::clock(uint32_t ms) if (m_debug) Utils::dump(1U, "Network Received, DMR", buffer.get(), length); + if (length > 255) + LogError(LOG_NET, "DMR Stream %u, frame oversized? this shouldn't happen, pktSeq = %u, len = %u", streamId, m_pktSeq, length); uint8_t len = length; m_rxDMRData.addData(&len, 1U); @@ -306,6 +308,8 @@ void Network::clock(uint32_t ms) if (m_debug) Utils::dump(1U, "Network Received, P25", buffer.get(), length); + if (length > 255) + LogError(LOG_NET, "P25 Stream %u, frame oversized? this shouldn't happen, pktSeq = %u, len = %u", streamId, m_pktSeq, length); uint8_t len = length; m_rxP25Data.addData(&len, 1U); @@ -332,6 +336,8 @@ void Network::clock(uint32_t ms) if (m_debug) Utils::dump(1U, "Network Received, NXDN", buffer.get(), length); + if (length > 255) + LogError(LOG_NET, "NXDN Stream %u, frame oversized? this shouldn't happen, pktSeq = %u, len = %u", streamId, m_pktSeq, length); uint8_t len = length; m_rxNXDNData.addData(&len, 1U); diff --git a/src/host/p25/packet/Data.cpp b/src/host/p25/packet/Data.cpp index f1941472..64ce0dcf 100644 --- a/src/host/p25/packet/Data.cpp +++ b/src/host/p25/packet/Data.cpp @@ -122,9 +122,9 @@ bool Data::process(uint8_t* data, uint32_t len) } if (m_verbose) { - LogMessage(LOG_RF, P25_PDU_STR ", ISP, ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u", + LogMessage(LOG_RF, P25_PDU_STR ", ISP, ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, S = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u", m_rfDataHeader.getAckNeeded(), m_rfDataHeader.getOutbound(), m_rfDataHeader.getFormat(), m_rfDataHeader.getMFId(), m_rfDataHeader.getSAP(), m_rfDataHeader.getFullMessage(), - m_rfDataHeader.getBlocksToFollow(), m_rfDataHeader.getPadLength(), m_rfDataHeader.getPacketLength(), m_rfDataHeader.getNs(), m_rfDataHeader.getFSN(), m_rfDataHeader.getLastFragment(), + m_rfDataHeader.getBlocksToFollow(), m_rfDataHeader.getPadLength(), m_rfDataHeader.getPacketLength(), m_rfDataHeader.getSynchronize(), m_rfDataHeader.getNs(), m_rfDataHeader.getFSN(), m_rfDataHeader.getLastFragment(), m_rfDataHeader.getHeaderOffset(), m_rfDataHeader.getLLId()); } @@ -306,6 +306,7 @@ bool Data::process(uint8_t* data, uint32_t len) 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_rfPduUserDataLength); + writeRF_PDU_Ack_Response(PDUAckClass::NACK, PDUAckType::NACK_PACKET_CRC, 0U, (m_rfExtendedAddress) ? m_rfDataHeader.getSrcLLId() : m_rfDataHeader.getLLId()); } } @@ -387,7 +388,8 @@ bool Data::process(uint8_t* data, uint32_t len) } } - if (m_repeatPDU) { + // only repeat the PDU locally if the packet isn't for the FNE + if (m_repeatPDU && m_rfDataHeader.getLLId() != WUID_FNE) { if (m_verbose) { LogMessage(LOG_RF, P25_PDU_STR ", repeating ACK PDU, llId = %u, srcLlId = %u", m_rfDataHeader.getLLId(), m_rfDataHeader.getSrcLLId()); } @@ -455,17 +457,17 @@ bool Data::process(uint8_t* data, uint32_t len) } break; default: - ::ActivityLog("P25", true, "RF data transmission from %u to %u, %u blocks", srcId, dstId, m_rfDataHeader.getBlocksToFollow()); - - if (m_repeatPDU) { + // only repeat the PDU locally if the packet isn't for the FNE + if (m_repeatPDU && m_rfDataHeader.getLLId() != WUID_FNE) { + ::ActivityLog("P25", true, "RF data transmission from %u to %u, %u blocks", srcId, dstId, m_rfDataHeader.getBlocksToFollow()); + if (m_verbose) { LogMessage(LOG_RF, P25_PDU_STR ", repeating PDU, llId = %u", (m_rfExtendedAddress) ? m_rfDataHeader.getSrcLLId() : m_rfDataHeader.getLLId()); } writeRF_PDU_Buffered(); // re-generate buffered PDU and send it on + ::ActivityLog("P25", true, "end of RF data transmission"); } - - ::ActivityLog("P25", true, "end of RF data transmission"); break; } } @@ -525,9 +527,9 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength) } if (m_verbose) { - LogMessage(LOG_NET, P25_PDU_STR ", ack = %u, outbound = %u, fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, n = %u, seqNo = %u, hdrOffset = %u, llId = %u", + LogMessage(LOG_NET, P25_PDU_STR ", ack = %u, outbound = %u, fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, S = %u, n = %u, seqNo = %u, hdrOffset = %u, llId = %u", m_netDataHeader.getAckNeeded(), m_netDataHeader.getOutbound(), m_netDataHeader.getFormat(), m_netDataHeader.getSAP(), m_netDataHeader.getFullMessage(), - m_netDataHeader.getBlocksToFollow(), m_netDataHeader.getPadLength(), m_netDataHeader.getPacketLength(), m_netDataHeader.getNs(), m_netDataHeader.getFSN(), + m_netDataHeader.getBlocksToFollow(), m_netDataHeader.getPadLength(), m_netDataHeader.getPacketLength(), m_netDataHeader.getSynchronize(), m_netDataHeader.getNs(), m_netDataHeader.getFSN(), m_netDataHeader.getHeaderOffset(), m_netDataHeader.getLLId()); } @@ -825,8 +827,9 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, uint32_t bitLength = ((dataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; uint32_t offset = P25_PREAMBLE_LENGTH_BITS; - UInt8Array __data = std::make_unique(bitLength / 8U); + UInt8Array __data = std::make_unique((bitLength / 8U) + 1U); uint8_t* data = __data.get(); + ::memset(data, 0x00U, bitLength / 8U); uint8_t block[P25_PDU_FEC_LENGTH_BYTES]; ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); @@ -834,9 +837,9 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress, uint32_t blocksToFollow = dataHeader.getBlocksToFollow(); if (m_verbose) { - LogMessage(LOG_RF, P25_PDU_STR ", OSP, ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, bitLength = %u, llId = %u", + LogMessage(LOG_RF, P25_PDU_STR ", OSP, ack = %u, outbound = %u, fmt = $%02X, mfId = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padLength = %u, packetLength = %u, S = %u, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, bitLength = %u, llId = %u", dataHeader.getAckNeeded(), dataHeader.getOutbound(), dataHeader.getFormat(), dataHeader.getMFId(), dataHeader.getSAP(), dataHeader.getFullMessage(), - dataHeader.getBlocksToFollow(), dataHeader.getPadLength(), dataHeader.getPacketLength(), dataHeader.getNs(), dataHeader.getFSN(), dataHeader.getLastFragment(), + dataHeader.getBlocksToFollow(), dataHeader.getPadLength(), dataHeader.getPacketLength(), dataHeader.getSynchronize(), dataHeader.getNs(), dataHeader.getFSN(), dataHeader.getLastFragment(), dataHeader.getHeaderOffset(), bitLength, dataHeader.getLLId()); } @@ -1461,8 +1464,9 @@ void Data::writeNet_PDU_Buffered() uint32_t bitLength = ((m_netDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; uint32_t offset = P25_PREAMBLE_LENGTH_BITS; - UInt8Array __data = std::make_unique(bitLength / 8U); + UInt8Array __data = std::make_unique((bitLength / 8U) + 1U); uint8_t* data = __data.get(); + ::memset(data, 0x00U, bitLength / 8U); uint8_t block[P25_PDU_FEC_LENGTH_BYTES]; ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); @@ -1551,8 +1555,9 @@ void Data::writeRF_PDU_Buffered() uint32_t bitLength = ((m_rfDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; uint32_t offset = P25_PREAMBLE_LENGTH_BITS; - UInt8Array __data = std::make_unique(bitLength / 8U); + UInt8Array __data = std::make_unique((bitLength / 8U) + 1U); uint8_t* data = __data.get(); + ::memset(data, 0x00U, bitLength / 8U); uint8_t block[P25_PDU_FEC_LENGTH_BYTES]; ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); @@ -1677,8 +1682,9 @@ void Data::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint8_t a uint32_t bitLength = (1U * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; uint32_t offset = P25_PREAMBLE_LENGTH_BITS; - UInt8Array __data = std::make_unique(bitLength / 8U); + UInt8Array __data = std::make_unique((bitLength / 8U) + 1U); uint8_t* data = __data.get(); + ::memset(data, 0x00U, bitLength / 8U); uint8_t block[P25_PDU_FEC_LENGTH_BYTES]; ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);