From d774b89d390a42fa076f4d79378d3bf00a003588 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <naylorjs@yahoo.com>
Date: Thu, 3 Aug 2017 20:49:44 +0100
Subject: [PATCH] Fix LCSS bug and improve swap usage.

---
 Rewrite.cpp | 86 +++++++++++++++++++++++++++++++++++------------------
 Rewrite.h   |  5 +++-
 2 files changed, 61 insertions(+), 30 deletions(-)

diff --git a/Rewrite.cpp b/Rewrite.cpp
index a160d5d..745d7fb 100644
--- a/Rewrite.cpp
+++ b/Rewrite.cpp
@@ -20,6 +20,7 @@
 #include "DMRFullLC.h"
 #include "DMRCSBK.h"
 #include "Rewrite.h"
+#include "DMREMB.h"
 
 #include <cstdio>
 
@@ -28,7 +29,8 @@ m_lc(),
 m_embeddedLC(),
 m_data(NULL),
 m_writeNum(0U),
-m_readNum(0U)
+m_readNum(0U),
+m_lastN(0U)
 {
 	m_data = new CDMREmbeddedData[2U];
 }
@@ -48,6 +50,10 @@ void CRewrite::processMessage(CDMRData& data)
 		processHeader(data, dataType);
 		break;
 
+	case DT_VOICE_SYNC:
+		processVoiceSync(data);
+		break;
+
 	case DT_VOICE:
 		processVoice(data);
 		break;
@@ -63,13 +69,10 @@ void CRewrite::processMessage(CDMRData& data)
 	case DT_RATE_12_DATA:
 	case DT_RATE_34_DATA:
 	case DT_RATE_1_DATA:
-		// Nothing to do
-		break;
-
-	case DT_VOICE_SYNC:
-		swap();
+		processData(data);
 		break;
 
+	case DT_VOICE_PI_HEADER:
 	default:
 		// Not sure what to do
 		break;
@@ -91,33 +94,29 @@ void CRewrite::setLC(FLCO flco, unsigned int srcId, unsigned int dstId)
 	m_writeNum = 0U;
 }
 
-unsigned char CRewrite::processEmbeddedData(unsigned char* data, unsigned char n)
+void CRewrite::processEmbeddedData(unsigned char* data, unsigned char n)
 {
-	unsigned char lcss = 0U;
+	CDMREMB emb;
+	emb.putData(data);
 
-	switch (n) {
-	case 1U:
-		lcss = 1U;
-		break;
-	case 4U:
-		lcss = 2U;
-		break;
-	case 2U:
-	case 3U:
-		lcss = 3U;
-		break;
-	default:
-		break;
-	}
+	unsigned char lcss = emb.getLCSS();
 
 	m_data[m_writeNum].addData(data, lcss);
 
-	if (m_readNum == 0U && m_writeNum == 0U)
-		return m_embeddedLC.getData(data, n);
+	if (m_readNum == 0U && m_writeNum == 0U) {
+		lcss = m_embeddedLC.getData(data, n);
+		emb.setLCSS(lcss);
+		emb.getData(data);
+		return;
+	}
 
 	CDMRLC* lc = m_data[m_readNum].getLC();
-	if (lc == NULL)
-		return m_embeddedLC.getData(data, n);
+	if (lc == NULL) {
+		lcss = m_embeddedLC.getData(data, n);
+		emb.setLCSS(lcss);
+		emb.getData(data);
+		return;
+	}
 
 	FLCO flco = lc->getFLCO();
 
@@ -125,13 +124,20 @@ unsigned char CRewrite::processEmbeddedData(unsigned char* data, unsigned char n
 
 	// Replace any identity embedded data with the new one
 	if (flco == FLCO_GROUP || flco == FLCO_USER_USER)
-		return m_embeddedLC.getData(data, n);
+		lcss = m_embeddedLC.getData(data, n);
 	else
-		return m_data[m_readNum].getData(data, n);
+		lcss = m_data[m_readNum].getData(data, n);
+
+	emb.setLCSS(lcss);
+	emb.getData(data);
 }
 
 void CRewrite::swap()
 {
+	// If we get a voice sync straight after a voice header (or another voice sync)
+	if (m_lastN == 0U)
+		return;
+
 	if (m_readNum == 0U && m_writeNum == 0U) {
 		m_writeNum = 1U;
 		return;
@@ -159,18 +165,35 @@ void CRewrite::processHeader(CDMRData& data, unsigned char dataType)
 	fullLC.encode(m_lc, buffer, dataType);
 
 	data.setData(buffer);
+
+	m_lastN = 0U;
+}
+
+void CRewrite::processVoiceSync(CDMRData& data)
+{
+	swap();
+
+	m_lastN = 0U;
 }
 
 void CRewrite::processVoice(CDMRData& data)
 {
 	setLC(data.getFLCO(), data.getSrcId(), data.getDstId());
 
+	unsigned char n = data.getN();
+
+	// In case we missed a voice sync packet
+	if (n < m_lastN)
+		swap();
+
 	unsigned char buffer[DMR_FRAME_LENGTH_BYTES];
 	data.getData(buffer);
 
-	processEmbeddedData(buffer, data.getN());
+	processEmbeddedData(buffer, n);
 
 	data.setData(buffer);
+
+	m_lastN = n;
 }
 
 void CRewrite::processDataHeader(CDMRData& data)
@@ -192,6 +215,11 @@ void CRewrite::processDataHeader(CDMRData& data)
 	data.setData(buffer);
 }
 
+void CRewrite::processData(CDMRData& data)
+{
+	// Nothing to do
+}
+
 void CRewrite::processCSBK(CDMRData& data)
 {
 	unsigned char buffer[DMR_FRAME_LENGTH_BYTES];
diff --git a/Rewrite.h b/Rewrite.h
index bd04340..f2789eb 100644
--- a/Rewrite.h
+++ b/Rewrite.h
@@ -39,16 +39,19 @@ private:
 	CDMREmbeddedData* m_data;
 	unsigned int      m_writeNum;
 	unsigned int      m_readNum;
+	unsigned char     m_lastN;
 
 	void processHeader(CDMRData& data, unsigned char dataType);
+	void processVoiceSync(CDMRData& data);
 	void processVoice(CDMRData& data);
 	void processDataHeader(CDMRData& data);
+	void processData(CDMRData& data);
 	void processCSBK(CDMRData& data);
 	void swap();
 
 	void setLC(FLCO flco, unsigned int srcId, unsigned int dstId);
 
-	unsigned char processEmbeddedData(unsigned char* data, unsigned char n);
+	void processEmbeddedData(unsigned char* data, unsigned char n);
 };
 
 #endif