From b9b8cf12f8690a0087efbf77b285ea2d145feb25 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <naylorjs@yahoo.com>
Date: Tue, 1 Mar 2016 07:19:44 +0000
Subject: [PATCH] Add V/D Mode 1 AMBE FEC.

---
 AMBEFEC.cpp    | 114 +++++++++++++++++++++++++++++++++++++++++++++++++
 AMBEFEC.h      |   3 ++
 YSFPayload.cpp |   7 +--
 3 files changed, 118 insertions(+), 6 deletions(-)

diff --git a/AMBEFEC.cpp b/AMBEFEC.cpp
index e24c38c..e47cc35 100644
--- a/AMBEFEC.cpp
+++ b/AMBEFEC.cpp
@@ -583,6 +583,120 @@ unsigned int CAMBEFEC::regenerateDStar(unsigned char* bytes) const
 	return errors;
 }
 
+unsigned int CAMBEFEC::regenerateYSF1(unsigned char* bytes) const
+{
+	assert(bytes != NULL);
+
+	unsigned int a1 = 0U, a2 = 0U, a3 = 0U, a4 = 0U, a5 = 0U;
+	unsigned int b1 = 0U, b2 = 0U, b3 = 0U, b4 = 0U, b5 = 0U;
+	unsigned int c1 = 0U, c2 = 0U, c3 = 0U, c4 = 0U, c5 = 0U;
+
+	unsigned int MASK = 0x800000U;
+	for (unsigned int i = 0U; i < 24U; i++) {
+		unsigned int a1Pos = DMR_A_TABLE[i] + 72U;
+		unsigned int b1Pos = DMR_B_TABLE[i] + 72U;
+		unsigned int c1Pos = DMR_C_TABLE[i] + 72U;
+
+		unsigned int a2Pos = a1Pos + 144U;
+		unsigned int b2Pos = b1Pos + 144U;
+		unsigned int c2Pos = c1Pos + 144U;
+
+		unsigned int a3Pos = a2Pos + 144U;
+		unsigned int b3Pos = b2Pos + 144U;
+		unsigned int c3Pos = c2Pos + 144U;
+
+		unsigned int a4Pos = a3Pos + 144U;
+		unsigned int b4Pos = b3Pos + 144U;
+		unsigned int c4Pos = c3Pos + 144U;
+
+		unsigned int a5Pos = a4Pos + 144U;
+		unsigned int b5Pos = b4Pos + 144U;
+		unsigned int c5Pos = c4Pos + 144U;
+
+		if (READ_BIT(bytes, a1Pos))
+			a1 |= MASK;
+		if (READ_BIT(bytes, a2Pos))
+			a2 |= MASK;
+		if (READ_BIT(bytes, a3Pos))
+			a3 |= MASK;
+		if (READ_BIT(bytes, a4Pos))
+			a4 |= MASK;
+		if (READ_BIT(bytes, a5Pos))
+			a5 |= MASK;
+		if (READ_BIT(bytes, b1Pos))
+			b1 |= MASK;
+		if (READ_BIT(bytes, b2Pos))
+			b2 |= MASK;
+		if (READ_BIT(bytes, b3Pos))
+			b3 |= MASK;
+		if (READ_BIT(bytes, b4Pos))
+			b4 |= MASK;
+		if (READ_BIT(bytes, b5Pos))
+			b5 |= MASK;
+		if (READ_BIT(bytes, c1Pos))
+			c1 |= MASK;
+		if (READ_BIT(bytes, c2Pos))
+			c2 |= MASK;
+		if (READ_BIT(bytes, c3Pos))
+			c3 |= MASK;
+		if (READ_BIT(bytes, c4Pos))
+			c4 |= MASK;
+		if (READ_BIT(bytes, c5Pos))
+			c5 |= MASK;
+
+		MASK >>= 1;
+	}
+
+	unsigned int errors = regenerate(a1, b1, c1, true);
+	errors += regenerate(a2, b2, c2, true);
+	errors += regenerate(a3, b3, c3, true);
+	errors += regenerate(a4, b4, c4, true);
+	errors += regenerate(a5, b5, c5, true);
+
+	MASK = 0x800000U;
+	for (unsigned int i = 0U; i < 24U; i++) {
+		unsigned int a1Pos = DMR_A_TABLE[i] + 72U;
+		unsigned int b1Pos = DMR_B_TABLE[i] + 72U;
+		unsigned int c1Pos = DMR_C_TABLE[i] + 72U;
+
+		unsigned int a2Pos = a1Pos + 144U;
+		unsigned int b2Pos = b1Pos + 144U;
+		unsigned int c2Pos = c1Pos + 144U;
+
+		unsigned int a3Pos = a2Pos + 144U;
+		unsigned int b3Pos = b2Pos + 144U;
+		unsigned int c3Pos = c2Pos + 144U;
+
+		unsigned int a4Pos = a3Pos + 144U;
+		unsigned int b4Pos = b3Pos + 144U;
+		unsigned int c4Pos = c3Pos + 144U;
+
+		unsigned int a5Pos = a4Pos + 144U;
+		unsigned int b5Pos = b4Pos + 144U;
+		unsigned int c5Pos = c4Pos + 144U;
+
+		WRITE_BIT(bytes, a1Pos, a1 & MASK);
+		WRITE_BIT(bytes, a2Pos, a2 & MASK);
+		WRITE_BIT(bytes, a3Pos, a3 & MASK);
+		WRITE_BIT(bytes, a4Pos, a4 & MASK);
+		WRITE_BIT(bytes, a5Pos, a5 & MASK);
+		WRITE_BIT(bytes, b1Pos, b1 & MASK);
+		WRITE_BIT(bytes, b2Pos, b2 & MASK);
+		WRITE_BIT(bytes, b3Pos, b3 & MASK);
+		WRITE_BIT(bytes, b4Pos, b4 & MASK);
+		WRITE_BIT(bytes, b5Pos, b5 & MASK);
+		WRITE_BIT(bytes, c1Pos, c1 & MASK);
+		WRITE_BIT(bytes, c2Pos, c2 & MASK);
+		WRITE_BIT(bytes, c3Pos, c3 & MASK);
+		WRITE_BIT(bytes, c4Pos, c4 & MASK);
+		WRITE_BIT(bytes, c5Pos, c5 & MASK);
+
+		MASK >>= 1;
+	}
+
+	return errors;
+}
+
 unsigned int CAMBEFEC::regenerate(unsigned int& a, unsigned int& b, unsigned int& c, bool b23) const
 {
 	unsigned int old_a = a;
diff --git a/AMBEFEC.h b/AMBEFEC.h
index 04a8b94..bf5efcc 100644
--- a/AMBEFEC.h
+++ b/AMBEFEC.h
@@ -25,8 +25,11 @@ public:
 	~CAMBEFEC();
 
 	unsigned int regenerateDMR(unsigned char* bytes) const;
+
 	unsigned int regenerateDStar(unsigned char* bytes) const;
 
+	unsigned int regenerateYSF1(unsigned char* bytes) const;
+
 private:
 	unsigned int regenerate(unsigned int& a, unsigned int& b, unsigned int& c, bool b23) const;
 };
diff --git a/YSFPayload.cpp b/YSFPayload.cpp
index 96e7f24..d999d8b 100644
--- a/YSFPayload.cpp
+++ b/YSFPayload.cpp
@@ -255,12 +255,7 @@ void CYSFPayload::decodeHeader()
 void CYSFPayload::decodeVDMode1(unsigned char fn)
 {
 	// Regenerate the AMBE FEC
-	unsigned int errors = 0U;
-	unsigned char* p = m_data + 9U;
-	for (unsigned int i = 0U; i < 5U; i++) {
-		errors += m_fec.regenerateDMR(p);
-		p += 9U;
-	}
+	unsigned int errors = m_fec.regenerateYSF1(m_data);
 
 	LogMessage("YSF, V/D Mode 1, AMBE FEC %u/235 (%.1f%%)", errors, float(errors) / 235.0F);