diff --git a/AMBEFEC.cpp b/AMBEFEC.cpp
index 9d9d713..c7c651b 100644
--- a/AMBEFEC.cpp
+++ b/AMBEFEC.cpp
@@ -17,6 +17,7 @@
  */
 
 #include "Golay24128.h"
+#include "Hamming.h"
 #include "AMBEFEC.h"
 
 #include <cstdio>
@@ -453,6 +454,15 @@ const unsigned int DSTAR_B_TABLE[] = {2U,  8U, 14U, 20U, 26U, 32U, 38U, 44U, 50U
 const unsigned int DSTAR_C_TABLE[] = {4U, 10U, 16U, 22U, 28U, 34U, 40U, 46U, 52U, 58U, 64U, 70U,
 									  5U, 11U, 17U, 23U, 29U, 35U, 41U, 47U, 53U, 59U, 65U, 71U};
 
+const unsigned int IMBE_INTERLEAVE[] = {
+	0,  7, 12, 19, 24, 31, 36, 43, 48, 55, 60, 67, 72, 79, 84, 91,  96, 103, 108, 115, 120, 127, 132, 139,
+	1,  6, 13, 18, 25, 30, 37, 42, 49, 54, 61, 66, 73, 78, 85, 90,  97, 102, 109, 114, 121, 126, 133, 138,
+	2,  9, 14, 21, 26, 33, 38, 45, 50, 57, 62, 69, 74, 81, 86, 93,  98, 105, 110, 117, 122, 129, 134, 141,
+	3,  8, 15, 20, 27, 32, 39, 44, 51, 56, 63, 68, 75, 80, 87, 92,  99, 104, 111, 116, 123, 128, 135, 140,
+	4, 11, 16, 23, 28, 35, 40, 47, 52, 59, 64, 71, 76, 83, 88, 95, 100, 107, 112, 119, 124, 131, 136, 143,
+	5, 10, 17, 22, 29, 34, 41, 46, 53, 58, 65, 70, 77, 82, 89, 94, 101, 106, 113, 118, 125, 130, 137, 142
+};
+
 CAMBEFEC::CAMBEFEC()
 {
 }
@@ -583,50 +593,132 @@ unsigned int CAMBEFEC::regenerateDStar(unsigned char* bytes) const
 	return errors;
 }
 
-unsigned int CAMBEFEC::regenerateYSF1(unsigned char* bytes) const
+unsigned int CAMBEFEC::regenerateYSF3(unsigned char* bytes) const
 {
 	assert(bytes != NULL);
 
-	unsigned int errors = 0U;
-	unsigned int offset = 72U;
-	for (unsigned int j = 0U; j < 5U; j++, offset += 144U) {
-		unsigned int a = 0U;
-		unsigned int b = 0U;
-		unsigned int c = 0U;
+	bool temp[144U];
 
-		unsigned int MASK = 0x800000U;
-		for (unsigned int i = 0U; i < 24U; i++) {
-			unsigned int aPos = DMR_A_TABLE[i] + offset;
-			unsigned int bPos = DMR_B_TABLE[i] + offset;
-			unsigned int cPos = DMR_C_TABLE[i] + offset;
-
-			if (READ_BIT(bytes, aPos))
-				a |= MASK;
-			if (READ_BIT(bytes, bPos))
-				b |= MASK;
-			if (READ_BIT(bytes, cPos))
-				c |= MASK;
-
-			MASK >>= 1;
-		}
-
-		errors += regenerate(a, b, c, true);
-
-		MASK = 0x800000U;
-		for (unsigned int i = 0U; i < 24U; i++) {
-			unsigned int aPos = DMR_A_TABLE[i] + offset;
-			unsigned int bPos = DMR_B_TABLE[i] + offset;
-			unsigned int cPos = DMR_C_TABLE[i] + offset;
-
-			WRITE_BIT(bytes, aPos, a & MASK);
-			WRITE_BIT(bytes, bPos, b & MASK);
-			WRITE_BIT(bytes, cPos, c & MASK);
-
-			MASK >>= 1;
-		}
+	// De-interleave
+	for (unsigned int i = 0U; i < 144U; i++) {
+		unsigned int n = IMBE_INTERLEAVE[i];
+		temp[i] = READ_BIT(bytes, n);
 	}
 
-	return errors;
+	// now ..
+
+	// 12 voice bits     0
+	// 11 golay bits     12
+	//
+	// 12 voice bits     23
+	// 11 golay bits     35
+	//
+	// 12 voice bits     46
+	// 11 golay bits     58
+	//
+	// 12 voice bits     69
+	// 11 golay bits     81
+	//
+	// 11 voice bits     92
+	//  4 hamming bits   103
+	//
+	// 11 voice bits     107
+	//  4 hamming bits   118
+	//
+	// 11 voice bits     122
+	//  4 hamming bits   133
+	//
+	//  7 voice bits     137
+
+	// De-Whiten some bits
+	unsigned int prn = 0x00U;
+	for (int i = 0U; i < 12U; i++)
+		prn = (prn << 1) | (temp[i] & 1);
+	prn <<= 4;
+	for (unsigned int i = 23U; i < 137U; i++) {
+		prn = (unsigned short)((173 * prn) + 13849);
+		temp[i] ^= (prn >> 15) & 1;
+	}
+
+	// Check/Fix FEC
+	bool* bit = temp;
+
+	// c0
+	unsigned int g1 = 0U;
+	for (unsigned int i = 0U; i < 23U; i++)
+		g1 = (g1 << 1) | (bit[i] & 1);
+	unsigned int g3 = CGolay24128::decode23127(g1);
+	unsigned int g2 = CGolay24128::encode23127(g3);
+	for (int i = 23; i >= 0; i--) {
+		bit[i] = g2 & 1;
+		g2 >>= 1;
+	}
+	bit += 23U;
+
+	// c1
+	g1 = 0U;
+	for (unsigned int i = 0U; i < 23U; i++)
+		g1 = (g1 << 1) | (bit[i] & 1);
+	g3 = CGolay24128::decode23127(g1);
+	g2 = CGolay24128::encode23127(g3);
+	for (int i = 23; i >= 0; i--) {
+		bit[i] = g2 & 1;
+		g2 >>= 1;
+	}
+	bit += 23U;
+
+	// c2
+	g1 = 0;
+	for (int i = 0; i < 23; i++)
+		g1 = (g1 << 1) | (bit[i] & 1);
+	g3 = CGolay24128::decode23127(g1);
+	g2 = CGolay24128::encode23127(g3);
+	for (int i = 23; i >= 0; i--) {
+		bit[i] = g2 & 1;
+		g2 >>= 1;
+	}
+	bit += 23U;
+
+	// c3
+	g1 = 0U;
+	for (int i = 0U; i < 23U; i++)
+		g1 = (g1 << 1) | (bit[i] & 1);
+	g3 = CGolay24128::decode23127(g1);
+	g2 = CGolay24128::encode23127(g3);
+	for (int i = 23; i >= 0; i--) {
+		bit[i] = g2 & 1;
+		g2 >>= 1;
+	}
+	bit += 23U;
+
+	// c4
+	CHamming::decode15113_1(bit);
+	bit += 15U;
+
+	// c5
+	CHamming::decode15113_1(bit);
+	bit += 15U;
+
+	// c6
+	CHamming::decode15113_1(bit);
+
+	// Whiten some bits
+	prn = 0x00U;
+	for (unsigned int i = 0U; i < 12U; i++)
+		prn = (prn << 1) | (temp[i] & 1);
+	prn <<= 4;
+	for (unsigned int i = 23U; i < 137U; i++) {
+		prn = (unsigned short)((173 * prn) + 13849);
+		temp[i] ^= (prn >> 15) & 1;
+	}
+
+	// Interleave
+	for (unsigned int i = 0U; i < 144U; i++) {
+		unsigned int n = IMBE_INTERLEAVE[i];
+		WRITE_BIT(bytes, n, temp[i]);
+	}
+
+	return 0U;
 }
 
 unsigned int CAMBEFEC::regenerate(unsigned int& a, unsigned int& b, unsigned int& c, bool b23) const
diff --git a/AMBEFEC.h b/AMBEFEC.h
index bf5efcc..35e8d27 100644
--- a/AMBEFEC.h
+++ b/AMBEFEC.h
@@ -28,7 +28,7 @@ public:
 
 	unsigned int regenerateDStar(unsigned char* bytes) const;
 
-	unsigned int regenerateYSF1(unsigned char* bytes) const;
+	unsigned int regenerateYSF3(unsigned char* bytes) const;
 
 private:
 	unsigned int regenerate(unsigned int& a, unsigned int& b, unsigned int& c, bool b23) const;
diff --git a/YSFControl.cpp b/YSFControl.cpp
index ebb4d8b..a1ba034 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -108,8 +108,7 @@ bool CYSFControl::writeModem(unsigned char *data)
 
 		LogMessage("YSF, EOT, FI=%X FN=%u FT=%u DT=%X", fi, fn, ft, dt);
 
-		m_payload.decode(data + 2U, fi, fn, ft, dt);
-		// m_payload.encode(data + 2U);			XXX
+		m_payload.process(data + 2U, fi, fn, ft, dt);
 
 		m_frames++;
 
@@ -150,14 +149,19 @@ bool CYSFControl::writeModem(unsigned char *data)
 
 			LogMessage("YSF, Valid FICH, FI=%X FN=%u FT=%u DT=%X", fi, fn, ft, dt);
 
-			m_payload.decode(data + 2U, fi, fn, ft, dt);
-			// payload.encode(data + 2U);			XXX
+			m_payload.process(data + 2U, fi, fn, ft, dt);
 
 			bool change = false;
 
-			if (cm == 0x00U && m_dest == NULL) {
-				m_dest = (unsigned char*)"CQCQCQ    ";
-				change = true;
+			if (m_dest == NULL) {
+				if (cm == YSF_CM_GROUP) {
+					m_dest = (unsigned char*)"CQCQCQ    ";
+					change = true;
+				} else {
+					m_dest = m_payload.getDest();
+					if (m_dest != NULL)
+						change = true;
+				}
 			}
 
 			if (m_source == NULL) {
@@ -166,12 +170,6 @@ bool CYSFControl::writeModem(unsigned char *data)
 					change = true;
 			}
 
-			if (m_dest == NULL) {
-				m_dest = m_payload.getDest();
-				if (m_dest != NULL)
-					change = true;
-			}
-
 			if (change) {
 				if (m_source != NULL && m_dest != NULL) {
 					m_display->writeFusion((char*)m_source, (char*)m_dest);
@@ -188,7 +186,7 @@ bool CYSFControl::writeModem(unsigned char *data)
 			}
 		} else {
 			// Reconstruct FICH based on the last valid frame
-			m_fich.setFI(0x01U);		// Communication channel
+			m_fich.setFI(YSF_FI_COMMUNICATIONS);		// Communication channel
 		}
 
 		m_frames++;
diff --git a/YSFPayload.cpp b/YSFPayload.cpp
index d2cf07d..d9357b6 100644
--- a/YSFPayload.cpp
+++ b/YSFPayload.cpp
@@ -75,70 +75,56 @@ const unsigned char BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U
 #define READ_BIT1(p,i)    (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
 
 CYSFPayload::CYSFPayload() :
-m_data(NULL),
 m_uplink(NULL),
 m_downlink(NULL),
 m_source(NULL),
 m_dest(NULL),
 m_fec()
 {
-	m_data = new unsigned char[90U];
 }
 
 CYSFPayload::~CYSFPayload()
 {
-	delete[] m_data;
 	delete[] m_uplink;
 	delete[] m_downlink;
 	delete[] m_source;
 	delete[] m_dest;
 }
 
-void CYSFPayload::decode(const unsigned char* bytes, unsigned char fi, unsigned char fn, unsigned char ft, unsigned char dt)
+void CYSFPayload::process(unsigned char* bytes, unsigned char fi, unsigned char fn, unsigned char ft, unsigned char dt)
 {
 	assert(bytes != NULL);
 
-	::memcpy(m_data, bytes + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES, 90U);
-
 	// Header and trailer
 	if (fi == 0U || fi == 2U) {
-		decodeHeader();
+		processHeader(bytes + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES);
 		return;
 	}
 
-	// V/D Mode 1
-	if (dt == 0U) {
-		decodeVDMode1(fn);
-		return;
-	}
+	switch (dt) {
+	case YSF_DT_VD_MODE1:
+		processVDMode1(bytes + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES, fn);
+		break;
 
-	// V/D Mode 2
-	if (dt == 2U) {
-		decodeVDMode2(fn);
-		return;
-	}
+	case YSF_DT_VD_MODE2:
+		processVDMode2(bytes + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES, fn);
+		break;
 
-	// Data FR Mode
-	if (dt == 1U) {
-		decodeDataFRMode(fn);
-		return;
-	}
+	case YSF_DT_DATA_FR_MODE:
+		processDataFRMode(bytes + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES, fn);
+		break;
 
-	// Voice FR Mode
+	default:		// YSF_DT_VOICE_FR_MODE
+		processVoiceFRMode(bytes + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES);
+		break;
+	}
 }
 
-void CYSFPayload::encode(unsigned char* bytes)
-{
-	assert(bytes != NULL);
-
-	::memcpy(bytes + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES, m_data, 90U);
-}
-
-void CYSFPayload::decodeHeader()
+void CYSFPayload::processHeader(unsigned char* data)
 {
 	unsigned char dch[45U];
 
-	unsigned char* p1 = m_data;
+	unsigned char* p1 = data;
 	unsigned char* p2 = dch;
 	for (unsigned int i = 0U; i < 5U; i++) {
 		::memcpy(p2, p1, 9U);
@@ -205,7 +191,7 @@ void CYSFPayload::decodeHeader()
 			WRITE_BIT1(bytes, n, s1);
 		}
 
-		p1 = m_data;
+		p1 = data;
 		p2 = bytes;
 		for (unsigned int i = 0U; i < 5U; i++) {
 			::memcpy(p1, p2, 9U);
@@ -244,7 +230,7 @@ void CYSFPayload::decodeHeader()
 		WRITE_BIT1(bytes, n, s1);
 	}
 
-	p1 = m_data + 9U;
+	p1 = data + 9U;
 	p2 = bytes;
 	for (unsigned int i = 0U; i < 5U; i++) {
 		::memcpy(p1, p2, 9U);
@@ -252,16 +238,21 @@ void CYSFPayload::decodeHeader()
 	}
 }
 
-void CYSFPayload::decodeVDMode1(unsigned char fn)
+void CYSFPayload::processVDMode1(unsigned char* data, unsigned char fn)
 {
 	// Regenerate the AMBE FEC
-	unsigned int errors = m_fec.regenerateYSF1(m_data);
+	unsigned int errors = 0U;
+	errors += m_fec.regenerateDMR(data + 9U);
+	errors += m_fec.regenerateDMR(data + 27U);
+	errors += m_fec.regenerateDMR(data + 45U);
+	errors += m_fec.regenerateDMR(data + 63U);
+	errors += m_fec.regenerateDMR(data + 81U);
 
 	LogMessage("YSF, V/D Mode 1, AMBE FEC %u/235 (%.1f%%)", errors, float(errors) / 235.0F);
 
 	unsigned char dch[45U];
 
-	unsigned char* p1 = m_data;
+	unsigned char* p1 = data;
 	unsigned char* p2 = dch;
 	for (unsigned int i = 0U; i < 5U; i++) {
 		::memcpy(p2, p1, 9U);
@@ -356,7 +347,7 @@ void CYSFPayload::decodeVDMode1(unsigned char fn)
 			WRITE_BIT1(bytes, n, s1);
 		}
 
-		p1 = m_data;
+		p1 = data;
 		p2 = bytes;
 		for (unsigned int i = 0U; i < 5U; i++) {
 			::memcpy(p1, p2, 9U);
@@ -365,11 +356,11 @@ void CYSFPayload::decodeVDMode1(unsigned char fn)
 	}
 }
 
-void CYSFPayload::decodeVDMode2(unsigned char fn)
+void CYSFPayload::processVDMode2(unsigned char* data, unsigned char fn)
 {
 	unsigned char dch[25U];
 
-	unsigned char* p1 = m_data;
+	unsigned char* p1 = data;
 	unsigned char* p2 = dch;
 	for (unsigned int i = 0U; i < 5U; i++) {
 		::memcpy(p2, p1, 5U);
@@ -467,7 +458,7 @@ void CYSFPayload::decodeVDMode2(unsigned char fn)
 			WRITE_BIT1(bytes, n, s1);
 		}
 
-		p1 = m_data;
+		p1 = data;
 		p2 = bytes;
 		for (unsigned int i = 0U; i < 5U; i++) {
 			::memcpy(p1, p2, 5U);
@@ -476,11 +467,11 @@ void CYSFPayload::decodeVDMode2(unsigned char fn)
 	}
 }
 
-void CYSFPayload::decodeDataFRMode(unsigned char fn)
+void CYSFPayload::processDataFRMode(unsigned char* data, unsigned char fn)
 {
 	unsigned char dch[45U];
 
-	unsigned char* p1 = m_data;
+	unsigned char* p1 = data;
 	unsigned char* p2 = dch;
 	for (unsigned int i = 0U; i < 5U; i++) {
 		::memcpy(p2, p1, 9U);
@@ -559,7 +550,7 @@ void CYSFPayload::decodeDataFRMode(unsigned char fn)
 			WRITE_BIT1(bytes, n, s1);
 		}
 
-		p1 = m_data;
+		p1 = data;
 		p2 = bytes;
 		for (unsigned int i = 0U; i < 5U; i++) {
 			::memcpy(p1, p2, 9U);
@@ -567,7 +558,7 @@ void CYSFPayload::decodeDataFRMode(unsigned char fn)
 		}
 	}
 
-	p1 = m_data + 9U;
+	p1 = data + 9U;
 	p2 = dch;
 	for (unsigned int i = 0U; i < 5U; i++) {
 		::memcpy(p2, p1, 9U);
@@ -630,7 +621,7 @@ void CYSFPayload::decodeDataFRMode(unsigned char fn)
 			WRITE_BIT1(bytes, n, s1);
 		}
 
-		p1 = m_data + 9U;
+		p1 = data + 9U;
 		p2 = bytes;
 		for (unsigned int i = 0U; i < 5U; i++) {
 			::memcpy(p1, p2, 9U);
@@ -639,6 +630,19 @@ void CYSFPayload::decodeDataFRMode(unsigned char fn)
 	}
 }
 
+void CYSFPayload::processVoiceFRMode(unsigned char* data)
+{
+	// Regenerate the AMBE FEC
+	unsigned int errors = 0U;
+	errors += m_fec.regenerateYSF3(data + 0U);
+	errors += m_fec.regenerateYSF3(data + 18U);
+	errors += m_fec.regenerateYSF3(data + 36U);
+	errors += m_fec.regenerateYSF3(data + 54U);
+	errors += m_fec.regenerateYSF3(data + 72U);
+
+	LogMessage("YSF, V Mode 3, AMBE FEC %u/720 (%.1f%%)", errors, float(errors) / 720.0F);
+}
+
 void CYSFPayload::setUplink(const std::string& callsign)
 {
 	m_uplink = new unsigned char[10U];
diff --git a/YSFPayload.h b/YSFPayload.h
index 09f33de..e9ec5ca 100644
--- a/YSFPayload.h
+++ b/YSFPayload.h
@@ -28,9 +28,7 @@ public:
 	CYSFPayload();
 	~CYSFPayload();
 
-	void decode(const unsigned char* bytes, unsigned char fi, unsigned char fn, unsigned char ft, unsigned char dt);
-
-	void encode(unsigned char* bytes);
+	void process(unsigned char* bytes, unsigned char fi, unsigned char fn, unsigned char ft, unsigned char dt);
 
 	unsigned char* getSource();
 	unsigned char* getDest();
@@ -41,17 +39,17 @@ public:
 	void reset();
 
 private:
-	unsigned char* m_data;
 	unsigned char* m_uplink;
 	unsigned char* m_downlink;
 	unsigned char* m_source;
 	unsigned char* m_dest;
 	CAMBEFEC       m_fec;
 
-	void decodeHeader();
-	void decodeVDMode1(unsigned char fn);
-	void decodeVDMode2(unsigned char fn);
-	void decodeDataFRMode(unsigned char fn);
+	void processHeader(unsigned char* bytes);
+	void processVDMode1(unsigned char* bytes, unsigned char fn);
+	void processVDMode2(unsigned char* bytes, unsigned char fn);
+	void processDataFRMode(unsigned char* bytes, unsigned char fn);
+	void processVoiceFRMode(unsigned char* bytes);
 };
 
 #endif