Handle the P25 low speed data.

P25Control.cpp

@@ -17,7 +17,6 @@
 */
 
 #include "P25Control.h"
-#include "P25LowSpeedData.h"
 #include "P25Defines.h"
 #include "Sync.h"
 #include "Log.h"
@@ -58,7 +57,9 @@ m_nid(nac),
 m_lastDUID(P25_DUID_TERM),
 m_audio(),
 m_rfData(),
-m_netData()
+m_netData(),
+m_lsd(),
+m_fp(NULL)
 {
 	assert(display != NULL);
 	assert(lookup != NULL);
@@ -173,7 +174,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
 		m_rfData.processLDU1(data + 2U);
 
 		// Regenerate the Low Speed Data
-		CP25LowSpeedData::process(data + 2U);
+		m_lsd.process(data + 2U);
 
 		// Regenerate Audio
 		unsigned int errors = m_audio.process(data + 2U);
@@ -222,7 +223,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
 		m_rfData.processLDU2(data + 2U);
 
 		// Regenerate the Low Speed Data
-		CP25LowSpeedData::process(data + 2U);
+		m_lsd.process(data + 2U);
 
 		// Regenerate Audio
 		unsigned int errors = m_audio.process(data + 2U);

P25Control.h

@@ -19,6 +19,7 @@
 #if !defined(P25Control_H)
 #define	P25Control_H
 
+#include "P25LowSpeedData.h"
 #include "RingBuffer.h"
 #include "P25Network.h"
 #include "DMRLookup.h"
@@ -69,6 +70,7 @@ class CP25Control {
 	CP25Audio     m_audio;
 	CP25Data      m_rfData;
 	CP25Data      m_netData;
+	CP25LowSpeedData m_lsd;
 	FILE*         m_fp;
 
 	void writeQueueRF(const unsigned char* data, unsigned int length);

P25LowSpeedData.cpp

@@ -18,16 +18,81 @@
 
 #include "P25LowSpeedData.h"
 #include "P25Utils.h"
-#include "Utils.h"
-#include "Log.h"
 
 #include <cstdio>
 #include <cassert>
 
+const unsigned int MAX_CCS_ERRS = 4U;
+
+CP25LowSpeedData::CP25LowSpeedData()
+{
+}
+
+CP25LowSpeedData::~CP25LowSpeedData()
+{
+}
+
 void CP25LowSpeedData::process(unsigned char* data)
 {
+	assert(data != NULL);
+
 	unsigned char lsd[4U];
 	CP25Utils::decode(data, lsd, 1546U, 1578U);
 
-	CUtils::dump(1U, "P25, Low Speed Data", lsd, 4U);
+	for (unsigned int a = 0x00U; a < 0x100U; a++) {
+		unsigned char ccs[2U];
+		ccs[0U] = a;
+		ccs[1U] = encode(ccs[0U]);
+
+		unsigned int errs = CP25Utils::compare(ccs, lsd + 0U, 2U);
+		if (errs < MAX_CCS_ERRS) {
+			lsd[0U] = ccs[0U];
+			lsd[1U] = ccs[1U];
+			break;
+		}
+	}
+
+	for (unsigned int a = 0x00U; a < 0x100U; a++) {
+		unsigned char ccs[2U];
+		ccs[0U] = a;
+		ccs[1U] = encode(ccs[0U]);
+
+		unsigned int errs = CP25Utils::compare(ccs, lsd + 2U, 2U);
+		if (errs < MAX_CCS_ERRS) {
+			lsd[2U] = ccs[0U];
+			lsd[3U] = ccs[1U];
+			break;
+		}
+	}
+}
+
+unsigned char CP25LowSpeedData::encode(unsigned char in)
+{
+	unsigned char result = 0x00U;
+
+	if ((in & 0x01U) == 0x01U)
+		result ^= 0x39U;
+
+	if ((in & 0x02U) == 0x02U)
+		result ^= 0x72U;
+
+	if ((in & 0x04U) == 0x04U)
+		result ^= 0xE4U;
+
+	if ((in & 0x08U) == 0x08U)
+		result ^= 0xF1U;
+
+	if ((in & 0x10U) == 0x10U)
+		result ^= 0xDBU;
+
+	if ((in & 0x20U) == 0x20U)
+		result ^= 0x8FU;
+
+	if ((in & 0x40U) == 0x40U)
+		result ^= 0x27U;
+
+	if ((in & 0x80U) == 0x80U)
+		result ^= 0x4EU;
+
+	return result;
 }

P25LowSpeedData.h

@@ -21,7 +21,12 @@
 
 class CP25LowSpeedData {
 public:
-	static void process(unsigned char* data);
+	CP25LowSpeedData();
+	~CP25LowSpeedData();
+
+	void process(unsigned char* data);
+
+	unsigned char encode(const unsigned char in);
 
 private:
 };

P25NID.cpp

@@ -88,31 +88,31 @@ bool CP25NID::decode(const unsigned char* data)
 	unsigned char nid[P25_NID_LENGTH_BYTES];
 	CP25Utils::decode(data, nid, 48U, 114U);
 
-	unsigned int errs = compare(nid, m_ldu1);
+	unsigned int errs = CP25Utils::compare(nid, m_ldu1, P25_NID_LENGTH_BYTES);
 	if (errs < MAX_NID_ERRS) {
 		m_duid = P25_DUID_LDU1;
 		return true;
 	}
 
-	errs = compare(nid, m_ldu2);
+	errs = CP25Utils::compare(nid, m_ldu2, P25_NID_LENGTH_BYTES);
 	if (errs < MAX_NID_ERRS) {
 		m_duid = P25_DUID_LDU2;
 		return true;
 	}
 
-	errs = compare(nid, m_term);
+	errs = CP25Utils::compare(nid, m_term, P25_NID_LENGTH_BYTES);
 	if (errs < MAX_NID_ERRS) {
 		m_duid = P25_DUID_TERM;
 		return true;
 	}
 
-	errs = compare(nid, m_termlc);
+	errs = CP25Utils::compare(nid, m_termlc, P25_NID_LENGTH_BYTES);
 	if (errs < MAX_NID_ERRS) {
 		m_duid = P25_DUID_TERM_LC;
 		return true;
 	}
 
-	errs = compare(nid, m_hdr);
+	errs = CP25Utils::compare(nid, m_hdr, P25_NID_LENGTH_BYTES);
 	if (errs < MAX_NID_ERRS) {
 		m_duid = P25_DUID_HEADER;
 		return true;
@@ -150,20 +150,3 @@ unsigned char CP25NID::getDUID() const
 {
 	return m_duid;
 }
-
-unsigned int CP25NID::compare(const unsigned char* nid1, const unsigned char* nid2) const
-{
-	assert(nid1 != NULL);
-	assert(nid2 != NULL);
-
-	unsigned int errs = 0U;
-	for (unsigned int i = 0U; i < P25_NID_LENGTH_BYTES; i++) {
-		unsigned char v = nid1[i] ^ nid2[i];
-		while (v != 0U) {
-			v &= v - 1U;
-			errs++;
-		}
-	}
-
-	return errs;
-}

P25NID.h

@@ -37,8 +37,6 @@ class CP25NID {
 	unsigned char* m_ldu2;
 	unsigned char* m_termlc;
 	unsigned char* m_term;
-
-	unsigned int compare(const unsigned char* nid1, const unsigned char* nid2) const;
 };
 
 #endif

P25Utils.cpp

@@ -82,3 +82,20 @@ void CP25Utils::encode(const unsigned char* in, unsigned char* out, unsigned int
 		}
 	}
 }
+
+unsigned int CP25Utils::compare(const unsigned char* data1, const unsigned char* data2, unsigned int length)
+{
+	assert(data1 != NULL);
+	assert(data2 != NULL);
+
+	unsigned int errs = 0U;
+	for (unsigned int i = 0U; i < length; i++) {
+		unsigned char v = data1[i] ^ data2[i];
+		while (v != 0U) {
+			v &= v - 1U;
+			errs++;
+		}
+	}
+
+	return errs;
+}

P25Utils.h

@@ -25,6 +25,8 @@ class CP25Utils {
 
 	static void decode(const unsigned char* in, unsigned char* out, unsigned int start, unsigned int stop);
 
+	static unsigned int compare(const unsigned char* data1, const unsigned char* data2, unsigned int length);
+
 private:
 };