diff --git a/src/host/p25/Control.cpp b/src/host/p25/Control.cpp index 569dcc4a..2f7afa5e 100644 --- a/src/host/p25/Control.cpp +++ b/src/host/p25/Control.cpp @@ -1487,6 +1487,7 @@ void Control::processNetwork() } uint32_t blockLength = GET_UINT24(buffer, 8U); + uint8_t totalBlocks = data[20U] + 1U; uint8_t currentBlock = buffer[21U]; if (m_debug) { @@ -1494,7 +1495,7 @@ void Control::processNetwork() } if (!m_dedicatedControl) - m_data->processNetwork(data.get(), frameLength, currentBlock, blockLength); + m_data->processNetwork(data.get(), frameLength, currentBlock, blockLength, totalBlocks); return; } diff --git a/src/host/p25/packet/Data.cpp b/src/host/p25/packet/Data.cpp index 4c030073..dd868c47 100644 --- a/src/host/p25/packet/Data.cpp +++ b/src/host/p25/packet/Data.cpp @@ -390,146 +390,184 @@ bool Data::process(uint8_t* data, uint32_t len) /* Process a data frame from the network. */ -bool Data::processNetwork(uint8_t* data, uint32_t len, uint8_t currentBlock, uint32_t blockLength) +bool Data::processNetwork(uint8_t* data, uint32_t len, uint8_t currentBlock, uint32_t blockLength, uint16_t totalBlocks) { - if ((m_p25->m_netState != RS_NET_DATA) || (currentBlock == 0U)) { + if (m_p25->m_netState != RS_NET_DATA) { m_p25->m_netState = RS_NET_DATA; m_inbound = false; + } - bool ret = m_netAssembler->disassemble(data + 24U, blockLength, true); - if (!ret) { - m_p25->m_netState = RS_NET_IDLE; - return false; - } - - // if we're a dedicated CC or in control only mode, we only want to handle AMBTs. Otherwise return - if ((m_p25->m_dedicatedControl || m_p25->m_controlOnly) && m_netAssembler->dataHeader.getFormat() != PDUFormatType::AMBT) { - if (m_debug) { - LogDebug(LOG_NET, "CC only mode, ignoring non-AMBT PDU from network"); - } + LogInfoEx(LOG_NET, P25_PDU_STR ", received block %u, len = %u, totalBlocks = %u", + currentBlock, blockLength, totalBlocks); - m_p25->m_netState = RS_NET_IDLE; - return false; - } + // store the received block + uint8_t* blockData = new uint8_t[blockLength]; + ::memcpy(blockData, data + 24U, blockLength); + m_netReceivedBlocks[currentBlock] = blockData; + m_netDataBlockCnt++; + m_netTotalBlocks = totalBlocks; - // did we receive a response header? - if (m_netAssembler->dataHeader.getFormat() == PDUFormatType::RSP) { - m_p25->m_netState = RS_NET_IDLE; + if (m_p25->m_netState == RS_NET_DATA) { + if (m_netDataBlockCnt == m_netTotalBlocks) { + for (uint16_t i = 0U; i < totalBlocks; i++) { + if (m_netReceivedBlocks.find(i) != m_netReceivedBlocks.end()) { + // block 0 is always the PDU header block + if (i == 0U) { + bool ret = m_netAssembler->disassemble(m_netReceivedBlocks[i], blockLength, true); + if (!ret) { + m_p25->m_netState = RS_NET_IDLE; + resetReceivedBlocks(); + return false; + } - if (m_verbose) { - LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, fmt = $%02X, rspClass = $%02X, rspType = $%02X, rspStatus = $%02X, llId = %u, srcLlId = %u", - m_netAssembler->dataHeader.getFormat(), m_netAssembler->dataHeader.getResponseClass(), m_netAssembler->dataHeader.getResponseType(), m_netAssembler->dataHeader.getResponseStatus(), - m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getSrcLLId()); + // if we're a dedicated CC or in control only mode, we only want to handle AMBTs. Otherwise return + if ((m_p25->m_dedicatedControl || m_p25->m_controlOnly) && m_netAssembler->dataHeader.getFormat() != PDUFormatType::AMBT) { + if (m_debug) { + LogDebug(LOG_NET, "CC only mode, ignoring non-AMBT PDU from network"); + } - if (m_netAssembler->dataHeader.getResponseClass() == PDUAckClass::ACK && m_netAssembler->dataHeader.getResponseType() == PDUAckType::ACK) { - LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP ACK, llId = %u, all blocks received OK, n = %u", - m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getResponseStatus()); - } else { - if (m_netAssembler->dataHeader.getResponseClass() == PDUAckClass::NACK) { - switch (m_netAssembler->dataHeader.getResponseType()) { - case PDUAckType::NACK_ILLEGAL: - LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP NACK, illegal format, llId = %u", - m_netAssembler->dataHeader.getLLId()); - break; - case PDUAckType::NACK_PACKET_CRC: - LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP NACK, packet CRC error, llId = %u, n = %u", - m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getResponseStatus()); - break; - case PDUAckType::NACK_SEQ: - case PDUAckType::NACK_OUT_OF_SEQ: - LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP NACK, packet out of sequence, llId = %u, seqNo = %u", - m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getResponseStatus()); - break; - case PDUAckType::NACK_UNDELIVERABLE: - LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP NACK, packet undeliverable, llId = %u, n = %u", - m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getResponseStatus()); - break; + m_p25->m_netState = RS_NET_IDLE; + resetReceivedBlocks(); + return false; + } - default: - break; + // did we receive a response header? + if (m_netAssembler->dataHeader.getFormat() == PDUFormatType::RSP) { + m_p25->m_netState = RS_NET_IDLE; + + if (m_verbose) { + LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, fmt = $%02X, rspClass = $%02X, rspType = $%02X, rspStatus = $%02X, llId = %u, srcLlId = %u", + m_netAssembler->dataHeader.getFormat(), m_netAssembler->dataHeader.getResponseClass(), m_netAssembler->dataHeader.getResponseType(), m_netAssembler->dataHeader.getResponseStatus(), + m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getSrcLLId()); + + if (m_netAssembler->dataHeader.getResponseClass() == PDUAckClass::ACK && m_netAssembler->dataHeader.getResponseType() == PDUAckType::ACK) { + LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP ACK, llId = %u, all blocks received OK, n = %u", + m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getResponseStatus()); + } else { + if (m_netAssembler->dataHeader.getResponseClass() == PDUAckClass::NACK) { + switch (m_netAssembler->dataHeader.getResponseType()) { + case PDUAckType::NACK_ILLEGAL: + LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP NACK, illegal format, llId = %u", + m_netAssembler->dataHeader.getLLId()); + break; + case PDUAckType::NACK_PACKET_CRC: + LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP NACK, packet CRC error, llId = %u, n = %u", + m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getResponseStatus()); + break; + case PDUAckType::NACK_SEQ: + case PDUAckType::NACK_OUT_OF_SEQ: + LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP NACK, packet out of sequence, llId = %u, seqNo = %u", + m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getResponseStatus()); + break; + case PDUAckType::NACK_UNDELIVERABLE: + LogInfoEx(LOG_NET, P25_PDU_STR ", FNE ISP, response, OSP NACK, packet undeliverable, llId = %u, n = %u", + m_netAssembler->dataHeader.getLLId(), m_netAssembler->dataHeader.getResponseStatus()); + break; + + default: + break; + } + } + } } - } - } - } - writeRF_PDU_Ack_Response(m_netAssembler->dataHeader.getResponseClass(), m_netAssembler->dataHeader.getResponseType(), m_netAssembler->dataHeader.getResponseStatus(), - m_netAssembler->dataHeader.getLLId(), (m_netAssembler->dataHeader.getSrcLLId() > 0U), m_netAssembler->dataHeader.getSrcLLId()); - } + writeRF_PDU_Ack_Response(m_netAssembler->dataHeader.getResponseClass(), m_netAssembler->dataHeader.getResponseType(), m_netAssembler->dataHeader.getResponseStatus(), + m_netAssembler->dataHeader.getLLId(), (m_netAssembler->dataHeader.getSrcLLId() > 0U), m_netAssembler->dataHeader.getSrcLLId()); - return true; - } - - if (m_p25->m_netState == RS_NET_DATA) { - // block 0 is always the PDU header block -- if we got here with that bail bail bail - if (currentBlock == 0U) { - return false; // bail - } + resetReceivedBlocks(); + return true; + } - bool ret = m_netAssembler->disassemble(data + 24U, blockLength); - if (!ret) { - m_p25->m_netState = RS_NET_IDLE; - return false; - } - else { - if (m_netAssembler->getComplete()) { - m_netPduUserDataLength = m_netAssembler->getUserDataLength(); - m_netAssembler->getUserData(m_netPduUserData); + continue; + } - uint32_t srcId = (m_netAssembler->getExtendedAddress()) ? m_netAssembler->dataHeader.getSrcLLId() : m_netAssembler->dataHeader.getLLId(); - uint32_t dstId = m_netAssembler->dataHeader.getLLId(); + bool ret = m_netAssembler->disassemble(m_netReceivedBlocks[i], blockLength); + if (!ret) { + m_p25->m_netState = RS_NET_IDLE; + resetReceivedBlocks(); + return false; + } + else { + if (m_netAssembler->getComplete()) { + m_netPduUserDataLength = m_netAssembler->getUserDataLength(); + m_netAssembler->getUserData(m_netPduUserData); + + uint32_t srcId = (m_netAssembler->getExtendedAddress()) ? m_netAssembler->dataHeader.getSrcLLId() : m_netAssembler->dataHeader.getLLId(); + uint32_t dstId = m_netAssembler->dataHeader.getLLId(); + + uint8_t sap = (m_netAssembler->getExtendedAddress()) ? m_netAssembler->dataHeader.getEXSAP() : m_netAssembler->dataHeader.getSAP(); + if (m_netAssembler->getAuxiliaryES()) + sap = m_netAssembler->dataHeader.getEXSAP(); + + // 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_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 tgtHWAddr = GET_UINT24(arpPacket, 15U); + uint32_t tgtProtoAddr = GET_UINT32(arpPacket, 18U); + + if (m_verbose) { + if (opcode == P25_PDU_ARP_REQUEST) { + LogInfoEx(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) { + LogInfoEx(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_netAssembler->dataHeader.getBlocksToFollow()); + LogInfoEx(LOG_NET, "P25 Data Call, srcId = %u, dstId = %u", srcId, dstId); - uint8_t sap = (m_netAssembler->getExtendedAddress()) ? m_netAssembler->dataHeader.getEXSAP() : m_netAssembler->dataHeader.getSAP(); - if (m_netAssembler->getAuxiliaryES()) - sap = m_netAssembler->dataHeader.getEXSAP(); + if (m_verbose) { + LogInfoEx(LOG_NET, P25_PDU_STR ", transmitting network PDU, llId = %u", (m_netAssembler->getExtendedAddress()) ? m_netAssembler->dataHeader.getSrcLLId() : m_netAssembler->dataHeader.getLLId()); + } - // 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_ARP_PCKT_LENGTH); + writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on - uint16_t opcode = GET_UINT16(arpPacket, 6U); - uint32_t srcHWAddr = GET_UINT24(arpPacket, 8U); - uint32_t srcProtoAddr = GET_UINT32(arpPacket, 11U); - //uint32_t tgtHWAddr = GET_UINT24(arpPacket, 15U); - uint32_t tgtProtoAddr = GET_UINT32(arpPacket, 18U); + ::ActivityLog("P25", false, "end of Net data transmission"); + break; + } - if (m_verbose) { - if (opcode == P25_PDU_ARP_REQUEST) { - LogInfoEx(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) { - LogInfoEx(LOG_NET, P25_PDU_STR ", ARP reply, %s is at %u", __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr); + m_netPduUserDataLength = 0U; + m_p25->m_netState = RS_NET_IDLE; + m_p25->m_network->resetP25(); + resetReceivedBlocks(); + break; } } - - 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_netAssembler->dataHeader.getBlocksToFollow()); - LogInfoEx(LOG_NET, "P25 Data Call, srcId = %u, dstId = %u", srcId, dstId); - - if (m_verbose) { - LogInfoEx(LOG_NET, P25_PDU_STR ", transmitting network PDU, llId = %u", (m_netAssembler->getExtendedAddress()) ? m_netAssembler->dataHeader.getSrcLLId() : m_netAssembler->dataHeader.getLLId()); - } + } + } + } - writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on + return true; +} - ::ActivityLog("P25", false, "end of Net data transmission"); - break; - } +/* Helper to reset received network blocks. */ - m_netPduUserDataLength = 0U; - m_p25->m_netState = RS_NET_IDLE; - m_p25->m_network->resetP25(); - } +void Data::resetReceivedBlocks() +{ + for (auto it : m_netReceivedBlocks) { + if (it.second != nullptr) { + delete[] it.second; + it.second = nullptr; } } + m_netReceivedBlocks.clear(); - return true; + m_netDataBlockCnt = 0U; + m_netTotalBlocks = 0U; } /* Helper to check if a logical link ID has registered with data services. */ @@ -723,6 +761,9 @@ Data::Data(Control* p25, bool dumpPDUData, bool repeatPDU, bool debug, bool verb m_rfPDUCount(0U), m_rfPDUBits(0U), m_netAssembler(nullptr), + m_netReceivedBlocks(), + m_netDataBlockCnt(0U), + m_netTotalBlocks(0U), m_retryPDUData(nullptr), m_retryPDUBitLength(0U), m_retryCount(0U), diff --git a/src/host/p25/packet/Data.h b/src/host/p25/packet/Data.h index 9d125dc0..6c550270 100644 --- a/src/host/p25/packet/Data.h +++ b/src/host/p25/packet/Data.h @@ -70,9 +70,11 @@ namespace p25 * @param len Length of data frame. * @param currentBlock * @param blockLength + * @param totalBlocks * @returns bool True, if data frame is processed, otherwise false. */ - bool processNetwork(uint8_t* data, uint32_t len, uint8_t currentBlock, uint32_t blockLength); + bool processNetwork(uint8_t* data, uint32_t len, uint8_t currentBlock, uint32_t blockLength, + uint16_t totalBlocks); /** @} */ /** @@ -136,6 +138,9 @@ namespace p25 uint32_t m_rfPDUCount; uint32_t m_rfPDUBits; data::Assembler* m_netAssembler; + std::unordered_map m_netReceivedBlocks; + uint16_t m_netDataBlockCnt; + uint16_t m_netTotalBlocks; uint8_t* m_retryPDUData; uint32_t m_retryPDUBitLength; @@ -196,6 +201,11 @@ namespace p25 */ void writeNetwork(const uint8_t currentBlock, const uint8_t* data, uint32_t len, bool lastBlock); + /** + * @brief Helper to reset received network blocks. + */ + void resetReceivedBlocks(); + /** * @brief Helper to write a P25 PDU packet. * @param[in] pdu Constructed PDU to transmit.