From 38756799e1f0aedd2478613c9ddf0a76d0895574 Mon Sep 17 00:00:00 2001
From: Bryan Biedenkapp <gatekeep@gmail.com>
Date: Fri, 29 Apr 2022 15:19:39 -0400
Subject: [PATCH] fix issue where selected channel number with a negative tx
 offset may result in a Rx frequency that may be below the base frequency (all
 frequencies must be channelized properly and be must be greater then the base
 frequency); fix SN-DATA_CHN_ANN_EXP Rx channel number calculation;

---
 host/Host.cpp   | 18 +++++++++++++++---
 p25/lc/TSBK.cpp | 11 ++++++++++-
 2 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/host/Host.cpp b/host/Host.cpp
index 535092cb..bf81dc69 100644
--- a/host/Host.cpp
+++ b/host/Host.cpp
@@ -1412,17 +1412,29 @@ bool Host::readParams()
         uint32_t calcSpace = (uint32_t)(entry.chSpaceKhz() / 0.125);
         float calcTxOffset = entry.txOffsetMhz() * 1000000;
 
-        m_rxFrequency = (uint32_t)((entry.baseFrequency() + ((calcSpace * 125) * m_channelNo)) + calcTxOffset);
         m_txFrequency = (uint32_t)((entry.baseFrequency() + ((calcSpace * 125) * m_channelNo)));
+        m_rxFrequency = (uint32_t)(m_txFrequency + calcTxOffset);
+
+        if (calcTxOffset < 0.0f && m_rxFrequency < entry.baseFrequency()) {
+            ::LogError(LOG_HOST, "Channel Id %u Channel No $%04X has an invalid frequency. Rx Frequency (%u) is less then the base frequency (%u), all frequencies must be higher then the base frequency.", m_channelId, m_channelNo,
+                m_rxFrequency, entry.baseFrequency());
+            return false;
+        }
 
         if (!m_duplex && simplexSameFreq) {
-            m_rxFrequency = (uint32_t)((entry.baseFrequency() + ((calcSpace * 125) * m_channelNo)));
-            m_txFrequency = m_rxFrequency;
+            m_rxFrequency = m_txFrequency;
         }
 
         yaml::Node& voiceChList = rfssConfig["voiceChNo"];
         for (size_t i = 0; i < voiceChList.size(); i++) {
             uint32_t chNo = (uint32_t)::strtoul(voiceChList[i].as<std::string>("1").c_str(), NULL, 16);
+            if (chNo == 0U) { // clamp to 1
+                chNo = 1U;
+            }
+            if (chNo > 4095U) { // clamp to 4095
+                chNo = 4095U;
+            }
+
             m_voiceChNo.push_back(chNo);
         }
 
diff --git a/p25/lc/TSBK.cpp b/p25/lc/TSBK.cpp
index 73153ce1..599687d4 100644
--- a/p25/lc/TSBK.cpp
+++ b/p25/lc/TSBK.cpp
@@ -635,6 +635,15 @@ void TSBK::encode(uint8_t* data, bool rawTSBK, bool noTrellis)
         break;
     case TSBK_OSP_SNDCP_CH_ANN:
     {
+        uint32_t calcSpace = (uint32_t)(m_siteIdenEntry.chSpaceKhz() / 0.125);
+        float calcTxOffset = m_siteIdenEntry.txOffsetMhz() * 1000000;
+
+        uint32_t txFrequency = (uint32_t)((m_siteIdenEntry.baseFrequency() + ((calcSpace * 125) * m_siteData.channelNo())));
+        uint32_t rxFrequency = (uint32_t)(txFrequency + calcTxOffset);
+
+        uint32_t rootFreq = rxFrequency - m_siteIdenEntry.baseFrequency();
+        uint32_t rxChNo = rootFreq / (m_siteIdenEntry.chSpaceKhz() * 1000);
+
         tsbkValue = 0U;                                                             // 
         tsbkValue = (m_emergency ? 0x80U : 0x00U) +                                 // Emergency Flag
             (m_encrypted ? 0x40U : 0x00U);                                          // Encrypted Flag
@@ -644,7 +653,7 @@ void TSBK::encode(uint8_t* data, bool rawTSBK, bool noTrellis)
         tsbkValue = (tsbkValue << 4) + m_siteData.channelId();                      // Channel (T) ID
         tsbkValue = (tsbkValue << 12) + m_siteData.channelNo();                     // Channel (T) Number
         tsbkValue = (tsbkValue << 4) + m_siteData.channelId();                      // Channel (R) ID
-        tsbkValue = (tsbkValue << 12) + m_siteData.channelNo();                     // Channel (R) Number
+        tsbkValue = (tsbkValue << 12) + (rxChNo & 0xFFFU);                          // Channel (R) Number
         tsbkValue = (tsbkValue << 16) + m_sndcpDAC;                                 // Data Access Control
     }
     break;