diff --git a/src/common/Utils.h b/src/common/Utils.h index 6b1a4d81..7a6c09b2 100644 --- a/src/common/Utils.h +++ b/src/common/Utils.h @@ -25,6 +25,8 @@ #include +#include + // --------------------------------------------------------------------------- // Constants // --------------------------------------------------------------------------- @@ -88,17 +90,29 @@ inline std::string __FLOAT_STR(const float& value) { } /** - * @brief IP address from ulong64_t value. + * @brief IP address from uint32_t value. * @ingroup utils * @param value Packed IP address. * @return std::string String representation of the packed IP address. */ -inline std::string __IP_FROM_ULONG(const ulong64_t& value) { +inline std::string __IP_FROM_UINT(const uint32_t& value) { std::stringstream ss; ss << ((value >> 24) & 0xFFU) << "." << ((value >> 16) & 0xFFU) << "." << ((value >> 8) & 0xFFU) << "." << (value & 0xFFU); return ss.str(); } +/** + * @brief IP address from uint32_t value. + * @ingroup utils + * @param value String representation of the IP address. + * @return uint32_t Packed IP address. + */ +inline uint32_t __IP_FROM_STR(const std::string& value) { + struct sockaddr_in sa; + inet_pton(AF_INET, value.c_str(), &(sa.sin_addr)); + return (uint32_t)sa.sin_addr.s_addr; +} + /** * @brief Helper to lower-case an input string. * @ingroup utils diff --git a/src/common/p25/P25Defines.h b/src/common/p25/P25Defines.h index 0911db26..779ba5eb 100644 --- a/src/common/p25/P25Defines.h +++ b/src/common/p25/P25Defines.h @@ -391,6 +391,8 @@ namespace p25 /** @brief SNDCP version 1 */ const uint8_t SNDCP_VERSION_1 = 0x01U; + /** @brief 296 byte MTU */ + const uint8_t SNDCP_MTU_296 = 1U; /** @brief 510 byte MTU */ const uint8_t SNDCP_MTU_510 = 2U; diff --git a/src/common/p25/data/DataHeader.cpp b/src/common/p25/data/DataHeader.cpp index 603bf58b..6588f4a2 100644 --- a/src/common/p25/data/DataHeader.cpp +++ b/src/common/p25/data/DataHeader.cpp @@ -20,6 +20,7 @@ using namespace p25::data; #include #include +#include // --------------------------------------------------------------------------- // Static Class Members @@ -294,6 +295,10 @@ void DataHeader::reset() uint32_t DataHeader::getPacketLength() 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 - 4 - m_padLength; } @@ -313,11 +318,26 @@ uint32_t DataHeader::getData(uint8_t* buffer) const return P25_PDU_HEADER_LENGTH_BYTES; } +/* Helper to calculate the number of blocks to follow and padding length for a PDU. */ + +void DataHeader::calculateLength(uint32_t packetLength) +{ + uint32_t len = packetLength + 4; // packet length + CRC32 + uint32_t blockLen = (m_fmt == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; + m_padLength = blockLen - (len % blockLen); + + if (len > blockLen) { + m_blocksToFollow = (uint8_t)ceilf((float)len / (float)blockLen); + } else { + m_blocksToFollow = 1U; + } +} + /* Helper to determine the pad length for a given packet length. */ uint32_t DataHeader::calculatePadLength(uint8_t fmt, uint32_t packetLength) { - uint32_t len = packetLength + 4; + uint32_t len = packetLength + 4; // packet length + CRC32 if (fmt == PDUFormatType::CONFIRMED) { return P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - (len % P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); } diff --git a/src/common/p25/data/DataHeader.h b/src/common/p25/data/DataHeader.h index e85b7b01..7cce148f 100644 --- a/src/common/p25/data/DataHeader.h +++ b/src/common/p25/data/DataHeader.h @@ -80,6 +80,12 @@ namespace p25 */ uint32_t getData(uint8_t* buffer) const; + /** + * @brief Helper to calculate the number of blocks to follow and padding length for a PDU. + * @param packetLength Length of PDU. + */ + void calculateLength(uint32_t packetLength); + /** * @brief Sets the flag indicating CRC-errors should be warnings and not errors. * @param warnCRC Flag indicating CRC-errors should be treated as warnings. diff --git a/src/common/p25/sndcp/SNDCPCtxActAccept.cpp b/src/common/p25/sndcp/SNDCPCtxActAccept.cpp index 87bc356a..dd7010ce 100644 --- a/src/common/p25/sndcp/SNDCPCtxActAccept.cpp +++ b/src/common/p25/sndcp/SNDCPCtxActAccept.cpp @@ -30,7 +30,9 @@ SNDCPCtxActAccept::SNDCPCtxActAccept() : SNDCPPacket(), m_standbyTimer(SNDCPStandbyTimer::ONE_MINUTE), m_nat(SNDCPNAT::IPV4_STATIC_ADDR), m_ipAddress(0U), - m_mtu(SNDCP_MTU_510) + m_mtu(SNDCP_MTU_510), + m_mdpco(0U), + m_sndcpDAC(1U) { m_pduType = SNDCP_PDUType::ACT_TDS_CTX; } @@ -41,20 +43,7 @@ bool SNDCPCtxActAccept::decode(const uint8_t* data) { assert(data != nullptr); - SNDCPPacket::decodeHeader(data, false); - - m_priority = (uint8_t)((data[1U] >> 4) & 0x0FU); // Priority - m_readyTimer = (uint8_t)(data[1U] & 0x0FU); // Ready Timer - m_standbyTimer = (uint8_t)((data[2U] >> 4) & 0x0FU); // Standby Timer - m_nat = (uint8_t)(data[2U] & 0x0FU); // NAT - - m_ipAddress = 0U; // IP Address - m_ipAddress = data[3U]; - m_ipAddress = (m_ipAddress << 8) + data[4U]; - m_ipAddress = (m_ipAddress << 8) + data[5U]; - m_ipAddress = (m_ipAddress << 8) + data[6U]; - - m_mtu = (uint8_t)((data[9U] >> 4) & 0x0FU); // MTU + /* stub */ return true; } @@ -78,6 +67,10 @@ void SNDCPCtxActAccept::encode(uint8_t* data) data[6U] = (uint8_t)((m_ipAddress >> 0) & 0xFFU); data[9U] = ((m_mtu << 4U) & 0xF0U); // MTU + + data[10U] = (m_mdpco & 0x0FU); // MDPCO + + __SET_UINT16B(m_sndcpDAC, data, 11U); // Data Access Control } // --------------------------------------------------------------------------- diff --git a/src/common/p25/sndcp/SNDCPCtxActAccept.h b/src/common/p25/sndcp/SNDCPCtxActAccept.h index 36acb173..54752066 100644 --- a/src/common/p25/sndcp/SNDCPCtxActAccept.h +++ b/src/common/p25/sndcp/SNDCPCtxActAccept.h @@ -74,14 +74,22 @@ namespace p25 /** * @brief IP Address */ - __PROPERTY(ulong64_t, ipAddress, IPAddress); + __PROPERTY(uint32_t, ipAddress, IPAddress); /** * @brief MTU */ __PROPERTY(uint8_t, mtu, MTU); + /** + * @brief MDPCO + */ + __PROPERTY(uint8_t, mdpco, MDPCO); + __COPY(SNDCPCtxActAccept); + + private: + uint16_t m_sndcpDAC; }; } // namespace sndcp } // namespace p25 diff --git a/src/common/p25/sndcp/SNDCPCtxActRequest.cpp b/src/common/p25/sndcp/SNDCPCtxActRequest.cpp index 96f35d82..9be2f8b7 100644 --- a/src/common/p25/sndcp/SNDCPCtxActRequest.cpp +++ b/src/common/p25/sndcp/SNDCPCtxActRequest.cpp @@ -27,7 +27,8 @@ using namespace p25::sndcp; SNDCPCtxActRequest::SNDCPCtxActRequest() : SNDCPPacket(), m_nat(SNDCPNAT::IPV4_NO_ADDRESS), m_ipAddress(0U), - m_dsut(SNDCP_DSUT::ALT_T_AND_C_DATA_VOICE) + m_dsut(SNDCP_DSUT::ALT_T_AND_C_DATA_VOICE), + m_mdpco(0U) { m_pduType = SNDCP_PDUType::ACT_TDS_CTX; } @@ -51,6 +52,8 @@ bool SNDCPCtxActRequest::decode(const uint8_t* data) m_dsut = (uint8_t)((data[6U] >> 4) & 0x0FU); // Data Subscriber Unit Type + m_mdpco = (uint8_t)(data[9U] & 0x0FU); // MDPCO + return true; } @@ -71,6 +74,8 @@ void SNDCPCtxActRequest::encode(uint8_t* data) data[5U] = (uint8_t)((m_ipAddress >> 0) & 0xFFU); data[6U] = ((m_dsut << 4U) & 0xF0U); // Data Subscriber Unit Type + + data[9U] = (m_mdpco & 0x0FU); // MDPCO } /* Internal helper to copy the the class. */ diff --git a/src/common/p25/sndcp/SNDCPCtxActRequest.h b/src/common/p25/sndcp/SNDCPCtxActRequest.h index b18ccfdf..65ae9a22 100644 --- a/src/common/p25/sndcp/SNDCPCtxActRequest.h +++ b/src/common/p25/sndcp/SNDCPCtxActRequest.h @@ -69,6 +69,11 @@ namespace p25 */ __PROPERTY(uint8_t, dsut, DSUT); + /** + * @brief MDPCO + */ + __PROPERTY(uint8_t, mdpco, MDPCO); + __COPY(SNDCPCtxActRequest); }; } // namespace sndcp diff --git a/src/host/p25/Control.cpp b/src/host/p25/Control.cpp index 61a73743..1ee93fb5 100644 --- a/src/host/p25/Control.cpp +++ b/src/host/p25/Control.cpp @@ -1623,7 +1623,7 @@ void Control::writeRF_Preamble(uint32_t preambleCount, bool force) /* Helper to write a P25 TDU packet. */ -void Control::writeRF_TDU(bool noNetwork) +void Control::writeRF_TDU(bool noNetwork, bool imm) { uint8_t data[P25_TDU_FRAME_LENGTH_BYTES + 2U]; ::memset(data + 2U, 0x00U, P25_TDU_FRAME_LENGTH_BYTES); @@ -1644,7 +1644,7 @@ void Control::writeRF_TDU(bool noNetwork) data[0U] = modem::TAG_EOT; data[1U] = 0x00U; - addFrame(data, P25_TDU_FRAME_LENGTH_BYTES + 2U); + addFrame(data, P25_TDU_FRAME_LENGTH_BYTES + 2U, false, imm); } } diff --git a/src/host/p25/Control.h b/src/host/p25/Control.h index d69b55a4..00c5e2f4 100644 --- a/src/host/p25/Control.h +++ b/src/host/p25/Control.h @@ -424,8 +424,9 @@ namespace p25 /** * @brief Helper to write a P25 TDU packet. * @param noNetwork Flag indicating this TDU shouldn't be written to the network. + * @param imm Flag indicating the TDU should be written to the immediate queue. */ - void writeRF_TDU(bool noNetwork); + void writeRF_TDU(bool noNetwork, bool imm = false); /** * @brief Helper to setup and generate LLA AM1 parameters. diff --git a/src/host/p25/packet/ControlSignaling.cpp b/src/host/p25/packet/ControlSignaling.cpp index 0591e395..8e2c3b39 100644 --- a/src/host/p25/packet/ControlSignaling.cpp +++ b/src/host/p25/packet/ControlSignaling.cpp @@ -473,7 +473,7 @@ bool ControlSignaling::process(uint8_t* data, uint32_t len, std::unique_ptrgetService(), false, true); } break; case TSBKO::IOSP_EXT_FNCT: @@ -1608,7 +1608,7 @@ void ControlSignaling::writeRF_TSDU_AMBT(lc::AMBT* ambt) Utils::dump(1U, "!!! *PDU (AMBT) TSBK Block Data", pduUserData, P25_PDU_UNCONFIRMED_LENGTH_BYTES * header.getBlocksToFollow()); } - m_p25->m_data->writeRF_PDU_User(header, header, false, pduUserData); + m_p25->m_data->writeRF_PDU_User(header, header, false, pduUserData, true); } /* diff --git a/src/host/p25/packet/Data.cpp b/src/host/p25/packet/Data.cpp index 27af0e0e..42cf29a5 100644 --- a/src/host/p25/packet/Data.cpp +++ b/src/host/p25/packet/Data.cpp @@ -380,7 +380,7 @@ bool Data::process(uint8_t* data, uint32_t len) } } } - +/* if (m_repeatPDU) { if (!m_rfDataHeader.getFullMessage()) { m_rfDataHeader.setSAP(PDUSAP::EXT_ADDR); @@ -389,6 +389,7 @@ bool Data::process(uint8_t* data, uint32_t len) writeRF_PDU_Ack_Response(m_rfDataHeader.getResponseClass(), m_rfDataHeader.getResponseType(), m_rfDataHeader.getResponseStatus(), m_rfDataHeader.getLLId(), m_rfDataHeader.getSrcLLId()); } +*/ } else { // handle standard P25 service access points @@ -404,7 +405,7 @@ bool Data::process(uint8_t* data, uint32_t len) (m_pduUserData[10U] << 8) + m_pduUserData[11U]; if (m_verbose) { - LogMessage(LOG_RF, P25_PDU_STR ", CONNECT (Registration Request Connect), llId = %u, ipAddr = %s", llId, __IP_FROM_ULONG(ipAddr).c_str()); + LogMessage(LOG_RF, P25_PDU_STR ", CONNECT (Registration Request Connect), llId = %u, ipAddr = %s", llId, __IP_FROM_UINT(ipAddr).c_str()); } m_connQueueTable[llId] = std::make_tuple(m_rfDataHeader.getMFId(), ipAddr); @@ -745,10 +746,12 @@ bool Data::hasLLIdFNEReg(uint32_t llId) const /* Helper to write user data as a P25 PDU packet. */ -void Data::writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& secondHeader, bool useSecondHeader, uint8_t* pduUserData) +void Data::writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& secondHeader, bool useSecondHeader, uint8_t* pduUserData, bool imm) { assert(pduUserData != nullptr); + m_p25->writeRF_TDU(true, imm); + uint32_t bitLength = ((dataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; uint32_t offset = P25_PREAMBLE_LENGTH_BITS; @@ -760,9 +763,9 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& seco 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, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %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, n = %u, seqNo = %u, lastFragment = %u, hdrOffset = %u, llId = %u", dataHeader.getAckNeeded(), dataHeader.getOutbound(), dataHeader.getFormat(), dataHeader.getMFId(), dataHeader.getSAP(), dataHeader.getFullMessage(), - dataHeader.getBlocksToFollow(), dataHeader.getPadLength(), dataHeader.getNs(), dataHeader.getFSN(), dataHeader.getLastFragment(), + dataHeader.getBlocksToFollow(), dataHeader.getPadLength(), dataHeader.getPacketLength(), dataHeader.getNs(), dataHeader.getFSN(), dataHeader.getLastFragment(), dataHeader.getHeaderOffset(), dataHeader.getLLId()); } @@ -795,7 +798,7 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& seco } if (dataHeader.getFormat() != PDUFormatType::AMBT) { - edac::CRC::addCRC32(pduUserData, m_pduUserDataLength); + edac::CRC::addCRC32(pduUserData, (dataHeader.getPacketLength() + dataHeader.getPadLength() + 4U)); } // generate the PDU data @@ -843,7 +846,7 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& seco dataOffset += (dataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; } - writeRF_PDU(data, bitLength); + writeRF_PDU(data, bitLength, false, imm); } /* Updates the processor by the passed number of milliseconds. */ @@ -867,7 +870,7 @@ void Data::clock(uint32_t ms) uint64_t ipAddr = std::get<1>(m_connQueueTable[llId]); if (!acl::AccessControl::validateSrcId(llId)) { - LogWarning(LOG_RF, P25_PDU_STR ", DENY (Registration Response Deny), llId = %u, ipAddr = %s", llId, __IP_FROM_ULONG(ipAddr).c_str()); + LogWarning(LOG_RF, P25_PDU_STR ", DENY (Registration Response Deny), llId = %u, ipAddr = %s", llId, __IP_FROM_UINT(ipAddr).c_str()); writeRF_PDU_Reg_Response(PDURegType::DENY, mfId, llId, ipAddr); } else { @@ -877,7 +880,7 @@ void Data::clock(uint32_t ms) } if (m_verbose) { - LogMessage(LOG_RF, P25_PDU_STR ", ACCPT (Registration Response Accept), llId = %u, ipAddr = %s", llId, __IP_FROM_ULONG(ipAddr).c_str()); + LogMessage(LOG_RF, P25_PDU_STR ", ACCPT (Registration Response Accept), llId = %u, ipAddr = %s", llId, __IP_FROM_UINT(ipAddr).c_str()); } writeRF_PDU_Reg_Response(PDURegType::ACCPT, mfId, llId, ipAddr); @@ -898,6 +901,17 @@ void Data::clock(uint32_t ms) } } + // clock all the SNDCP standby timers + std::vector sndcpStandbyExpired = std::vector(); + for (auto entry : m_sndcpStandbyTimers) { + uint32_t llId = entry.first; + + m_sndcpStandbyTimers[llId].clock(ms); + if (m_sndcpStandbyTimers[llId].isRunning() && m_sndcpStandbyTimers[llId].hasExpired()) { + sndcpStandbyExpired.push_back(llId); + } + } + // process and SNDCP enabled LLIDs for (auto entry : m_sndcpStateTable) { uint32_t llId = entry.first; @@ -929,6 +943,9 @@ void Data::clock(uint32_t ms) std::unique_ptr lc = std::make_unique(); m_p25->m_control->writeRF_TDULC(lc.get(), true); + for (uint8_t i = 0U; i < 8U; i++) { + m_p25->writeRF_TDU(true); + } if (m_p25->m_notifyCC) { m_p25->notifyCC_ReleaseGrant(llId); @@ -936,6 +953,14 @@ void Data::clock(uint32_t ms) } } break; + case SNDCPState::STANDBY: + { + // has the LLID reached standby state expiration? + if (std::find(sndcpStandbyExpired.begin(), sndcpStandbyExpired.end(), llId) != sndcpStandbyExpired.end()) { + sndcpReset(llId); + } + } + break; case SNDCPState::READY: break; default: @@ -992,6 +1017,9 @@ void Data::sndcpReset(uint32_t llId, bool callTerm) std::unique_ptr lc = std::make_unique(); m_p25->m_control->writeRF_TDULC(lc.get(), true); + for (uint8_t i = 0U; i < 8U; i++) { + m_p25->writeRF_TDU(true); + } if (m_p25->m_notifyCC) { m_p25->notifyCC_ReleaseGrant(llId); @@ -1081,8 +1109,8 @@ bool Data::processSNDCPControl() return false; } - uint8_t data[P25_PDU_UNCONFIRMED_LENGTH_BYTES]; - ::memset(data, 0x00U, P25_PDU_UNCONFIRMED_LENGTH_BYTES); + uint8_t pduUserData[P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES]; + ::memset(pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES); std::unique_ptr packet = SNDCPFactory::create(m_pduUserData); if (packet == nullptr) { @@ -1097,8 +1125,8 @@ bool Data::processSNDCPControl() { SNDCPCtxActRequest* isp = static_cast(packet.get()); if (m_verbose) { - LogMessage(LOG_RF, P25_PDU_STR ", SNDCP context activation request, llId = %u, ipAddr = %08lX, nat = $%02X, dsut = $%02X", llId, - isp->getIPAddress(), isp->getNAT(), isp->getDSUT()); + LogMessage(LOG_RF, P25_PDU_STR ", SNDCP context activation request, llId = %u, ipAddr = %s, nat = $%02X, dsut = $%02X, mdpco = $%02X", llId, + __IP_FROM_UINT(isp->getIPAddress()).c_str(), isp->getNAT(), isp->getDSUT(), isp->getMDPCO()); } DataHeader rspHeader = DataHeader(); @@ -1107,9 +1135,22 @@ bool Data::processSNDCPControl() rspHeader.setAckNeeded(true); rspHeader.setOutbound(true); rspHeader.setSAP(PDUSAP::SNDCP_CTRL_DATA); + rspHeader.setNs(m_rfDataHeader.getNs()); rspHeader.setLLId(llId); rspHeader.setBlocksToFollow(1U); + if (!isSNDCPInitialized(llId)) { + std::unique_ptr osp = std::make_unique(); + osp->setNSAPI(packet->getNSAPI()); + osp->setRejectCode(SNDCPRejectReason::SU_NOT_PROVISIONED); + + osp->encode(pduUserData); + + rspHeader.calculateLength(2U); + writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData); + return true; + } + // which network address type is this? switch (isp->getNAT()) { case SNDCPNAT::IPV4_STATIC_ADDR: @@ -1118,12 +1159,12 @@ bool Data::processSNDCPControl() osp->setNSAPI(packet->getNSAPI()); osp->setRejectCode(SNDCPRejectReason::STATIC_IP_ALLOCATION_UNSUPPORTED); - osp->encode(data); - writeRF_PDU_User(rspHeader, rspHeader, false, data); + osp->encode(pduUserData); - sndcpReset(llId, true); + rspHeader.calculateLength(2U); + writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData); - // TODO TODO TODO + sndcpReset(llId, true); } break; @@ -1133,11 +1174,12 @@ bool Data::processSNDCPControl() osp->setNSAPI(packet->getNSAPI()); osp->setRejectCode(SNDCPRejectReason::DYN_IP_ALLOCATION_UNSUPPORTED); - osp->encode(data); - writeRF_PDU_User(rspHeader, rspHeader, false, data); + osp->encode(pduUserData); - sndcpReset(llId, true); + rspHeader.calculateLength(2U); + writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData); + sndcpReset(llId, true); // TODO TODO TODO } break; @@ -1148,8 +1190,10 @@ bool Data::processSNDCPControl() osp->setNSAPI(packet->getNSAPI()); osp->setRejectCode(SNDCPRejectReason::ANY_REASON); - osp->encode(data); - writeRF_PDU_User(rspHeader, rspHeader, false, data); + osp->encode(pduUserData); + + rspHeader.calculateLength(2U); + writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData); sndcpReset(llId, true); } @@ -1199,7 +1243,7 @@ void Data::writeNetwork(const uint8_t currentBlock, const uint8_t *data, uint32_ /* Helper to write a P25 PDU packet. */ -void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls) +void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls, bool imm) { assert(pdu != nullptr); assert(bitLength > 0U); @@ -1229,13 +1273,20 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls) data[0U] = modem::TAG_DATA; data[1U] = 0x00U; - m_p25->addFrame(data, newByteLength + 2U); + m_p25->addFrame(data, newByteLength + 2U, false, imm); } // add trailing null pad; only if control data isn't being transmitted if (!m_p25->m_ccRunning && !noNulls) { m_p25->writeRF_Nulls(); } + + // transmit TDUs; only if control data isn't being transmitted + if (!m_p25->m_ccRunning) { + for (uint8_t i = 0U; i < 8U; i++) { + m_p25->writeRF_TDU(true); + } + } } /* Helper to write a network P25 PDU packet. */ @@ -1436,18 +1487,13 @@ void Data::writeRF_PDU_Buffered() /* Helper to write a PDU registration response. */ -void Data::writeRF_PDU_Reg_Response(uint8_t regType, uint8_t mfId, uint32_t llId, ulong64_t ipAddr) +void Data::writeRF_PDU_Reg_Response(uint8_t regType, uint8_t mfId, uint32_t llId, uint32_t ipAddr) { if ((regType != PDURegType::ACCPT) && (regType != PDURegType::DENY)) return; - uint32_t bitLength = (2U * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS; - uint32_t offset = P25_PREAMBLE_LENGTH_BITS; - - uint8_t data[bitLength / 8U]; - ::memset(data, 0x00U, bitLength / 8U); - uint8_t block[P25_PDU_FEC_LENGTH_BYTES]; - ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); + uint8_t pduUserData[P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES]; + ::memset(pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES); DataHeader rspHeader = DataHeader(); rspHeader.setFormat(PDUFormatType::CONFIRMED); @@ -1458,39 +1504,19 @@ void Data::writeRF_PDU_Reg_Response(uint8_t regType, uint8_t mfId, uint32_t llId rspHeader.setLLId(llId); rspHeader.setBlocksToFollow(1U); - // Generate the PDU header and 1/2 rate Trellis - rspHeader.encode(block); - Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS); - offset += P25_PDU_FEC_LENGTH_BITS; - - // build registration response data - uint8_t rspData[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES]; - ::memset(rspData, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); - - rspData[0U] = ((regType & 0x0FU) << 4); // Registration Type & Options - rspData[1U] = (llId >> 16) & 0xFFU; // Logical Link ID - rspData[2U] = (llId >> 8) & 0xFFU; - rspData[3U] = (llId >> 0) & 0xFFU; + pduUserData[0U] = ((regType & 0x0FU) << 4); // Registration Type & Options + pduUserData[1U] = (llId >> 16) & 0xFFU; // Logical Link ID + pduUserData[2U] = (llId >> 8) & 0xFFU; + pduUserData[3U] = (llId >> 0) & 0xFFU; if (regType == PDURegType::ACCPT) { - rspData[8U] = (ipAddr >> 24) & 0xFFU; // IP Address - rspData[9U] = (ipAddr >> 16) & 0xFFU; - rspData[10U] = (ipAddr >> 8) & 0xFFU; - rspData[11U] = (ipAddr >> 0) & 0xFFU; + pduUserData[8U] = (ipAddr >> 24) & 0xFFU; // IP Address + pduUserData[9U] = (ipAddr >> 16) & 0xFFU; + pduUserData[10U] = (ipAddr >> 8) & 0xFFU; + pduUserData[11U] = (ipAddr >> 0) & 0xFFU; } - edac::CRC::addCRC32(rspData, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); - - // Generate the PDU data - DataBlock rspBlock = DataBlock(); - rspBlock.setFormat(PDUFormatType::CONFIRMED); - rspBlock.setSerialNo(0U); - rspBlock.setData(rspData); - - ::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES); - rspBlock.encode(block); - Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS); - - writeRF_PDU(data, bitLength); + rspHeader.calculateLength(12U); + writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData); } /* Helper to write a PDU acknowledge response. */ diff --git a/src/host/p25/packet/Data.h b/src/host/p25/packet/Data.h index 7e937afb..a280aa9e 100644 --- a/src/host/p25/packet/Data.h +++ b/src/host/p25/packet/Data.h @@ -86,8 +86,9 @@ namespace p25 * @param secondHeader Instance of a PDU data header. * @param useSecondHeader Flag indicating whether or not to use a second data header. * @param pduUserData Buffer containing user data to transmit. + * @param imm Flag indicating the PDU should be written to the immediate queue. */ - void writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& secondHeader, bool useSecondHeader, uint8_t* pduUserData); + void writeRF_PDU_User(data::DataHeader& dataHeader, data::DataHeader& secondHeader, bool useSecondHeader, uint8_t* pduUserData, bool imm = false); /** * @brief Updates the processor by the passed number of milliseconds. @@ -192,8 +193,9 @@ namespace p25 * @param[in] pdu Constructed PDU to transmit. * @param bitlength Length of PDU in bits. * @param noNulls Flag indicating no trailing nulls should be transmitted. + * @param imm Flag indicating the PDU should be written to the immediate queue. */ - void writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls = false); + void writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls = false, bool imm = false); /** * @brief Helper to write a network P25 PDU packet. * This will take buffered network PDU data and repeat it over the air. @@ -211,7 +213,7 @@ namespace p25 * @param llId Logical Link ID. * @param ipAddr */ - void writeRF_PDU_Reg_Response(uint8_t regType, uint8_t mfId, uint32_t llId, ulong64_t ipAddr); + void writeRF_PDU_Reg_Response(uint8_t regType, uint8_t mfId, uint32_t llId, uint32_t ipAddr); /** * @brief Helper to write a PDU acknowledge response. * @param ackClass Acknowledgement Class.