From cb07423092e4f382c07155f9f93360213007b329 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <naylorjs@yahoo.com>
Date: Tue, 4 Oct 2016 18:37:08 +0100
Subject: [PATCH] Handle missing P25 network frames better.

---
 MMDVM.ini      |  2 --
 P25Control.cpp | 53 ++++++++++++++++++++++++++++++++++++++++----------
 P25Control.h   |  3 +++
 3 files changed, 46 insertions(+), 12 deletions(-)

diff --git a/MMDVM.ini b/MMDVM.ini
index c92d839..b31416a 100644
--- a/MMDVM.ini
+++ b/MMDVM.ini
@@ -77,8 +77,6 @@ TGRewriteSlot1=0
 TGRewriteSlot2=0
 BMAutoRewrite=0
 BMRewriteReflectorVoicePrompts=0
-
-
 DirectDial=0
 TargetTG=9
 #RewriteMapSlot1=
diff --git a/P25Control.cpp b/P25Control.cpp
index 35fe37b..c82d313 100644
--- a/P25Control.cpp
+++ b/P25Control.cpp
@@ -313,61 +313,73 @@ void CP25Control::writeNetwork()
 	switch (data[0U]) {
 	case 0x62U:
 		::memcpy(m_netLDU1 + 0U, data, 22U);
+		checkNetLDU2();
 		break;
 	case 0x63U:
 		::memcpy(m_netLDU1 + 25U, data, 14U);
+		checkNetLDU2();
 		break;
 	case 0x64U:
 		::memcpy(m_netLDU1 + 50U, data, 17U);
+		checkNetLDU2();
 		break;
 	case 0x65U:
 		::memcpy(m_netLDU1 + 75U, data, 17U);
+		checkNetLDU2();
 		break;
 	case 0x66U:
 		::memcpy(m_netLDU1 + 100U, data, 17U);
+		checkNetLDU2();
 		break;
 	case 0x67U:
 		::memcpy(m_netLDU1 + 125U, data, 17U);
+		checkNetLDU2();
 		break;
 	case 0x68U:
 		::memcpy(m_netLDU1 + 150U, data, 17U);
+		checkNetLDU2();
 		break;
 	case 0x69U:
 		::memcpy(m_netLDU1 + 175U, data, 17U);
+		checkNetLDU2();
 		break;
 	case 0x6AU:
 		::memcpy(m_netLDU1 + 200U, data, 16U);
-		if (m_netState != RS_NET_IDLE) {
-			// Check for an unflushed LDU2
-			void* p = ::memchr(m_netLDU2, 0x02U, 9U * 25U);
-			if (p != NULL)
-				createNetLDU2();
+		checkNetLDU2();
+		if (m_netState != RS_NET_IDLE)
 			createNetLDU1();
-		}
 		break;
 	case 0x6BU:
 		::memcpy(m_netLDU2 + 0U, data, 22U);
+		checkNetLDU1();
 		break;
 	case 0x6CU:
 		::memcpy(m_netLDU2 + 25U, data, 14U);
+		checkNetLDU1();
 		break;
 	case 0x6DU:
 		::memcpy(m_netLDU2 + 50U, data, 17U);
+		checkNetLDU1();
 		break;
 	case 0x6EU:
 		::memcpy(m_netLDU2 + 75U, data, 17U);
+		checkNetLDU1();
 		break;
 	case 0x6FU:
 		::memcpy(m_netLDU2 + 100U, data, 17U);
+		checkNetLDU1();
 		break;
 	case 0x70U:
 		::memcpy(m_netLDU2 + 125U, data, 17U);
+		checkNetLDU1();
 		break;
 	case 0x71U:
 		::memcpy(m_netLDU2 + 150U, data, 17U);
+		checkNetLDU1();
 		break;
 	case 0x72U:
 		::memcpy(m_netLDU2 + 175U, data, 17U);
+		checkNetLDU1();
 		break;
 	case 0x73U:
 		::memcpy(m_netLDU2 + 200U, data, 16U);
@@ -375,10 +387,7 @@ void CP25Control::writeNetwork()
 			createNetHeader();
 			createNetLDU1();
 		} else {
-			// Check for an unflushed LDU1
-			void* p = ::memchr(m_netLDU1, 0x02U, 9U * 25U);
-			if (p != NULL)
-				createNetLDU1();
+			checkNetLDU1();
 		}
 		createNetLDU2();
 		break;
@@ -484,6 +493,30 @@ void CP25Control::addBusyBits(unsigned char* data, unsigned int length, bool b1,
 	}
 }
 
+void CP25Control::checkNetLDU1()
+{
+	if (m_netState == RS_NET_IDLE)
+		return;
+
+	// Check for an unflushed LDU1
+	if (m_netLDU1[0U]   != 0x00U || m_netLDU1[25U]  != 0x00U || m_netLDU1[50U]  != 0x00U ||
+		m_netLDU1[75U]  != 0x00U || m_netLDU1[100U] != 0x00U || m_netLDU1[125U] != 0x00U ||
+		m_netLDU1[150U] != 0x00U || m_netLDU1[175U] != 0x00U || m_netLDU1[200U] != 0x00U)
+		createNetLDU1();
+}
+
+void CP25Control::checkNetLDU2()
+{
+	if (m_netState == RS_NET_IDLE)
+		return;
+
+	// Check for an unflushed LDU1
+	if (m_netLDU2[0U]   != 0x00U || m_netLDU2[25U]  != 0x00U || m_netLDU2[50U]  != 0x00U ||
+		m_netLDU2[75U]  != 0x00U || m_netLDU2[100U] != 0x00U || m_netLDU2[125U] != 0x00U ||
+		m_netLDU2[150U] != 0x00U || m_netLDU2[175U] != 0x00U || m_netLDU2[200U] != 0x00U)
+		createNetLDU2();
+}
+
 void CP25Control::insertMissingAudio(unsigned char* data)
 {
 	if (data[0U] == 0x00U) {
diff --git a/P25Control.h b/P25Control.h
index 66a2f54..edd726a 100644
--- a/P25Control.h
+++ b/P25Control.h
@@ -81,6 +81,9 @@ private:
 
 	void addBusyBits(unsigned char* data, unsigned int length, bool b1, bool b2);
 
+	void checkNetLDU1();
+	void checkNetLDU2();
+
 	void insertMissingAudio(unsigned char* data);
 
 	void createRFHeader();