From d672f8827f49d730cc6595b0d036c650abe19f09 Mon Sep 17 00:00:00 2001
From: Bryan Biedenkapp <gatekeep@jmp.cx>
Date: Thu, 22 Apr 2021 17:38:27 +0000
Subject: [PATCH] correct P25 response PDU generation;

---
 p25/DataPacket.cpp     | 28 +++++++++++++++-------------
 p25/data/DataBlock.cpp | 12 ++++++------
 2 files changed, 21 insertions(+), 19 deletions(-)

diff --git a/p25/DataPacket.cpp b/p25/DataPacket.cpp
index d3e842bb..9d6036e4 100644
--- a/p25/DataPacket.cpp
+++ b/p25/DataPacket.cpp
@@ -150,12 +150,6 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
                 return false;
             }
 
-            /*
-            if (m_rfDataHeader.getAckNeeded()) {
-                writeRF_PDU_Ack_Response(PDU_ACK_CLASS_ACK, PDU_ACK_TYPE_ACK, m_rfDataHeader.getLLId());
-            }
-            */
-
             writeNetworkRF(P25_DT_DATA_HEADER, buffer, P25_PDU_FEC_LENGTH_BYTES);
         }
 
@@ -274,12 +268,13 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
                             if (!hasLLIdFNEReg(llId)) {
                                 // update dynamic FNE registration table entry
                                 m_fneRegTable[llId] = ipAddr;
+                            }
 
-                                if (m_verbose) {
-                                    LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_RSP_ACCPT (Registration Response Accept), llId = %u, ipAddr = %u", llId, ipAddr);
-                                }
-                                writeRF_PDU_Reg_Response(PDU_REG_TYPE_RSP_ACCPT, llId, ipAddr);
+                            if (m_verbose) {
+                                LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_RSP_ACCPT (Registration Response Accept), llId = %u, ipAddr = %u", llId, ipAddr);
                             }
+
+                            writeRF_PDU_Reg_Response(PDU_REG_TYPE_RSP_ACCPT, llId, ipAddr);
                         }
                     }
                     break;
@@ -697,7 +692,13 @@ void DataPacket::writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong6
     rspHeader.setClass(PDU_ACK_CLASS_ACK);
     rspHeader.setType(PDU_ACK_TYPE_ACK);
     rspHeader.setLLId(llId);
-    rspHeader.setSrcLLId(P25_WUID_FNE);
+    if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) {
+        rspHeader.setSrcLLId(P25_WUID_FNE);
+        rspHeader.setExtended(true);
+    }
+    else {
+        rspHeader.setExtended(false);
+    }
     rspHeader.setBlocksToFollow(1U);
 
     // Generate the PDU header and 1/2 rate Trellis
@@ -706,7 +707,7 @@ void DataPacket::writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong6
     offset += P25_PDU_FEC_LENGTH_BITS;
 
     // build registration response data
-    ::memset(buffer, 0x00U, P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
+    ::memset(buffer, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
 
     buffer[0U] = (regType << 4);                                                    // Registration Type & Options
     buffer[1U] = (llId >> 16) & 0xFFU;                                              // Logical Link ID
@@ -719,10 +720,11 @@ void DataPacket::writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong6
         buffer[11U] = (ipAddr >> 0) & 0xFFU;
     }
 
-    edac::CRC::addCRC32(buffer, P25_PDU_CONFIRMED_LENGTH_BYTES);
+    edac::CRC::addCRC32(buffer, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
 
     // Generate the PDU data
     m_rfData[0].setFormat(PDU_FMT_RSP);
+    m_rfData[0].setConfirmed(true);
     m_rfData[0].setSerialNo(0U);
     m_rfData[0].setData(buffer);
 
diff --git a/p25/data/DataBlock.cpp b/p25/data/DataBlock.cpp
index 549e58c2..1713ff5f 100644
--- a/p25/data/DataBlock.cpp
+++ b/p25/data/DataBlock.cpp
@@ -159,7 +159,7 @@ void DataBlock::encode(uint8_t* data)
     assert(data != NULL);
     assert(m_data != NULL);
 
-    if (m_fmt == PDU_FMT_CONFIRMED) {
+    if ((m_fmt == PDU_FMT_CONFIRMED) || (m_fmt == PDU_FMT_RSP && m_confirmed)) {
         uint8_t buffer[P25_PDU_CONFIRMED_LENGTH_BYTES];
         ::memset(buffer, 0x00U, P25_PDU_CONFIRMED_LENGTH_BYTES);
 
@@ -187,7 +187,7 @@ void DataBlock::encode(uint8_t* data)
 
         m_trellis.encode34(buffer, data);
     }
-    else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP)) {
+    else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP && !m_confirmed)) {
         uint8_t buffer[P25_PDU_UNCONFIRMED_LENGTH_BYTES];
         ::memset(buffer, 0x00U, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
 
@@ -237,10 +237,10 @@ void DataBlock::setData(const uint8_t* buffer)
     assert(buffer != NULL);
     assert(m_data != NULL);
 
-    if (m_fmt == PDU_FMT_CONFIRMED) {
+    if ((m_fmt == PDU_FMT_CONFIRMED) || (m_fmt == PDU_FMT_RSP && m_confirmed)) {
         ::memcpy(m_data, buffer, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
     }
-    else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP)) {
+    else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP && !m_confirmed)) {
         ::memcpy(m_data, buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
     }
     else {
@@ -255,11 +255,11 @@ uint32_t DataBlock::getData(uint8_t* buffer) const
     assert(buffer != NULL);
     assert(m_data != NULL);
 
-    if (m_fmt == PDU_FMT_CONFIRMED) {
+    if ((m_fmt == PDU_FMT_CONFIRMED) || (m_fmt == PDU_FMT_RSP && m_confirmed)) {
         ::memcpy(buffer, m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
         return P25_PDU_CONFIRMED_DATA_LENGTH_BYTES;
     } 
-    else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP)) {
+    else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP && !m_confirmed)) {
         ::memcpy(buffer, m_data, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
         return P25_PDU_UNCONFIRMED_LENGTH_BYTES;
     }