From 80d97e94ec3470b7c8edeac2d5e0f4e3f112c3b0 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <naylorjs@yahoo.com>
Date: Thu, 25 Jan 2018 07:38:57 +0000
Subject: [PATCH] Add FACCH2 processing.

---
 NXDNControl.cpp | 38 ++++++++++++++++++++------------------
 NXDNFACCH2.cpp  | 45 ++++++++++++++++++++++++++++++++++++++++-----
 NXDNFACCH2.h    |  1 +
 NXDNSACCH.cpp   |  4 ++--
 4 files changed, 63 insertions(+), 25 deletions(-)

diff --git a/NXDNControl.cpp b/NXDNControl.cpp
index c205083..22b55c6 100644
--- a/NXDNControl.cpp
+++ b/NXDNControl.cpp
@@ -540,32 +540,34 @@ bool CNXDNControl::processData(unsigned char option, unsigned char *data)
 			return true;
 		}
 	} else {
-		CNXDNUDCH udch;
-		bool valid = udch.decode(data + 2U);
-		if (valid) {
-			data[0U] = TAG_DATA;
-			data[1U] = 0x00U;
+		if (m_rfState == RS_RF_DATA) {
+			CNXDNUDCH udch;
+			bool valid = udch.decode(data + 2U);
+			if (valid) {
+				data[0U] = TAG_DATA;
+				data[1U] = 0x00U;
 
-			CSync::addNXDNSync(data + 2U);
+				CSync::addNXDNSync(data + 2U);
 
-			CNXDNLICH lich;
-			lich.setRFCT(NXDN_LICH_RFCT_RDCH);
-			lich.setFCT(NXDN_LICH_USC_UDCH);
-			lich.setOption(NXDN_LICH_STEAL_NONE);
-			lich.setDirection(m_remoteGateway ? NXDN_LICH_DIRECTION_INBOUND : NXDN_LICH_DIRECTION_OUTBOUND);
-			lich.encode(data + 2U);
+				CNXDNLICH lich;
+				lich.setRFCT(NXDN_LICH_RFCT_RDCH);
+				lich.setFCT(NXDN_LICH_USC_UDCH);
+				lich.setOption(NXDN_LICH_STEAL_NONE);
+				lich.setDirection(m_remoteGateway ? NXDN_LICH_DIRECTION_INBOUND : NXDN_LICH_DIRECTION_OUTBOUND);
+				lich.encode(data + 2U);
 
-			udch.encode(data + 2U);
+				udch.encode(data + 2U);
 
-			writeQueueNet(data);
+				writeQueueNet(data);
 
-			if (m_duplex)
-				writeQueueRF(data);
+				if (m_duplex)
+					writeQueueRF(data);
 
 #if defined(DUMP_NXDN)
-			writeFile(data + 2U);
+				writeFile(data + 2U);
 #endif
-			return true;
+				return true;
+			}
 		}
 	}
 
diff --git a/NXDNFACCH2.cpp b/NXDNFACCH2.cpp
index fc53d22..1c7dfb9 100644
--- a/NXDNFACCH2.cpp
+++ b/NXDNFACCH2.cpp
@@ -18,9 +18,8 @@
 
 #include "NXDNFACCH2.h"
 
-#include "NXDNConvolution.h"
 #include "NXDNDefines.h"
-#include "NXDNCRC.h"
+#include "NXDNUDCH.h"
 #include "Utils.h"
 #include "Log.h"
 
@@ -33,50 +32,86 @@ const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04
 #define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
 #define READ_BIT1(p,i)    (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
 
-CNXDNFACCH2::CNXDNFACCH2(const CNXDNFACCH2& facch2)
+CNXDNFACCH2::CNXDNFACCH2(const CNXDNFACCH2& facch2) :
+m_data(NULL)
 {
+	m_data = new unsigned char[25U];
+	::memcpy(m_data, facch2.m_data, 25U);
 }
 
-CNXDNFACCH2::CNXDNFACCH2()
+CNXDNFACCH2::CNXDNFACCH2() :
+m_data(NULL)
 {
+	m_data = new unsigned char[25U];
 }
 
 CNXDNFACCH2::~CNXDNFACCH2()
 {
+	delete[] m_data;
 }
 
 bool CNXDNFACCH2::decode(const unsigned char* data)
 {
 	assert(data != NULL);
 
+	CNXDNUDCH udch;
+	bool valid = udch.decode(data);
+	if (!valid)
+		return false;
+
+	udch.getData(m_data);
+
 	return true;
 }
 
 void CNXDNFACCH2::encode(unsigned char* data) const
 {
 	assert(data != NULL);
+
+	CNXDNUDCH udch;
+
+	udch.setData(m_data);
+
+	udch.encode(data);
 }
 
 unsigned char CNXDNFACCH2::getRAN() const
 {
-	return 0U;
+	return m_data[0U] & 0x3FU;
 }
 
 void CNXDNFACCH2::getData(unsigned char* data) const
 {
 	assert(data != NULL);
+
+	unsigned int offset = 8U;
+	for (unsigned int i = 0U; i < 168U; i++, offset++) {
+		bool b = READ_BIT1(m_data, offset);
+		WRITE_BIT1(data, i, b);
+	}
 }
 
 void CNXDNFACCH2::setRAN(unsigned char ran)
 {
+	m_data[0U] &= 0xC0U;
+	m_data[0U] |= ran;
 }
 
 void CNXDNFACCH2::setData(const unsigned char* data)
 {
 	assert(data != NULL);
+
+	unsigned int offset = 8U;
+	for (unsigned int i = 0U; i < 168U; i++, offset++) {
+		bool b = READ_BIT1(data, i);
+		WRITE_BIT1(m_data, offset, b);
+	}
 }
 
 CNXDNFACCH2& CNXDNFACCH2::operator=(const CNXDNFACCH2& facch2)
 {
+	if (&facch2 != this)
+		::memcpy(m_data, facch2.m_data, 25U);
+
 	return *this;
 }
diff --git a/NXDNFACCH2.h b/NXDNFACCH2.h
index cbe8c6a..7f8ee57 100644
--- a/NXDNFACCH2.h
+++ b/NXDNFACCH2.h
@@ -40,6 +40,7 @@ public:
 	CNXDNFACCH2& operator=(const CNXDNFACCH2& facch);
 
 private:
+	unsigned char* m_data;
 };
 
 #endif
diff --git a/NXDNSACCH.cpp b/NXDNSACCH.cpp
index 51038e0..b6b26cb 100644
--- a/NXDNSACCH.cpp
+++ b/NXDNSACCH.cpp
@@ -180,8 +180,8 @@ void CNXDNSACCH::getData(unsigned char* data) const
 
 	unsigned int offset = 8U;
 	for (unsigned int i = 0U; i < 18U; i++, offset++) {
-		bool b = READ_BIT1(data, offset);
-		WRITE_BIT1(m_data, i, b);
+		bool b = READ_BIT1(m_data, offset);
+		WRITE_BIT1(data, i, b);
 	}
 }