From 30467a749ccc0e82fb2044bedf176f7579a428ac Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <naylorjs@yahoo.com>
Date: Tue, 13 Sep 2016 06:24:06 +0100
Subject: [PATCH] Fix wrong positions of the status bits.

---
 Modem.cpp      | 3 ++-
 P25Control.cpp | 9 ++++-----
 P25Defines.h   | 4 ++++
 P25NID.cpp     | 4 ++--
 4 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/Modem.cpp b/Modem.cpp
index 5589b72..9453073 100644
--- a/Modem.cpp
+++ b/Modem.cpp
@@ -308,7 +308,7 @@ void CModem::clock(unsigned int ms)
 		readStatus();
 		m_statusTimer.start();
 
-
+#ifdef notdef
 		const unsigned char* dat = P25_DATA[m_nn];
 		if (m_nn == 0U) {
 			unsigned char data = 101U;
@@ -329,6 +329,7 @@ void CModem::clock(unsigned int ms)
 		}
 
 		m_nn++;
+#endif
 	}
 
 	m_inactivityTimer.clock(ms);
diff --git a/P25Control.cpp b/P25Control.cpp
index e524d93..32a511a 100644
--- a/P25Control.cpp
+++ b/P25Control.cpp
@@ -281,10 +281,9 @@ void CP25Control::addBusyBits(unsigned char* data, unsigned int length, bool b1,
 {
 	assert(data != NULL);
 
-	for (unsigned int i = 0U; i < length; i++) {
-		if (i > 0U && (i % 70U) == 0U)
-			WRITE_BIT(data, i, b1);
-		if (i > 0U && (i % 71U) == 0U)
-			WRITE_BIT(data, i, b2);
+	for (unsigned int ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += P25_SS_INCREMENT) {
+		unsigned int ss1Pos = ss0Pos + 1U;
+		WRITE_BIT(data, ss0Pos, b1);
+		WRITE_BIT(data, ss1Pos, b2);
 	}
 }
diff --git a/P25Defines.h b/P25Defines.h
index d8ac5f0..d6fcbf4 100644
--- a/P25Defines.h
+++ b/P25Defines.h
@@ -37,6 +37,10 @@ const unsigned int P25_NID_LENGTH_BYTES  = 8U;
 const unsigned char P25_SYNC_BYTES[]      = {0x55U, 0x75U, 0xF5U, 0xFFU, 0x77U, 0xFFU};
 const unsigned char P25_SYNC_BYTES_LENGTH = 6U;
 
+const unsigned int  P25_SS0_START    = 70U;
+const unsigned int  P25_SS1_START    = 71U;
+const unsigned int  P25_SS_INCREMENT = 72U;
+
 const unsigned char P25_DUID_HEADER  = 0x00U;
 const unsigned char P25_DUID_TERM    = 0x03U;
 const unsigned char P25_DUID_LDU1    = 0x05U;
diff --git a/P25NID.cpp b/P25NID.cpp
index 5e15c74..784e0c2 100644
--- a/P25NID.cpp
+++ b/P25NID.cpp
@@ -45,7 +45,7 @@ void CP25NID::process(unsigned char* data)
 
 	unsigned int n = 0U;
 	for (unsigned int offset = 48U; offset < 114U; offset++) {
-		if (offset != 70U && offset != 71U) {
+		if (offset != P25_SS0_START && offset != P25_SS1_START) {
 			bool b = READ_BIT(data, offset);
 			WRITE_BIT(nid, n, b);
 			n++;
@@ -61,7 +61,7 @@ void CP25NID::process(unsigned char* data)
 
 	n = 0U;
 	for (unsigned int offset = 48U; offset < 114U; offset++) {
-		if (offset != 70U && offset != 71U) {
+		if (offset != P25_SS0_START && offset != P25_SS1_START) {
 			bool b = READ_BIT(nid, n);
 			WRITE_BIT(data, offset, b);
 			n++;