From e71e946990fae4f6c4915514e0476541166e386a Mon Sep 17 00:00:00 2001
From: Bryan Biedenkapp <gatekeep@jmp.cx>
Date: Fri, 12 Feb 2021 04:21:50 +0000
Subject: [PATCH] support location registration;

---
 p25/TrunkPacket.cpp | 54 ++++++++++++++++++++++++++++++++++++++++++++-
 p25/TrunkPacket.h   |  2 ++
 p25/lc/TSBK.cpp     |  3 ++-
 p25/lc/TSBK.h       |  4 ++++
 4 files changed, 61 insertions(+), 2 deletions(-)

diff --git a/p25/TrunkPacket.cpp b/p25/TrunkPacket.cpp
index bd8a1178..e34a61c3 100644
--- a/p25/TrunkPacket.cpp
+++ b/p25/TrunkPacket.cpp
@@ -606,7 +606,7 @@ bool TrunkPacket::process(uint8_t* data, uint32_t len)
                     LogMessage(LOG_RF, P25_TSDU_STR ", TSBK_ISP_LOC_REG_REQ (Location Registration Request), srcId = %u, dstId = %u", srcId, dstId);
                 }
 
-                writeRF_TSDU_U_Reg_Cmd(srcId);
+                writeRF_TSDU_Loc_Reg_Rsp(srcId, dstId);
                 break;
             default:
                 LogError(LOG_RF, P25_TSDU_STR ", unhandled LCO, mfId = $%02X, lco = $%02X", m_rfTSBK.getMFId(), m_rfTSBK.getLCO());
@@ -2289,6 +2289,58 @@ void TrunkPacket::writeRF_TSDU_Queue(uint8_t reason, uint8_t service)
     m_rfTSBK.setLCO(lco);
 }
 
+/// <summary>
+/// Helper to write a location registration response packet.
+/// </summary>
+/// <param name="srcId"></param>
+/// <param name="dstId"></param>
+bool TrunkPacket::writeRF_TSDU_Loc_Reg_Rsp(uint32_t srcId, uint32_t dstId)
+{
+    bool ret = false;
+
+    m_rfTSBK.setLCO(TSBK_OSP_LOC_REG_RSP);
+    m_rfTSBK.setResponse(P25_RSP_ACCEPT);
+    m_rfTSBK.setDstId(dstId);
+    m_rfTSBK.setSrcId(srcId);
+
+    // validate the source RID
+    if (!acl::AccessControl::validateSrcId(srcId)) {
+        LogWarning(LOG_RF, P25_TSDU_STR ", TSBK_OSP_LOC_REG_RSP (Location Registration Response) denial, RID rejection, srcId = %u", srcId);
+        ::ActivityLog("P25", true, "location registration request from %u denied", srcId);
+        m_rfTSBK.setResponse(P25_RSP_REFUSED);
+    }
+
+    // validate the source RID is registered
+    if (!hasSrcIdUnitReg(srcId)) {
+        LogWarning(LOG_RF, P25_TSDU_STR ", TSBK_OSP_LOC_REG_RSP (Location Registration Response) denial, RID not registered, srcId = %u", srcId);
+        ::ActivityLog("P25", true, "location registration request from %u denied", srcId);
+        writeRF_TSDU_U_Reg_Cmd(srcId);
+        return false;
+    }
+
+    // validate the talkgroup ID
+    if (m_rfTSBK.getGroup()) {
+        if (!acl::AccessControl::validateTGId(dstId)) {
+            LogWarning(LOG_RF, P25_TSDU_STR ", TSBK_OSP_LOC_REG_RSP (Location Registration Response) denial, TGID rejection, dstId = %u", dstId);
+            ::ActivityLog("P25", true, "location registration request from %u to %s %u denied", srcId, "TG ", dstId);
+            m_rfTSBK.setResponse(P25_RSP_DENY);
+        }
+    }
+
+    if (m_rfTSBK.getResponse() == P25_RSP_ACCEPT) {
+        if (m_verbose) {
+            LogMessage(LOG_RF, P25_TSDU_STR ", TSBK_OSP_LOC_REG_RSP (Location Registration Response), lra = %u, srcId = %u, dstId = %u",
+                m_rfTSBK.getLRA(), srcId, dstId);
+        }
+
+        ::ActivityLog("P25", true, "location registration request from %u", srcId);
+        ret = true;
+    }
+
+    writeRF_TSDU_SBF(false);
+    return ret;
+}
+
 /// <summary>
 /// Helper to write a network TSDU from the RF data queue.
 /// </summary>
diff --git a/p25/TrunkPacket.h b/p25/TrunkPacket.h
index 5a325b69..0345281d 100644
--- a/p25/TrunkPacket.h
+++ b/p25/TrunkPacket.h
@@ -229,6 +229,8 @@ namespace p25
         void writeRF_TSDU_U_Dereg_Ack(uint32_t srcId);
         /// <summary>Helper to write a queue packet.</summary>
         void writeRF_TSDU_Queue(uint8_t reason, uint8_t service);
+        /// <summary>Helper to write a location registration response packet.</summary>
+        bool writeRF_TSDU_Loc_Reg_Rsp(uint32_t srcId, uint32_t dstId);
 
         /// <summary>Helper to write a network TSDU from the RF data queue.</summary>
         void writeNet_TSDU_From_RF(uint8_t* data);
diff --git a/p25/lc/TSBK.cpp b/p25/lc/TSBK.cpp
index d555b4cb..12ae981a 100644
--- a/p25/lc/TSBK.cpp
+++ b/p25/lc/TSBK.cpp
@@ -237,7 +237,7 @@ bool TSBK::decode(const uint8_t* data)
         m_srcId = (uint32_t)(tsbkValue & 0xFFFFFFU);                                // Target Radio Address
         break;
     case TSBK_IOSP_GRP_AFF:
-        m_sysId = (uint32_t)((tsbkValue >> 16) & 0xFFFU);                           // System ID
+        m_sysId = (uint32_t)((tsbkValue >> 40) & 0xFFFU);                           // System ID
         m_dstId = (uint32_t)((tsbkValue >> 24) & 0xFFFFU);                          // Talkgroup Address
         m_srcId = (uint32_t)(tsbkValue & 0xFFFFFFU);                                // Source Radio Address
         break;
@@ -273,6 +273,7 @@ bool TSBK::decode(const uint8_t* data)
         m_srcId = (uint32_t)(tsbkValue & 0xFFFFFFU);                                // Source Radio Address
         break;
     case TSBK_ISP_LOC_REG_REQ:
+        m_lra = (uint8_t)((tsbkValue >> 40) & 0xFFU);                               // LRA
         m_dstId = (uint32_t)((tsbkValue >> 24) & 0xFFFFU);                          // Talkgroup Address
         m_srcId = (uint32_t)(tsbkValue & 0xFFFFFFU);                                // Source Radio Address
         break;
diff --git a/p25/lc/TSBK.h b/p25/lc/TSBK.h
index c609bb7f..73dd34e7 100644
--- a/p25/lc/TSBK.h
+++ b/p25/lc/TSBK.h
@@ -154,6 +154,10 @@ namespace p25
             /// <summary>Explicit SCCB channel number.</summary>
             __PROPERTY(uint32_t, sccbChannelNo, SCCBChnNo);
 
+            /** Location Data */
+            /// <summary>Location registration area.</summary>
+            __PROPERTY(uint8_t, lra, LRA);
+
             /** Patch Group data */
             /// <summary>Patch super group ID.</summary>
             __PROPERTY(uint32_t, patchSuperGroupId, PatchSuperGroupId);