From 701c6b2387586523f7147fc53250818374a75322 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <jnaylor@office.igence.com>
Date: Mon, 14 Aug 2017 14:08:43 +0100
Subject: [PATCH 1/8] Begin refacturing the different YSF modes.

---
 YSFControl.cpp | 746 ++++++++++++++++++++++++++++++++-----------------
 YSFControl.h   |  15 +-
 YSFFICH.cpp    |  16 ++
 YSFFICH.h      |   3 +
 4 files changed, 509 insertions(+), 271 deletions(-)

diff --git a/YSFControl.cpp b/YSFControl.cpp
index 3aa8ae2..9563098 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -12,7 +12,6 @@
  */
 
 #include "YSFControl.h"
-#include "YSFFICH.h"
 #include "Utils.h"
 #include "Sync.h"
 #include "Log.h"
@@ -52,12 +51,7 @@ m_rfSource(NULL),
 m_rfDest(NULL),
 m_netSource(NULL),
 m_netDest(NULL),
-m_lastFrame(NULL),
-m_lastFrameValid(false),
-m_lastSQL(false),
-m_lastSQ(0U),
-m_lastMode(YSF_DT_VOICE_FR_MODE),
-m_lastMR(YSF_MR_NOT_BUSY),
+m_lastFICH(),
 m_netN(0U),
 m_rfPayload(),
 m_netPayload(),
@@ -80,8 +74,6 @@ m_fp(NULL)
 	m_netSource = new unsigned char[YSF_CALLSIGN_LENGTH];
 	m_netDest   = new unsigned char[YSF_CALLSIGN_LENGTH];
 
-	m_lastFrame = new unsigned char[YSF_FRAME_LENGTH_BYTES + 2U];
-
 	m_callsign = new unsigned char[YSF_CALLSIGN_LENGTH];
 
 	std::string node = callsign;
@@ -95,7 +87,6 @@ CYSFControl::~CYSFControl()
 {
 	delete[] m_netSource;
 	delete[] m_netDest;
-	delete[] m_lastFrame;
 	delete[] m_callsign;
 }
 
@@ -148,89 +139,93 @@ bool CYSFControl::writeModem(unsigned char *data, unsigned int len)
 	CYSFFICH fich;
 	bool valid = fich.decode(data + 2U);
 
-	if (valid) {
-		m_lastSQL = fich.getSQL();
-		m_lastSQ  = fich.getSQ();
-
-		if (m_rfState == RS_RF_LISTENING) {
-			unsigned char fi = fich.getFI();
-			if (fi == YSF_FI_TERMINATOR)
-				return false;
-
-			if (m_sqlEnabled) {
-				if (!m_lastSQL || m_lastSQ != m_sqlValue)
-					return false;
-			}
-
-			m_rfFrames = 0U;
-			m_rfErrs = 0U;
-			m_rfBits = 1U;
-			m_rfTimeoutTimer.start();
-			m_rfPayload.reset();
-			m_rfState = RS_RF_AUDIO;
-
-			m_minRSSI = m_rssi;
-			m_maxRSSI = m_rssi;
-			m_aveRSSI = m_rssi;
-			m_rssiCount = 1U;
-#if defined(DUMP_YSF)
-			openFile();
-#endif
-		}
-	}
-
-	if (m_rfState != RS_RF_AUDIO)
-		return false;
+	if (valid)
+		m_lastFICH = fich;
 
+	// Validate the DSQ value if enabled
 	if (m_sqlEnabled) {
-		if (!m_lastSQL || m_lastSQ != m_sqlValue)
+		bool sql = m_lastFICH.getSQL();
+		unsigned char value = m_lastFICH.getSQ();
+
+		if (!sql || value != m_sqlValue)
 			return false;
 	}
 
-	if (valid)
-		m_lastMR = fich.getMR();
-
 	// Stop repeater packets coming through, unless we're acting as a remote gateway
 	if (m_remoteGateway) {
-		if (m_lastMR != YSF_MR_BUSY)
+		unsigned char mr = m_lastFICH.getMR();
+		if (mr != YSF_MR_BUSY)
 			return false;
 	} else {
-		if (m_lastMR == YSF_MR_BUSY)
+		unsigned char mr = m_lastFICH.getMR();
+		if (mr == YSF_MR_BUSY)
 			return false;
 	}
 
-	unsigned char fi = fich.getFI();
-	if (valid && fi == YSF_FI_HEADER) {
+	unsigned char dt = m_lastFICH.getDT();
+
+	bool ret = false;
+	switch (dt) {
+	case YSF_DT_VOICE_FR_MODE:
+		ret = processVWData(valid, data);
+		break;
+
+	case YSF_DT_VD_MODE1:
+	case YSF_DT_VD_MODE2:
+		ret = processDNData(valid, data);
+		break;
+
+	case YSF_DT_DATA_FR_MODE:
+		ret = processFRData(valid, data);
+		break;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+bool CYSFControl::processVWData(bool valid, unsigned char *data)
+{
+	unsigned char fi = m_lastFICH.getFI();
+	if (fi == YSF_FI_HEADER) {
+		if (m_rfState != RS_RF_LISTENING)
+			return false;
+
+		bool valid = m_rfPayload.processHeaderData(data + 2U);
+		if (!valid)
+			return false;
+
 		CSync::addYSFSync(data + 2U);
 
-		// LogDebug("YSF, FICH: FI: %u, DT: %u, FN: %u, FT: %u", fich.getFI(), fich.getDT(), fich.getFN(), fich.getFT());
+		m_rfSource = m_rfPayload.getSource();
 
-		valid = m_rfPayload.processHeaderData(data + 2U);
-
-		if (valid)
-			m_rfSource = m_rfPayload.getSource();
-
-		unsigned char cm = fich.getCM();
-		if (cm == YSF_CM_GROUP) {
+		unsigned char cm = m_lastFICH.getCM();
+		if (cm == YSF_CM_GROUP)
 			m_rfDest = (unsigned char*)"ALL       ";
-		} else {
-			if (valid)
-				m_rfDest = m_rfPayload.getDest();
-		}
+		else
+			m_rfDest = m_rfPayload.getDest();
 
-		if (m_rfSource != NULL && m_rfDest != NULL) {
-			m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
-			LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
-		} else if (m_rfSource == NULL && m_rfDest != NULL) {
-			m_display->writeFusion("??????????", (char*)m_rfDest, "R", "          ");
-			LogMessage("YSF, received RF header from ?????????? to %10.10s", m_rfDest);
-		} else if (m_rfSource != NULL && m_rfDest == NULL) {
-			m_display->writeFusion((char*)m_rfSource, "??????????", "R", "          ");
-			LogMessage("YSF, received RF header from %10.10s to ??????????", m_rfSource);
-		} else {
-			m_display->writeFusion("??????????", "??????????", "R", "          ");
-			LogMessage("YSF, received RF header from ?????????? to ??????????");
-		}
+		m_rfFrames = 0U;
+		m_rfErrs = 0U;
+		m_rfBits = 1U;
+		m_rfTimeoutTimer.start();
+		m_rfPayload.reset();
+		m_rfState = RS_RF_AUDIO;
+
+		m_minRSSI = m_rssi;
+		m_maxRSSI = m_rssi;
+		m_aveRSSI = m_rssi;
+		m_rssiCount = 1U;
+#if defined(DUMP_YSF)
+		openFile();
+#endif
+
+		m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
+		LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
+
+		CYSFFICH fich = m_lastFICH;
 
 		// Remove any DSQ information
 		fich.setSQL(false);
@@ -256,13 +251,16 @@ bool CYSFControl::writeModem(unsigned char *data, unsigned int len)
 		m_rfFrames++;
 
 		m_display->writeFusionRSSI(m_rssi);
-	} else if (valid && fi == YSF_FI_TERMINATOR) {
-		CSync::addYSFSync(data + 2U);
-
-		// LogDebug("YSF, FICH: FI: %u, DT: %u, FN: %u, FT: %u", fich.getFI(), fich.getDT(), fich.getFN(), fich.getFT());
+	} else if (fi == YSF_FI_TERMINATOR) {
+		if (m_rfState != RS_RF_AUDIO)
+			return false;
 
 		m_rfPayload.processHeaderData(data + 2U);
 
+		CSync::addYSFSync(data + 2U);
+
+		CYSFFICH fich = m_lastFICH;
+
 		// Remove any DSQ information
 		fich.setSQL(false);
 		fich.setSQ(0U);
@@ -294,92 +292,26 @@ bool CYSFControl::writeModem(unsigned char *data, unsigned int len)
 		writeEndRF();
 
 		return false;
-	} else if (valid) {
+	} else {
+		if (m_rfState != RS_RF_AUDIO)
+			return false;
+
+		// XXX If valid is false, update the m_lastFICH for this transmission
+
 		CSync::addYSFSync(data + 2U);
 
-		unsigned char bn = fich.getBN();
-		unsigned char bt = fich.getBT();
+		CYSFFICH fich = m_lastFICH;
+
 		unsigned char fn = fich.getFN();
 		unsigned char ft = fich.getFT();
-		unsigned char dt = fich.getDT();
 
-		// LogDebug("YSF, FICH: FI: %u, DT: %u, FN: %u, FT: %u", fich.getFI(), fich.getDT(), fich.getFN(), fich.getFT());
-
-		switch (dt) {
-		case YSF_DT_VD_MODE1: {
-				valid = m_rfPayload.processVDMode1Data(data + 2U, fn);
-				unsigned int errors = m_rfPayload.processVDMode1Audio(data + 2U);
-				m_rfErrs += errors;
-				m_rfBits += 235U;
-				m_display->writeFusionBER(float(errors) / 2.35F);
-				LogDebug("YSF, V/D Mode 1, seq %u, AMBE FEC %u/235 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 2.35F);
-			}
-			break;
-
-		case YSF_DT_VD_MODE2: {
-				valid = m_rfPayload.processVDMode2Data(data + 2U, fn);
-				unsigned int errors = m_rfPayload.processVDMode2Audio(data + 2U);
-				m_rfErrs += errors;
-				m_rfBits += 135U;
-				m_display->writeFusionBER(float(errors) / 1.35F);
-				LogDebug("YSF, V/D Mode 2, seq %u, Repetition FEC %u/135 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 1.35F);
-			}
-			break;
-
-		case YSF_DT_DATA_FR_MODE:
-			LogDebug("YSF, RF data FICH B=%u/%u F=%u/%u", bn, bt, fn, ft);
-			valid = m_rfPayload.processDataFRModeData(data + 2U, fn);
-			break;
-
-		case YSF_DT_VOICE_FR_MODE:
-			if (fn != 0U || ft != 1U) {
-				// The first packet after the header is odd, don't try and regenerate it
-				unsigned int errors = m_rfPayload.processVoiceFRModeAudio(data + 2U);
-				m_rfErrs += errors;
-				m_rfBits += 720U;
-				m_display->writeFusionBER(float(errors) / 7.2F);
-				LogDebug("YSF, V Mode 3, seq %u, AMBE FEC %u/720 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 7.2F);
-			}
-			valid = false;
-			break;
-
-		default:
-			break;
-		}
-
-		bool change = false;
-
-		if (m_rfDest == NULL) {
-			unsigned char cm = fich.getCM();
-			if (cm == YSF_CM_GROUP) {
-				m_rfDest = (unsigned char*)"ALL       ";
-				change = true;
-			} else if (valid) {
-				m_rfDest = m_rfPayload.getDest();
-				if (m_rfDest != NULL)
-					change = true;
-			}
-		}
-
-		if (valid && m_rfSource == NULL) {
-			m_rfSource = m_rfPayload.getSource();
-			if (m_rfSource != NULL)
-				change = true;
-		}
-
-		if (change) {
-			if (m_rfSource != NULL && m_rfDest != NULL) {
-				m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
-				LogMessage("YSF, received RF data from %10.10s to %10.10s", m_rfSource, m_rfDest);
-			}
-			if (m_rfSource != NULL && m_rfDest == NULL) {
-				m_display->writeFusion((char*)m_rfSource, "??????????", "R", "          ");
-				LogMessage("YSF, received RF data from %10.10s to ??????????", m_rfSource);
-			}
-			if (m_rfSource == NULL && m_rfDest != NULL) {
-				m_display->writeFusion("??????????", (char*)m_rfDest, "R", "          ");
-				LogMessage("YSF, received RF data from ?????????? to %10.10s", m_rfDest);
-			}
+		if (fn != 0U || ft != 1U) {
+			// The first packet after the header is odd, don't try and regenerate it
+			unsigned int errors = m_rfPayload.processVoiceFRModeAudio(data + 2U);
+			m_rfErrs += errors;
+			m_rfBits += 720U;
+			m_display->writeFusionBER(float(errors) / 7.2F);
+			LogDebug("YSF, V Mode 3, seq %u, AMBE FEC %u/720 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 7.2F);
 		}
 
 		// Remove any DSQ information
@@ -406,20 +338,403 @@ bool CYSFControl::writeModem(unsigned char *data, unsigned int len)
 		m_rfFrames++;
 
 		m_display->writeFusionRSSI(m_rssi);
-	} else {
+	}
+
+	return true;
+}
+
+bool CYSFControl::processDNData(bool valid, unsigned char *data)
+{
+	unsigned char fi = m_lastFICH.getFI();
+	if (fi == YSF_FI_HEADER) {
+		if (m_rfState != RS_RF_LISTENING)
+			return false;
+
+		bool valid = m_rfPayload.processHeaderData(data + 2U);
+		if (!valid)
+			return false;
+
 		CSync::addYSFSync(data + 2U);
 
+		m_rfSource = m_rfPayload.getSource();
+
+		unsigned char cm = m_lastFICH.getCM();
+		if (cm == YSF_CM_GROUP)
+			m_rfDest = (unsigned char*)"ALL       ";
+		else
+			m_rfDest = m_rfPayload.getDest();
+
+		m_rfFrames = 0U;
+		m_rfErrs = 0U;
+		m_rfBits = 1U;
+		m_rfTimeoutTimer.start();
+		m_rfPayload.reset();
+		m_rfState = RS_RF_AUDIO;
+
+		m_minRSSI = m_rssi;
+		m_maxRSSI = m_rssi;
+		m_aveRSSI = m_rssi;
+		m_rssiCount = 1U;
+#if defined(DUMP_YSF)
+		openFile();
+#endif
+
+		m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
+		LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
+
+		CYSFFICH fich = m_lastFICH;
+
+		// Remove any DSQ information
+		fich.setSQL(false);
+		fich.setSQ(0U);
+		fich.encode(data + 2U);
+
 		data[0U] = TAG_DATA;
 		data[1U] = 0x00U;
 
 		writeNetwork(data, m_rfFrames % 128U);
 
-		if (m_duplex)
+#if defined(DUMP_YSF)
+		writeFile(data + 2U);
+#endif
+
+		if (m_duplex) {
+			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+			fich.setDev(m_lowDeviation);
+			fich.encode(data + 2U);
 			writeQueueRF(data);
+		}
+
+		m_rfFrames++;
+
+		m_display->writeFusionRSSI(m_rssi);
+	} else if (fi == YSF_FI_TERMINATOR) {
+		if (m_rfState != RS_RF_AUDIO)
+			return false;
+
+		m_rfPayload.processHeaderData(data + 2U);
+
+		CSync::addYSFSync(data + 2U);
+
+		CYSFFICH fich = m_lastFICH;
+
+		// Remove any DSQ information
+		fich.setSQL(false);
+		fich.setSQ(0U);
+		fich.encode(data + 2U);
+
+		data[0U] = TAG_EOT;
+		data[1U] = 0x00U;
+
+		writeNetwork(data, m_rfFrames % 128U);
 
 #if defined(DUMP_YSF)
 		writeFile(data + 2U);
 #endif
+
+		if (m_duplex) {
+			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+			fich.setDev(m_lowDeviation);
+			fich.encode(data + 2U);
+			writeQueueRF(data);
+		}
+
+		m_rfFrames++;
+
+		if (m_rssi != 0U)
+			LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
+		else
+			LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits));
+
+		writeEndRF();
+
+		return false;
+	} else {
+		if (m_rfState == RS_RF_AUDIO) {
+			// XXX If valid is false, update the m_lastFICH for this transmission
+
+			CSync::addYSFSync(data + 2U);
+
+			unsigned char fn = m_lastFICH.getFN();
+			unsigned char dt = m_lastFICH.getDT();
+
+			switch (dt) {
+			case YSF_DT_VD_MODE1: {
+					m_rfPayload.processVDMode1Data(data + 2U, fn);
+					unsigned int errors = m_rfPayload.processVDMode1Audio(data + 2U);
+					m_rfErrs += errors;
+					m_rfBits += 235U;
+					m_display->writeFusionBER(float(errors) / 2.35F);
+					LogDebug("YSF, V/D Mode 1, seq %u, AMBE FEC %u/235 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 2.35F);
+				}
+				break;
+
+			case YSF_DT_VD_MODE2: {
+					m_rfPayload.processVDMode2Data(data + 2U, fn);
+					unsigned int errors = m_rfPayload.processVDMode2Audio(data + 2U);
+					m_rfErrs += errors;
+					m_rfBits += 135U;
+					m_display->writeFusionBER(float(errors) / 1.35F);
+					LogDebug("YSF, V/D Mode 2, seq %u, Repetition FEC %u/135 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 1.35F);
+				}
+				break;
+
+			default:
+				break;
+			}
+
+			CYSFFICH fich = m_lastFICH;
+
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
+			fich.encode(data + 2U);
+
+			data[0U] = TAG_DATA;
+			data[1U] = 0x00U;
+
+			writeNetwork(data, m_rfFrames % 128U);
+
+			if (m_duplex) {
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
+
+#if defined(DUMP_YSF)
+			writeFile(data + 2U);
+#endif
+
+			m_rfFrames++;
+
+			m_display->writeFusionRSSI(m_rssi);
+		} else if (valid) {
+			CSync::addYSFSync(data + 2U);
+
+			unsigned char fn = m_lastFICH.getFN();
+			unsigned char dt = m_lastFICH.getDT();
+
+			switch (dt) {
+			case YSF_DT_VD_MODE1:
+				valid = m_rfPayload.processVDMode1Data(data + 2U, fn);
+				break;
+
+			case YSF_DT_VD_MODE2:
+				valid = m_rfPayload.processVDMode2Data(data + 2U, fn);
+				break;
+
+			default:
+				break;
+			}
+
+			if (!valid)
+				return false;
+
+			if (m_rfDest == NULL) {
+				unsigned char cm = m_lastFICH.getCM();
+				if (cm == YSF_CM_GROUP)
+					m_rfDest = (unsigned char*)"ALL       ";
+				else
+					m_rfDest = m_rfPayload.getDest();
+			}
+
+			if (m_rfSource == NULL)
+				m_rfSource = m_rfPayload.getSource();
+
+			if (m_rfSource == NULL || m_rfDest == NULL)
+				return false;
+
+			m_rfFrames = 0U;
+			m_rfErrs = 0U;
+			m_rfBits = 1U;
+			m_rfTimeoutTimer.start();
+			m_rfPayload.reset();
+			m_rfState = RS_RF_AUDIO;
+
+			m_minRSSI = m_rssi;
+			m_maxRSSI = m_rssi;
+			m_aveRSSI = m_rssi;
+			m_rssiCount = 1U;
+#if defined(DUMP_YSF)
+			openFile();
+#endif
+
+			// XXX Build a new header and transmit it here
+
+			m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
+			LogMessage("YSF, received RF data from %10.10s to %10.10s", m_rfSource, m_rfDest);
+
+			CYSFFICH fich = m_lastFICH;
+
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
+			fich.encode(data + 2U);
+
+			data[0U] = TAG_DATA;
+			data[1U] = 0x00U;
+
+			writeNetwork(data, m_rfFrames % 128U);
+
+			if (m_duplex) {
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
+
+#if defined(DUMP_YSF)
+			writeFile(data + 2U);
+#endif
+
+			m_rfFrames++;
+
+			m_display->writeFusionRSSI(m_rssi);
+		}
+	}
+
+	return true;
+}
+
+bool CYSFControl::processFRData(bool valid, unsigned char *data)
+{
+	unsigned char fi = m_lastFICH.getFI();
+	if (fi == YSF_FI_HEADER) {
+		if (m_rfState != RS_RF_LISTENING)
+			return false;
+
+		valid = m_rfPayload.processHeaderData(data + 2U);
+		if (!valid)
+			return false;
+
+		CSync::addYSFSync(data + 2U);
+
+		m_rfSource = m_rfPayload.getSource();
+
+		unsigned char cm = m_lastFICH.getCM();
+		if (cm == YSF_CM_GROUP)
+			m_rfDest = (unsigned char*)"ALL       ";
+		else
+			m_rfDest = m_rfPayload.getDest();
+
+		m_rfFrames = 0U;
+		m_rfPayload.reset();
+		m_rfState = RS_RF_DATA;
+
+		m_minRSSI = m_rssi;
+		m_maxRSSI = m_rssi;
+		m_aveRSSI = m_rssi;
+		m_rssiCount = 1U;
+#if defined(DUMP_YSF)
+		openFile();
+#endif
+
+		m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
+		LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
+
+		CYSFFICH fich = m_lastFICH;
+
+		// Remove any DSQ information
+		fich.setSQL(false);
+		fich.setSQ(0U);
+		fich.encode(data + 2U);
+
+		data[0U] = TAG_DATA;
+		data[1U] = 0x00U;
+
+		writeNetwork(data, m_rfFrames % 128U);
+
+#if defined(DUMP_YSF)
+		writeFile(data + 2U);
+#endif
+
+		if (m_duplex) {
+			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+			fich.setDev(m_lowDeviation);
+			fich.encode(data + 2U);
+			writeQueueRF(data);
+		}
+
+		m_rfFrames++;
+
+		m_display->writeFusionRSSI(m_rssi);
+	} else if (fi == YSF_FI_TERMINATOR) {
+		if (m_rfState != RS_RF_DATA)
+			return false;
+
+		m_rfPayload.processHeaderData(data + 2U);
+
+		CSync::addYSFSync(data + 2U);
+
+		CYSFFICH fich = m_lastFICH;
+
+		// Remove any DSQ information
+		fich.setSQL(false);
+		fich.setSQ(0U);
+		fich.encode(data + 2U);
+
+		data[0U] = TAG_EOT;
+		data[1U] = 0x00U;
+
+		writeNetwork(data, m_rfFrames % 128U);
+
+#if defined(DUMP_YSF)
+		writeFile(data + 2U);
+#endif
+
+		if (m_duplex) {
+			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+			fich.setDev(m_lowDeviation);
+			fich.encode(data + 2U);
+			writeQueueRF(data);
+		}
+
+		m_rfFrames++;
+
+		if (m_rssi != 0U)
+			LogMessage("YSF, received RF end of transmission, %.1f seconds, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
+		else
+			LogMessage("YSF, received RF end of transmission, %.1f seconds", float(m_rfFrames) / 10.0F);
+
+		writeEndRF();
+
+		return false;
+	} else {
+		if (m_rfState != RS_RF_DATA)
+			return false;
+
+		// XXX If valid is false, update the m_lastFICH for this transmission
+
+		CSync::addYSFSync(data + 2U);
+
+		unsigned char fn = m_lastFICH.getFN();
+
+		m_rfPayload.processDataFRModeData(data + 2U, fn);
+
+		CYSFFICH fich = m_lastFICH;
+
+		// Remove any DSQ information
+		fich.setSQL(false);
+		fich.setSQ(0U);
+		fich.encode(data + 2U);
+
+		data[0U] = TAG_DATA;
+		data[1U] = 0x00U;
+
+		writeNetwork(data, m_rfFrames % 128U);
+
+		if (m_duplex) {
+			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+			fich.setDev(m_lowDeviation);
+			fich.encode(data + 2U);
+			writeQueueRF(data);
+		}
+
+#if defined(DUMP_YSF)
+		writeFile(data + 2U);
+#endif
+
 		m_rfFrames++;
 
 		m_display->writeFusionRSSI(m_rssi);
@@ -474,8 +789,6 @@ void CYSFControl::writeEndNet()
 	m_networkWatchdog.stop();
 	m_packetTimer.stop();
 
-	m_lastFrameValid = false;
-
 	m_netPayload.reset();
 
 	m_display->clearFusion();
@@ -522,7 +835,6 @@ void CYSFControl::writeNetwork()
 		m_netPayload.reset();
 		m_packetTimer.start();
 		m_elapsed.start();
-		m_lastFrameValid = false;
 		m_netState  = RS_NET_AUDIO;
 		m_netFrames = 0U;
 		m_netLost   = 0U;
@@ -572,8 +884,6 @@ void CYSFControl::writeNetwork()
 		fich.setDev(m_lowDeviation);
 		fich.encode(data + 35U);
 
-		m_lastMode = dt;
-
 		// Set the downlink callsign
 		switch (fi) {
 		case YSF_FI_HEADER:
@@ -586,22 +896,16 @@ void CYSFControl::writeNetwork()
 			case YSF_DT_VD_MODE1: {
 					m_netPayload.processVDMode1Data(data + 35U, fn, gateway);
 					unsigned int errors = m_netPayload.processVDMode1Audio(data + 35U);
-					// send = insertSilence(data + 33U, n);
-					// if (send) {
-						m_netErrs += errors;
-						m_netBits += 235U;
-					// }
+					m_netErrs += errors;
+					m_netBits += 235U;
 				}
 				break;
 
 			case YSF_DT_VD_MODE2: {
 					m_netPayload.processVDMode2Data(data + 35U, fn, gateway);
 					unsigned int errors = m_netPayload.processVDMode2Audio(data + 35U);
-					// send = insertSilence(data + 33U, n);
-					// if (send) {
-						m_netErrs += errors;
-						m_netBits += 135U;
-					// }
+					m_netErrs += errors;
+					m_netBits += 135U;
 				}
 				break;
 
@@ -613,11 +917,8 @@ void CYSFControl::writeNetwork()
 				if (fn != 0U || ft != 1U) {
 					// The first packet after the header is odd, don't try and regenerate it
 					unsigned int errors = m_netPayload.processVoiceFRModeAudio(data + 35U);
-					// send = insertSilence(data + 33U, n);
-					// if (send) {
-						m_netErrs += errors;
-						m_netBits += 720U;
-					// }
+					m_netErrs += errors;
+					m_netBits += 720U;
 				}
 				break;
 
@@ -659,27 +960,6 @@ void CYSFControl::clock(unsigned int ms)
 			writeEndNet();
 		}
 	}
-
-	/*
-	if (m_netState == RS_NET_AUDIO) {
-		m_packetTimer.clock(ms);
-
-		if (m_packetTimer.isRunning() && m_packetTimer.hasExpired()) {
-			unsigned int elapsed = m_elapsed.elapsed();
-			unsigned int frames = elapsed / YSF_FRAME_TIME;
-
-			if (frames > m_netFrames) {
-				unsigned int count = frames - m_netFrames;
-				if (count > 2U) {
-					LogDebug("YSF, lost audio for 200ms filling in, elapsed: %ums, expected: %u, received: %u", elapsed, frames, m_netFrames);
-					insertSilence(count - 1U);
-				}
-			}
-
-			m_packetTimer.start();
-		}
-	}
-	*/
 }
 
 void CYSFControl::writeQueueRF(const unsigned char *data)
@@ -777,61 +1057,3 @@ void CYSFControl::closeFile()
 		m_fp = NULL;
 	}
 }
-
-bool CYSFControl::insertSilence(const unsigned char* data, unsigned char n)
-{
-	assert(data != NULL);
-
-	// Check to see if we have any spaces to fill?
-	unsigned char newN = (m_netN + 1U) % 128U;
-	if (newN == n) {
-		// Just copy the data, nothing else to do here
-		::memcpy(m_lastFrame, data, YSF_FRAME_LENGTH_BYTES + 2U);
-		m_lastFrameValid = true;
-		return true;
-	}
-
-	LogDebug("YSF, current=%u last=%u", n, m_netN);
-
-	unsigned int count;
-	if (n > newN)
-		count = n - newN;
-	else
-		count = (128U + n) - newN;
-
-	if (count >= 4U) {
-		LogDebug("YSF, frame is out of range, count = %u", count);
-		return false;
-	}
-
-	insertSilence(count);
-
-	::memcpy(m_lastFrame, data, YSF_FRAME_LENGTH_BYTES + 2U);
-	m_lastFrameValid = true;
-
-	return true;
-}
-
-void CYSFControl::insertSilence(unsigned int count)
-{
-	// We can't meaningfully create "silent" data
-	if (m_lastMode == YSF_DT_DATA_FR_MODE)
-		return;
-
-	LogDebug("YSF, insert %u frames", count);
-
-	unsigned char n = (m_netN + 1U) % 128U;
-
-	for (unsigned int i = 0U; i < count; i++) {
-		writeQueueNet(m_lastFrame);
-
-		m_netN = n;
-
-		m_netFrames++;
-		m_netLost++;
-
-		n = (n + 1U) % 128U;
-	}
-
-	LogDebug("YSF, last=%u", m_netN);
-}
diff --git a/YSFControl.h b/YSFControl.h
index 5fb2e8a..be603ed 100644
--- a/YSFControl.h
+++ b/YSFControl.h
@@ -25,6 +25,7 @@
 #include "YSFPayload.h"
 #include "RingBuffer.h"
 #include "StopWatch.h"
+#include "YSFFICH.h"
 #include "Display.h"
 #include "Defines.h"
 #include "Timer.h"
@@ -73,12 +74,7 @@ private:
 	unsigned char*             m_rfDest;
 	unsigned char*             m_netSource;
 	unsigned char*             m_netDest;
-	unsigned char*             m_lastFrame;
-	bool                       m_lastFrameValid;
-	bool                       m_lastSQL;
-	unsigned char              m_lastSQ;
-	unsigned char              m_lastMode;
-	unsigned char              m_lastMR;
+	CYSFFICH                   m_lastFICH;
 	unsigned char              m_netN;
 	CYSFPayload                m_rfPayload;
 	CYSFPayload                m_netPayload;
@@ -90,6 +86,10 @@ private:
 	unsigned int               m_rssiCount;
 	FILE*                      m_fp;
 
+	bool processVWData(bool valid, unsigned char *data);
+	bool processDNData(bool valid, unsigned char *data);
+	bool processFRData(bool valid, unsigned char *data);
+
 	void writeQueueRF(const unsigned char* data);
 	void writeQueueNet(const unsigned char* data);
 	void writeNetwork(const unsigned char* data, unsigned int count);
@@ -101,9 +101,6 @@ private:
 	bool openFile();
 	bool writeFile(const unsigned char* data);
 	void closeFile();
-
-	bool insertSilence(const unsigned char* data, unsigned char n);
-	void insertSilence(unsigned int count);
 };
 
 #endif
diff --git a/YSFFICH.cpp b/YSFFICH.cpp
index e688016..92fd725 100644
--- a/YSFFICH.cpp
+++ b/YSFFICH.cpp
@@ -54,6 +54,14 @@ const unsigned int INTERLEAVE_TABLE[] = {
   36U, 76U, 116U, 156U, 196U,
   38U, 78U, 118U, 158U, 198U};
 
+CYSFFICH::CYSFFICH(const CYSFFICH& fich) :
+m_fich(NULL)
+{
+	m_fich = new unsigned char[6U];
+
+	::memcpy(m_fich, fich.m_fich, 6U);
+}
+
 CYSFFICH::CYSFFICH() :
 m_fich(NULL)
 {
@@ -249,3 +257,11 @@ void CYSFFICH::setSQ(unsigned char sq)
 	m_fich[3U] &= 0x80U;
 	m_fich[3U] |= sq & 0x7FU;
 }
+
+CYSFFICH& CYSFFICH::operator=(const CYSFFICH& fich)
+{
+	if (&fich != this)
+		::memcpy(m_fich, fich.m_fich, 6U);
+
+	return *this;
+}
diff --git a/YSFFICH.h b/YSFFICH.h
index 5b0a613..7dd1273 100644
--- a/YSFFICH.h
+++ b/YSFFICH.h
@@ -21,6 +21,7 @@
 
 class CYSFFICH {
 public:
+	CYSFFICH(const CYSFFICH& fich);
 	CYSFFICH();
 	~CYSFFICH();
 
@@ -46,6 +47,8 @@ public:
 	void setSQL(bool set);
 	void setSQ(unsigned char sq);
 
+	CYSFFICH& operator=(const CYSFFICH& fich);
+
 private:
 	unsigned char* m_fich;
 };

From 6d9bca00164f52a5b7c09922a7f59c41988bed80 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <jnaylor@office.igence.com>
Date: Mon, 14 Aug 2017 14:46:00 +0100
Subject: [PATCH 2/8] Re-add the DSQ to the RF data.

---
 YSFControl.cpp | 40 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/YSFControl.cpp b/YSFControl.cpp
index 9563098..a57b76d 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -242,6 +242,10 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 #endif
 
 		if (m_duplex) {
+			// Add the DSQ information.
+			fich.setSQL(m_sqlEnabled);
+			fich.setSQ(m_sqlValue);
+
 			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 			fich.setDev(m_lowDeviation);
 			fich.encode(data + 2U);
@@ -276,6 +280,10 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 #endif
 
 		if (m_duplex) {
+			// Add the DSQ information.
+			fich.setSQL(m_sqlEnabled);
+			fich.setSQ(m_sqlValue);
+
 			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 			fich.setDev(m_lowDeviation);
 			fich.encode(data + 2U);
@@ -325,6 +333,10 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 		writeNetwork(data, m_rfFrames % 128U);
 
 		if (m_duplex) {
+			// Add the DSQ information.
+			fich.setSQL(m_sqlEnabled);
+			fich.setSQ(m_sqlValue);
+
 			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 			fich.setDev(m_lowDeviation);
 			fich.encode(data + 2U);
@@ -399,6 +411,10 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 #endif
 
 		if (m_duplex) {
+			// Add the DSQ information.
+			fich.setSQL(m_sqlEnabled);
+			fich.setSQ(m_sqlValue);
+
 			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 			fich.setDev(m_lowDeviation);
 			fich.encode(data + 2U);
@@ -433,6 +449,10 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 #endif
 
 		if (m_duplex) {
+			// Add the DSQ information.
+			fich.setSQL(m_sqlEnabled);
+			fich.setSQ(m_sqlValue);
+
 			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 			fich.setDev(m_lowDeviation);
 			fich.encode(data + 2U);
@@ -496,6 +516,10 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			writeNetwork(data, m_rfFrames % 128U);
 
 			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
+
 				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 				fich.setDev(m_lowDeviation);
 				fich.encode(data + 2U);
@@ -578,6 +602,10 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			writeNetwork(data, m_rfFrames % 128U);
 
 			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
+
 				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 				fich.setDev(m_lowDeviation);
 				fich.encode(data + 2U);
@@ -650,6 +678,10 @@ bool CYSFControl::processFRData(bool valid, unsigned char *data)
 #endif
 
 		if (m_duplex) {
+			// Add the DSQ information.
+			fich.setSQL(m_sqlEnabled);
+			fich.setSQ(m_sqlValue);
+
 			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 			fich.setDev(m_lowDeviation);
 			fich.encode(data + 2U);
@@ -684,6 +716,10 @@ bool CYSFControl::processFRData(bool valid, unsigned char *data)
 #endif
 
 		if (m_duplex) {
+			// Add the DSQ information.
+			fich.setSQL(m_sqlEnabled);
+			fich.setSQ(m_sqlValue);
+
 			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 			fich.setDev(m_lowDeviation);
 			fich.encode(data + 2U);
@@ -725,6 +761,10 @@ bool CYSFControl::processFRData(bool valid, unsigned char *data)
 		writeNetwork(data, m_rfFrames % 128U);
 
 		if (m_duplex) {
+			// Add the DSQ information.
+			fich.setSQL(m_sqlEnabled);
+			fich.setSQ(m_sqlValue);
+
 			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
 			fich.setDev(m_lowDeviation);
 			fich.encode(data + 2U);

From f4beafc593b4d8cdc5d058fe0d30a2306ecdf700 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <jnaylor@office.igence.com>
Date: Mon, 14 Aug 2017 16:13:56 +0100
Subject: [PATCH 3/8] Add a dummy header when DN mode late entry occurs.

---
 YSFControl.cpp | 46 ++++++++++++++++++++++-
 YSFFICH.cpp    |  8 ++++
 YSFFICH.h      |  1 +
 YSFPayload.cpp | 99 ++++++++++++++++++++++++++++++++++++++++++++++++++
 YSFPayload.h   |  7 +++-
 5 files changed, 158 insertions(+), 3 deletions(-)

diff --git a/YSFControl.cpp b/YSFControl.cpp
index a57b76d..931d283 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -584,12 +584,54 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			openFile();
 #endif
 
-			// XXX Build a new header and transmit it here
+			// Build a new header and transmit it
+			unsigned char buffer[YSF_FRAME_LENGTH_BYTES + 2U];
+
+			CSync::addYSFSync(buffer + 2U);
+
+			CYSFFICH fich = m_lastFICH;
+			fich.setFI(YSF_FI_HEADER);
+			fich.setSQL(false);
+			fich.setSQ(0U);
+			fich.encode(buffer + 2U);
+
+			unsigned char csd1[20U], csd2[20U];
+			memcpy(csd1, m_rfSource, YSF_CALLSIGN_LENGTH);
+			memset(csd2, ' ', 20U);
+
+			unsigned char cm = fich.getCM();
+			if (cm == YSF_CM_GROUP)
+				memset(csd1 + YSF_CALLSIGN_LENGTH, '*', YSF_CALLSIGN_LENGTH);
+			else
+				memcpy(csd1 + YSF_CALLSIGN_LENGTH, m_rfDest, YSF_CALLSIGN_LENGTH);
+
+			CYSFPayload payload;
+			payload.writeHeader(buffer + 2U, csd1, csd2);
+
+			buffer[0U] = TAG_DATA;
+			buffer[1U] = 0x00U;
+
+			writeNetwork(buffer, m_rfFrames % 128U);
+
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
+
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(buffer + 2U);
+				writeQueueRF(buffer);
+			}
+
+#if defined(DUMP_YSF)
+			writeFile(buffer + 2U);
+#endif
 
 			m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
 			LogMessage("YSF, received RF data from %10.10s to %10.10s", m_rfSource, m_rfDest);
 
-			CYSFFICH fich = m_lastFICH;
+			fich = m_lastFICH;
 
 			// Remove any DSQ information
 			fich.setSQL(false);
diff --git a/YSFFICH.cpp b/YSFFICH.cpp
index 92fd725..cac13f6 100644
--- a/YSFFICH.cpp
+++ b/YSFFICH.cpp
@@ -66,6 +66,8 @@ CYSFFICH::CYSFFICH() :
 m_fich(NULL)
 {
 	m_fich  = new unsigned char[6U];
+
+	memset(m_fich, 0x00U, 6U);
 }
 
 CYSFFICH::~CYSFFICH()
@@ -222,6 +224,12 @@ unsigned char CYSFFICH::getSQ() const
 	return m_fich[3U] & 0x7FU;
 }
 
+void CYSFFICH::setFI(unsigned char fi)
+{
+	m_fich[0U] &= 0x3FU;
+	m_fich[0U] |= (fi << 6) & 0xC0U;
+}
+
 void CYSFFICH::setMR(unsigned char mr)
 {
 	m_fich[2U] &= 0xC7U;
diff --git a/YSFFICH.h b/YSFFICH.h
index 7dd1273..fa9b109 100644
--- a/YSFFICH.h
+++ b/YSFFICH.h
@@ -41,6 +41,7 @@ public:
 	bool getSQL() const;
 	unsigned char getSQ() const;
 
+	void setFI(unsigned char fi);
 	void setMR(unsigned char mr);
 	void setVoIP(bool set);
 	void setDev(bool set);
diff --git a/YSFPayload.cpp b/YSFPayload.cpp
index d64c710..41e4d17 100644
--- a/YSFPayload.cpp
+++ b/YSFPayload.cpp
@@ -816,6 +816,105 @@ unsigned int CYSFPayload::processVoiceFRModeAudio(unsigned char* data)
 	return errors;
 }
 
+void CYSFPayload::writeHeader(unsigned char* data, const unsigned char* csd1, const unsigned char* csd2)
+{
+	assert(data != NULL);
+	assert(csd1 != NULL);
+	assert(csd2 != NULL);
+
+	writeDataFRModeData1(csd1, data);
+
+	writeDataFRModeData2(csd2, data);
+}
+
+void CYSFPayload::writeDataFRModeData1(const unsigned char* dt, unsigned char* data)
+{
+	assert(dt != NULL);
+	assert(data != NULL);
+
+	data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES;
+
+	unsigned char output[25U];
+	for (unsigned int i = 0U; i < 20U; i++)
+		output[i] = dt[i] ^ WHITENING_DATA[i];
+
+	CCRC::addCCITT162(output, 22U);
+	output[22U] = 0x00U;
+
+	unsigned char convolved[45U];
+
+	CYSFConvolution conv;
+	conv.encode(output, convolved, 180U);
+
+	unsigned char bytes[45U];
+	unsigned int j = 0U;
+	for (unsigned int i = 0U; i < 180U; i++) {
+		unsigned int n = INTERLEAVE_TABLE_9_20[i];
+
+		bool s0 = READ_BIT1(convolved, j) != 0U;
+		j++;
+
+		bool s1 = READ_BIT1(convolved, j) != 0U;
+		j++;
+
+		WRITE_BIT1(bytes, n, s0);
+
+		n++;
+		WRITE_BIT1(bytes, n, s1);
+	}
+
+	unsigned char* p1 = data;
+	unsigned char* p2 = bytes;
+	for (unsigned int i = 0U; i < 5U; i++) {
+		::memcpy(p1, p2, 9U);
+		p1 += 18U; p2 += 9U;
+	}
+}
+
+void CYSFPayload::writeDataFRModeData2(const unsigned char* dt, unsigned char* data)
+{
+	assert(dt != NULL);
+	assert(data != NULL);
+
+	data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES;
+
+	unsigned char output[25U];
+	for (unsigned int i = 0U; i < 20U; i++)
+		output[i] = dt[i] ^ WHITENING_DATA[i];
+
+	CCRC::addCCITT162(output, 22U);
+	output[22U] = 0x00U;
+
+	unsigned char convolved[45U];
+
+	CYSFConvolution conv;
+	conv.encode(output, convolved, 180U);
+
+	unsigned char bytes[45U];
+	unsigned int j = 0U;
+	for (unsigned int i = 0U; i < 180U; i++) {
+		unsigned int n = INTERLEAVE_TABLE_9_20[i];
+
+		bool s0 = READ_BIT1(convolved, j) != 0U;
+		j++;
+
+		bool s1 = READ_BIT1(convolved, j) != 0U;
+		j++;
+
+		WRITE_BIT1(bytes, n, s0);
+
+		n++;
+		WRITE_BIT1(bytes, n, s1);
+	}
+
+	unsigned char* p1 = data + 9U;
+	unsigned char* p2 = bytes;
+	for (unsigned int i = 0U; i < 5U; i++) {
+		::memcpy(p1, p2, 9U);
+		p1 += 18U; p2 += 9U;
+	}
+}
+
 void CYSFPayload::setUplink(const std::string& callsign)
 {
 	m_uplink = new unsigned char[YSF_CALLSIGN_LENGTH];
diff --git a/YSFPayload.h b/YSFPayload.h
index 8fc5061..1baac99 100644
--- a/YSFPayload.h
+++ b/YSFPayload.h
@@ -1,5 +1,5 @@
 /*
-*   Copyright (C) 2016 by Jonathan Naylor G4KLX
+*   Copyright (C) 2016,2017 by Jonathan Naylor G4KLX
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
@@ -40,6 +40,11 @@ public:
 
 	unsigned int processVoiceFRModeAudio(unsigned char* bytes);
 
+	void writeHeader(unsigned char* data, const unsigned char* csd1, const unsigned char* csd2);
+
+	void writeDataFRModeData1(const unsigned char* dt, unsigned char* data);
+	void writeDataFRModeData2(const unsigned char* dt, unsigned char* data);
+
 	unsigned char* getSource();
 	unsigned char* getDest();
 

From b3c0e68b23b6a504b98c2c5138423747ada66702 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <jnaylor@office.igence.com>
Date: Mon, 14 Aug 2017 17:34:04 +0100
Subject: [PATCH 4/8] Rebuild broken FICHs.

---
 YSFControl.cpp | 50 +++++++++++++++++++++++++++++++++++++-------------
 YSFFICH.cpp    | 12 ++++++++++++
 YSFFICH.h      |  2 ++
 3 files changed, 51 insertions(+), 13 deletions(-)

diff --git a/YSFControl.cpp b/YSFControl.cpp
index 931d283..278ef71 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -189,7 +189,7 @@ bool CYSFControl::writeModem(unsigned char *data, unsigned int len)
 bool CYSFControl::processVWData(bool valid, unsigned char *data)
 {
 	unsigned char fi = m_lastFICH.getFI();
-	if (fi == YSF_FI_HEADER) {
+	if (valid && fi == YSF_FI_HEADER) {
 		if (m_rfState != RS_RF_LISTENING)
 			return false;
 
@@ -255,7 +255,7 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 		m_rfFrames++;
 
 		m_display->writeFusionRSSI(m_rssi);
-	} else if (fi == YSF_FI_TERMINATOR) {
+	} else if (valid && fi == YSF_FI_TERMINATOR) {
 		if (m_rfState != RS_RF_AUDIO)
 			return false;
 
@@ -304,7 +304,12 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 		if (m_rfState != RS_RF_AUDIO)
 			return false;
 
-		// XXX If valid is false, update the m_lastFICH for this transmission
+		// If valid is false, update the m_lastFICH for this transmission
+		if (!valid) {
+			// XXX Check these values
+			m_lastFICH.setFT(0U);
+			m_lastFICH.setFN(0U);
+		}
 
 		CSync::addYSFSync(data + 2U);
 
@@ -358,7 +363,7 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 bool CYSFControl::processDNData(bool valid, unsigned char *data)
 {
 	unsigned char fi = m_lastFICH.getFI();
-	if (fi == YSF_FI_HEADER) {
+	if (valid && fi == YSF_FI_HEADER) {
 		if (m_rfState != RS_RF_LISTENING)
 			return false;
 
@@ -424,7 +429,7 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 		m_rfFrames++;
 
 		m_display->writeFusionRSSI(m_rssi);
-	} else if (fi == YSF_FI_TERMINATOR) {
+	} else if (valid && fi == YSF_FI_TERMINATOR) {
 		if (m_rfState != RS_RF_AUDIO)
 			return false;
 
@@ -471,7 +476,16 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 		return false;
 	} else {
 		if (m_rfState == RS_RF_AUDIO) {
-			// XXX If valid is false, update the m_lastFICH for this transmission
+			// If valid is false, update the m_lastFICH for this transmission
+			if (!valid) {
+				unsigned char ft = m_lastFICH.getFT();
+				unsigned char fn = m_lastFICH.getFN() + 1U;
+
+				if (fn > ft)
+					fn = 0U;
+
+				m_lastFICH.setFN(fn);
+			}
 
 			CSync::addYSFSync(data + 2U);
 
@@ -534,6 +548,7 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 
 			m_display->writeFusionRSSI(m_rssi);
 		} else if (valid) {
+			// Only use clean frames for late entry.
 			CSync::addYSFSync(data + 2U);
 
 			unsigned char fn = m_lastFICH.getFN();
@@ -596,14 +611,14 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			fich.encode(buffer + 2U);
 
 			unsigned char csd1[20U], csd2[20U];
-			memcpy(csd1, m_rfSource, YSF_CALLSIGN_LENGTH);
-			memset(csd2, ' ', 20U);
+			memcpy(csd1 + YSF_CALLSIGN_LENGTH, m_rfSource, YSF_CALLSIGN_LENGTH);
+			memset(csd2, ' ', YSF_CALLSIGN_LENGTH + YSF_CALLSIGN_LENGTH);
 
 			unsigned char cm = fich.getCM();
 			if (cm == YSF_CM_GROUP)
-				memset(csd1 + YSF_CALLSIGN_LENGTH, '*', YSF_CALLSIGN_LENGTH);
+				memset(csd1 + 0U, '*', YSF_CALLSIGN_LENGTH);
 			else
-				memcpy(csd1 + YSF_CALLSIGN_LENGTH, m_rfDest, YSF_CALLSIGN_LENGTH);
+				memcpy(csd1 + 0U, m_rfDest, YSF_CALLSIGN_LENGTH);
 
 			CYSFPayload payload;
 			payload.writeHeader(buffer + 2U, csd1, csd2);
@@ -670,7 +685,7 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 bool CYSFControl::processFRData(bool valid, unsigned char *data)
 {
 	unsigned char fi = m_lastFICH.getFI();
-	if (fi == YSF_FI_HEADER) {
+	if (valid && fi == YSF_FI_HEADER) {
 		if (m_rfState != RS_RF_LISTENING)
 			return false;
 
@@ -733,7 +748,7 @@ bool CYSFControl::processFRData(bool valid, unsigned char *data)
 		m_rfFrames++;
 
 		m_display->writeFusionRSSI(m_rssi);
-	} else if (fi == YSF_FI_TERMINATOR) {
+	} else if (valid && fi == YSF_FI_TERMINATOR) {
 		if (m_rfState != RS_RF_DATA)
 			return false;
 
@@ -782,7 +797,16 @@ bool CYSFControl::processFRData(bool valid, unsigned char *data)
 		if (m_rfState != RS_RF_DATA)
 			return false;
 
-		// XXX If valid is false, update the m_lastFICH for this transmission
+		// If valid is false, update the m_lastFICH for this transmission
+		if (!valid) {
+			unsigned char ft = m_lastFICH.getFT();
+			unsigned char fn = m_lastFICH.getFN() + 1U;
+
+			if (fn > ft)
+				fn = 0U;
+
+			m_lastFICH.setFN(fn);
+		}
 
 		CSync::addYSFSync(data + 2U);
 
diff --git a/YSFFICH.cpp b/YSFFICH.cpp
index cac13f6..be60510 100644
--- a/YSFFICH.cpp
+++ b/YSFFICH.cpp
@@ -230,6 +230,18 @@ void CYSFFICH::setFI(unsigned char fi)
 	m_fich[0U] |= (fi << 6) & 0xC0U;
 }
 
+void CYSFFICH::setFN(unsigned char fn)
+{
+	m_fich[1U] &= 0xC7U;
+	m_fich[1U] |= (fn << 3) & 0x38U;
+}
+
+void CYSFFICH::setFT(unsigned char ft)
+{
+	m_fich[1U] &= 0xF8U;
+	m_fich[1U] |= ft & 0x07U;
+}
+
 void CYSFFICH::setMR(unsigned char mr)
 {
 	m_fich[2U] &= 0xC7U;
diff --git a/YSFFICH.h b/YSFFICH.h
index fa9b109..8a12034 100644
--- a/YSFFICH.h
+++ b/YSFFICH.h
@@ -42,6 +42,8 @@ public:
 	unsigned char getSQ() const;
 
 	void setFI(unsigned char fi);
+	void setFN(unsigned char fn);
+	void setFT(unsigned char ft);
 	void setMR(unsigned char mr);
 	void setVoIP(bool set);
 	void setDev(bool set);

From 8b2544ba85e6f11f1cdc5836b2ac5df202a4770f Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <jnaylor@office.igence.com>
Date: Tue, 15 Aug 2017 10:11:05 +0100
Subject: [PATCH 5/8] Add SelfOnly to YSF.

---
 Conf.cpp       |  8 ++++++++
 Conf.h         |  2 ++
 MMDVM.ini      |  1 +
 MMDVMHost.cpp  |  4 +++-
 YSFControl.cpp | 40 +++++++++++++++++++++++++++++++++++++++-
 YSFControl.h   |  6 +++++-
 6 files changed, 58 insertions(+), 3 deletions(-)

diff --git a/Conf.cpp b/Conf.cpp
index 25e30f6..79686dd 100644
--- a/Conf.cpp
+++ b/Conf.cpp
@@ -122,6 +122,7 @@ m_dmrTXHang(4U),
 m_fusionEnabled(false),
 m_fusionLowDeviation(false),
 m_fusionRemoteGateway(false),
+m_fusionSelfOnly(false),
 m_fusionSQLEnabled(false),
 m_fusionSQL(0U),
 m_p25Enabled(false),
@@ -459,6 +460,8 @@ bool CConf::read()
 			m_fusionSQL        = (unsigned int)::atoi(value);
 		} else if (::strcmp(key, "RemoteGateway") == 0)
 			m_fusionRemoteGateway = ::atoi(value) == 1;
+		else if (::strcmp(key, "SelfOnly") == 0)
+			m_fusionSelfOnly = ::atoi(value) == 1;
 	} else if (section == SECTION_P25) {
 		if (::strcmp(key, "Enable") == 0)
 			m_p25Enabled = ::atoi(value) == 1;
@@ -935,6 +938,11 @@ bool CConf::getFusionRemoteGateway() const
 	return m_fusionRemoteGateway;
 }
 
+bool CConf::getFusionSelfOnly() const
+{
+	return m_fusionSelfOnly;
+}
+
 bool CConf::getFusionSQLEnabled() const
 {
 	return m_fusionSQLEnabled;
diff --git a/Conf.h b/Conf.h
index b66c12d..82f34bb 100644
--- a/Conf.h
+++ b/Conf.h
@@ -117,6 +117,7 @@ public:
   bool          getFusionEnabled() const;
   bool          getFusionLowDeviation() const;
   bool          getFusionRemoteGateway() const;
+  bool          getFusionSelfOnly() const;
   bool          getFusionSQLEnabled() const;
   unsigned char getFusionSQL() const;
 
@@ -275,6 +276,7 @@ private:
   bool          m_fusionEnabled;
   bool          m_fusionLowDeviation;
   bool          m_fusionRemoteGateway;
+  bool          m_fusionSelfOnly;
   bool          m_fusionSQLEnabled;
   unsigned char m_fusionSQL;
 
diff --git a/MMDVM.ini b/MMDVM.ini
index 1ab671e..a9722be 100644
--- a/MMDVM.ini
+++ b/MMDVM.ini
@@ -86,6 +86,7 @@ TXHang=4
 [System Fusion]
 Enable=1
 LowDeviation=0
+SelfOnly=0
 #DSQ=1
 RemoteGateway=0
 
diff --git a/MMDVMHost.cpp b/MMDVMHost.cpp
index 3dae46e..9e9cd77 100644
--- a/MMDVMHost.cpp
+++ b/MMDVMHost.cpp
@@ -408,17 +408,19 @@ int CMMDVMHost::run()
 	if (m_ysfEnabled) {
 		bool lowDeviation  = m_conf.getFusionLowDeviation();
 		bool remoteGateway = m_conf.getFusionRemoteGateway();
+		bool selfOnly      = m_conf.getFusionSelfOnly();
 		bool sqlEnabled    = m_conf.getFusionSQLEnabled();
 		unsigned char sql  = m_conf.getFusionSQL();
 
 		LogInfo("YSF Parameters");
 		LogInfo("    Low Deviation: %s", lowDeviation ? "yes" : "no");
 		LogInfo("    Remote Gateway: %s", remoteGateway ? "yes" : "no");
+		LogInfo("    Self Only: %s", selfOnly ? "yes" : "no");
 		LogInfo("    DSQ: %s", sqlEnabled ? "yes" : "no");
 		if (sqlEnabled)
 			LogInfo("    DSQ Value: %u", sql);
 
-		ysf = new CYSFControl(m_callsign, m_ysfNetwork, m_display, m_timeout, m_duplex, lowDeviation, remoteGateway, rssi);
+		ysf = new CYSFControl(m_callsign, selfOnly, m_ysfNetwork, m_display, m_timeout, m_duplex, lowDeviation, remoteGateway, rssi);
 		ysf->setSQL(sqlEnabled, sql);
 	}
 
diff --git a/YSFControl.cpp b/YSFControl.cpp
index 278ef71..6cf1add 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -23,8 +23,10 @@
 
 // #define	DUMP_YSF
 
-CYSFControl::CYSFControl(const std::string& callsign, CYSFNetwork* network, CDisplay* display, unsigned int timeout, bool duplex, bool lowDeviation, bool remoteGateway, CRSSIInterpolator* rssiMapper) :
+CYSFControl::CYSFControl(const std::string& callsign, bool selfOnly, CYSFNetwork* network, CDisplay* display, unsigned int timeout, bool duplex, bool lowDeviation, bool remoteGateway, CRSSIInterpolator* rssiMapper) :
 m_callsign(NULL),
+m_selfCallsign(NULL),
+m_selfOnly(selfOnly),
 m_network(network),
 m_display(display),
 m_duplex(duplex),
@@ -81,6 +83,12 @@ m_fp(NULL)
 
 	for (unsigned int i = 0U; i < YSF_CALLSIGN_LENGTH; i++)
 		m_callsign[i] = node.at(i);
+
+	m_selfCallsign = new unsigned char[YSF_CALLSIGN_LENGTH];
+	::memset(m_selfCallsign, 0x00U, YSF_CALLSIGN_LENGTH);
+
+	for (unsigned int i = 0U; i < callsign.length(); i++)
+		m_selfCallsign[i] = callsign.at(i);
 }
 
 CYSFControl::~CYSFControl()
@@ -88,6 +96,7 @@ CYSFControl::~CYSFControl()
 	delete[] m_netSource;
 	delete[] m_netDest;
 	delete[] m_callsign;
+	delete[] m_selfCallsign;
 }
 
 void CYSFControl::setSQL(bool on, unsigned char value)
@@ -201,6 +210,12 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 
 		m_rfSource = m_rfPayload.getSource();
 
+		if (m_selfOnly) {
+			bool ret = checkCallsign(m_rfSource);
+			if (!ret)
+				return false;
+		}
+
 		unsigned char cm = m_lastFICH.getCM();
 		if (cm == YSF_CM_GROUP)
 			m_rfDest = (unsigned char*)"ALL       ";
@@ -375,6 +390,12 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 
 		m_rfSource = m_rfPayload.getSource();
 
+		if (m_selfOnly) {
+			bool ret = checkCallsign(m_rfSource);
+			if (!ret)
+				return false;
+		}
+
 		unsigned char cm = m_lastFICH.getCM();
 		if (cm == YSF_CM_GROUP)
 			m_rfDest = (unsigned char*)"ALL       ";
@@ -584,6 +605,12 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			if (m_rfSource == NULL || m_rfDest == NULL)
 				return false;
 
+			if (m_selfOnly) {
+				bool ret = checkCallsign(m_rfSource);
+				if (!ret)
+					return false;
+			}
+
 			m_rfFrames = 0U;
 			m_rfErrs = 0U;
 			m_rfBits = 1U;
@@ -697,6 +724,12 @@ bool CYSFControl::processFRData(bool valid, unsigned char *data)
 
 		m_rfSource = m_rfPayload.getSource();
 
+		if (m_selfOnly) {
+			bool ret = checkCallsign(m_rfSource);
+			if (!ret)
+				return false;
+		}
+
 		unsigned char cm = m_lastFICH.getCM();
 		if (cm == YSF_CM_GROUP)
 			m_rfDest = (unsigned char*)"ALL       ";
@@ -1163,3 +1196,8 @@ void CYSFControl::closeFile()
 		m_fp = NULL;
 	}
 }
+
+bool CYSFControl::checkCallsign(const unsigned char* callsign) const
+{
+	return ::memcmp(callsign, m_selfCallsign, ::strlen((char*)m_selfCallsign)) == 0;
+}
diff --git a/YSFControl.h b/YSFControl.h
index be603ed..a188006 100644
--- a/YSFControl.h
+++ b/YSFControl.h
@@ -35,7 +35,7 @@
 
 class CYSFControl {
 public:
-	CYSFControl(const std::string& callsign, CYSFNetwork* network, CDisplay* display, unsigned int timeout, bool duplex, bool lowDeviation, bool remoteGateway, CRSSIInterpolator* rssiMapper);
+	CYSFControl(const std::string& callsign, bool selfOnly, CYSFNetwork* network, CDisplay* display, unsigned int timeout, bool duplex, bool lowDeviation, bool remoteGateway, CRSSIInterpolator* rssiMapper);
 	~CYSFControl();
 
 	void setSQL(bool on, unsigned char value);
@@ -48,6 +48,8 @@ public:
 
 private:
 	unsigned char*             m_callsign;
+	unsigned char*             m_selfCallsign;
+	bool                       m_selfOnly;
 	CYSFNetwork*               m_network;
 	CDisplay*                  m_display;
 	bool                       m_duplex;
@@ -101,6 +103,8 @@ private:
 	bool openFile();
 	bool writeFile(const unsigned char* data);
 	void closeFile();
+
+	bool checkCallsign(const unsigned char* callsign) const;
 };
 
 #endif

From 0694a7a7c485ce0811cd022e572f8766fd6947ce Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <jnaylor@office.igence.com>
Date: Tue, 15 Aug 2017 12:48:48 +0100
Subject: [PATCH 6/8] Fix the late entry log message.

---
 YSFControl.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/YSFControl.cpp b/YSFControl.cpp
index 6cf1add..4318db2 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -671,7 +671,7 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 #endif
 
 			m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
-			LogMessage("YSF, received RF data from %10.10s to %10.10s", m_rfSource, m_rfDest);
+			LogMessage("YSF, received RF late entry from %10.10s to %10.10s", m_rfSource, m_rfDest);
 
 			fich = m_lastFICH;
 

From c30a1bd41627dc1a3b18ee68237d8688533704e9 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <jnaylor@office.igence.com>
Date: Tue, 15 Aug 2017 15:21:27 +0100
Subject: [PATCH 7/8] Add invalid access attempt logging.

---
 YSFControl.cpp | 213 ++++++++++++++++++++++++++-----------------------
 1 file changed, 115 insertions(+), 98 deletions(-)

diff --git a/YSFControl.cpp b/YSFControl.cpp
index 4318db2..9f8f075 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -120,8 +120,10 @@ bool CYSFControl::writeModem(unsigned char *data, unsigned int len)
 		return false;
 	}
 
-	if (type == TAG_LOST)
+	if (type == TAG_LOST) {
+		m_rfState = RS_RF_LISTENING;
 		return false;
+	}
 
 	// Have we got RSSI bytes on the end?
 	if (len == (YSF_FRAME_LENGTH_BYTES + 4U)) {
@@ -212,8 +214,11 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 
 		if (m_selfOnly) {
 			bool ret = checkCallsign(m_rfSource);
-			if (!ret)
+			if (!ret) {
+				LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
+				m_rfState = RS_RF_REJECTED;
 				return false;
+			}
 		}
 
 		unsigned char cm = m_lastFICH.getCM();
@@ -271,49 +276,50 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 
 		m_display->writeFusionRSSI(m_rssi);
 	} else if (valid && fi == YSF_FI_TERMINATOR) {
-		if (m_rfState != RS_RF_AUDIO)
-			return false;
+		if (m_rfState == RS_RF_REJECTED) {
+			m_rfState = RS_RF_LISTENING;
+		} else if (m_rfState == RS_RF_AUDIO) {
+			m_rfPayload.processHeaderData(data + 2U);
 
-		m_rfPayload.processHeaderData(data + 2U);
+			CSync::addYSFSync(data + 2U);
 
-		CSync::addYSFSync(data + 2U);
+			CYSFFICH fich = m_lastFICH;
 
-		CYSFFICH fich = m_lastFICH;
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
+			fich.encode(data + 2U);
 
-		// Remove any DSQ information
-		fich.setSQL(false);
-		fich.setSQ(0U);
-		fich.encode(data + 2U);
+			data[0U] = TAG_EOT;
+			data[1U] = 0x00U;
 
-		data[0U] = TAG_EOT;
-		data[1U] = 0x00U;
-
-		writeNetwork(data, m_rfFrames % 128U);
+			writeNetwork(data, m_rfFrames % 128U);
 
 #if defined(DUMP_YSF)
-		writeFile(data + 2U);
+			writeFile(data + 2U);
 #endif
 
-		if (m_duplex) {
-			// Add the DSQ information.
-			fich.setSQL(m_sqlEnabled);
-			fich.setSQ(m_sqlValue);
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
 
-			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
-			fich.setDev(m_lowDeviation);
-			fich.encode(data + 2U);
-			writeQueueRF(data);
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
+
+			m_rfFrames++;
+
+			if (m_rssi != 0U)
+				LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
+			else
+				LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits));
+
+			writeEndRF();
 		}
 
-		m_rfFrames++;
-
-		if (m_rssi != 0U)
-			LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
-		else
-			LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits));
-
-		writeEndRF();
-
 		return false;
 	} else {
 		if (m_rfState != RS_RF_AUDIO)
@@ -392,8 +398,11 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 
 		if (m_selfOnly) {
 			bool ret = checkCallsign(m_rfSource);
-			if (!ret)
+			if (!ret) {
+				LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
+				m_rfState = RS_RF_REJECTED;
 				return false;
+			}
 		}
 
 		unsigned char cm = m_lastFICH.getCM();
@@ -451,49 +460,50 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 
 		m_display->writeFusionRSSI(m_rssi);
 	} else if (valid && fi == YSF_FI_TERMINATOR) {
-		if (m_rfState != RS_RF_AUDIO)
-			return false;
+		if (m_rfState == RS_RF_REJECTED) {
+			m_rfState = RS_RF_LISTENING;
+		} else if (m_rfState == RS_RF_AUDIO) {
+			m_rfPayload.processHeaderData(data + 2U);
 
-		m_rfPayload.processHeaderData(data + 2U);
+			CSync::addYSFSync(data + 2U);
 
-		CSync::addYSFSync(data + 2U);
+			CYSFFICH fich = m_lastFICH;
 
-		CYSFFICH fich = m_lastFICH;
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
+			fich.encode(data + 2U);
 
-		// Remove any DSQ information
-		fich.setSQL(false);
-		fich.setSQ(0U);
-		fich.encode(data + 2U);
+			data[0U] = TAG_EOT;
+			data[1U] = 0x00U;
 
-		data[0U] = TAG_EOT;
-		data[1U] = 0x00U;
-
-		writeNetwork(data, m_rfFrames % 128U);
+			writeNetwork(data, m_rfFrames % 128U);
 
 #if defined(DUMP_YSF)
-		writeFile(data + 2U);
+			writeFile(data + 2U);
 #endif
 
-		if (m_duplex) {
-			// Add the DSQ information.
-			fich.setSQL(m_sqlEnabled);
-			fich.setSQ(m_sqlValue);
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
 
-			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
-			fich.setDev(m_lowDeviation);
-			fich.encode(data + 2U);
-			writeQueueRF(data);
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
+
+			m_rfFrames++;
+
+			if (m_rssi != 0U)
+				LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
+			else
+				LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits));
+
+			writeEndRF();
 		}
 
-		m_rfFrames++;
-
-		if (m_rssi != 0U)
-			LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
-		else
-			LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits));
-
-		writeEndRF();
-
 		return false;
 	} else {
 		if (m_rfState == RS_RF_AUDIO) {
@@ -607,8 +617,11 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 
 			if (m_selfOnly) {
 				bool ret = checkCallsign(m_rfSource);
-				if (!ret)
+				if (!ret) {
+					LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
+					m_rfState = RS_RF_REJECTED;
 					return false;
+				}
 			}
 
 			m_rfFrames = 0U;
@@ -726,8 +739,11 @@ bool CYSFControl::processFRData(bool valid, unsigned char *data)
 
 		if (m_selfOnly) {
 			bool ret = checkCallsign(m_rfSource);
-			if (!ret)
+			if (!ret) {
+				LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
+				m_rfState = RS_RF_REJECTED;
 				return false;
+			}
 		}
 
 		unsigned char cm = m_lastFICH.getCM();
@@ -782,49 +798,50 @@ bool CYSFControl::processFRData(bool valid, unsigned char *data)
 
 		m_display->writeFusionRSSI(m_rssi);
 	} else if (valid && fi == YSF_FI_TERMINATOR) {
-		if (m_rfState != RS_RF_DATA)
-			return false;
+		if (m_rfState == RS_RF_REJECTED) {
+			m_rfState = RS_RF_LISTENING;
+		} else if (m_rfState == RS_RF_DATA) {
+			m_rfPayload.processHeaderData(data + 2U);
 
-		m_rfPayload.processHeaderData(data + 2U);
+			CSync::addYSFSync(data + 2U);
 
-		CSync::addYSFSync(data + 2U);
+			CYSFFICH fich = m_lastFICH;
 
-		CYSFFICH fich = m_lastFICH;
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
+			fich.encode(data + 2U);
 
-		// Remove any DSQ information
-		fich.setSQL(false);
-		fich.setSQ(0U);
-		fich.encode(data + 2U);
+			data[0U] = TAG_EOT;
+			data[1U] = 0x00U;
 
-		data[0U] = TAG_EOT;
-		data[1U] = 0x00U;
-
-		writeNetwork(data, m_rfFrames % 128U);
+			writeNetwork(data, m_rfFrames % 128U);
 
 #if defined(DUMP_YSF)
-		writeFile(data + 2U);
+			writeFile(data + 2U);
 #endif
 
-		if (m_duplex) {
-			// Add the DSQ information.
-			fich.setSQL(m_sqlEnabled);
-			fich.setSQ(m_sqlValue);
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
 
-			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
-			fich.setDev(m_lowDeviation);
-			fich.encode(data + 2U);
-			writeQueueRF(data);
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
+
+			m_rfFrames++;
+
+			if (m_rssi != 0U)
+				LogMessage("YSF, received RF end of transmission, %.1f seconds, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
+			else
+				LogMessage("YSF, received RF end of transmission, %.1f seconds", float(m_rfFrames) / 10.0F);
+
+			writeEndRF();
 		}
 
-		m_rfFrames++;
-
-		if (m_rssi != 0U)
-			LogMessage("YSF, received RF end of transmission, %.1f seconds, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount);
-		else
-			LogMessage("YSF, received RF end of transmission, %.1f seconds", float(m_rfFrames) / 10.0F);
-
-		writeEndRF();
-
 		return false;
 	} else {
 		if (m_rfState != RS_RF_DATA)

From 7905a756185fc4572882bbf3606f4aa9788de2cb Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <naylorjs@yahoo.com>
Date: Sat, 19 Aug 2017 12:08:28 +0100
Subject: [PATCH 8/8] Fix various bugs in the new YSF state machine.

---
 YSFControl.cpp | 563 ++++++++++++++++++++++++-------------------------
 1 file changed, 280 insertions(+), 283 deletions(-)

diff --git a/YSFControl.cpp b/YSFControl.cpp
index 9f8f075..09fb9b2 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -121,6 +121,7 @@ bool CYSFControl::writeModem(unsigned char *data, unsigned int len)
 	}
 
 	if (type == TAG_LOST) {
+		m_rfPayload.reset();
 		m_rfState = RS_RF_LISTENING;
 		return false;
 	}
@@ -201,80 +202,80 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 {
 	unsigned char fi = m_lastFICH.getFI();
 	if (valid && fi == YSF_FI_HEADER) {
-		if (m_rfState != RS_RF_LISTENING)
-			return false;
-
-		bool valid = m_rfPayload.processHeaderData(data + 2U);
-		if (!valid)
-			return false;
-
-		CSync::addYSFSync(data + 2U);
-
-		m_rfSource = m_rfPayload.getSource();
-
-		if (m_selfOnly) {
-			bool ret = checkCallsign(m_rfSource);
-			if (!ret) {
-				LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
-				m_rfState = RS_RF_REJECTED;
+		if (m_rfState == RS_RF_LISTENING) {
+			bool valid = m_rfPayload.processHeaderData(data + 2U);
+			if (!valid)
 				return false;
+
+			m_rfSource = m_rfPayload.getSource();
+
+			if (m_selfOnly) {
+				bool ret = checkCallsign(m_rfSource);
+				if (!ret) {
+					LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
+					m_rfState = RS_RF_REJECTED;
+					return false;
+				}
 			}
-		}
 
-		unsigned char cm = m_lastFICH.getCM();
-		if (cm == YSF_CM_GROUP)
-			m_rfDest = (unsigned char*)"ALL       ";
-		else
-			m_rfDest = m_rfPayload.getDest();
+			unsigned char cm = m_lastFICH.getCM();
+			if (cm == YSF_CM_GROUP)
+				m_rfDest = (unsigned char*)"ALL       ";
+			else
+				m_rfDest = m_rfPayload.getDest();
 
-		m_rfFrames = 0U;
-		m_rfErrs = 0U;
-		m_rfBits = 1U;
-		m_rfTimeoutTimer.start();
-		m_rfPayload.reset();
-		m_rfState = RS_RF_AUDIO;
+			m_rfFrames = 0U;
+			m_rfErrs = 0U;
+			m_rfBits = 1U;
+			m_rfTimeoutTimer.start();
+			m_rfState = RS_RF_AUDIO;
 
-		m_minRSSI = m_rssi;
-		m_maxRSSI = m_rssi;
-		m_aveRSSI = m_rssi;
-		m_rssiCount = 1U;
+			m_minRSSI = m_rssi;
+			m_maxRSSI = m_rssi;
+			m_aveRSSI = m_rssi;
+			m_rssiCount = 1U;
 #if defined(DUMP_YSF)
-		openFile();
+			openFile();
 #endif
 
-		m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
-		LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
+			m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
+			LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
 
-		CYSFFICH fich = m_lastFICH;
+			CSync::addYSFSync(data + 2U);
 
-		// Remove any DSQ information
-		fich.setSQL(false);
-		fich.setSQ(0U);
-		fich.encode(data + 2U);
+			CYSFFICH fich = m_lastFICH;
 
-		data[0U] = TAG_DATA;
-		data[1U] = 0x00U;
-
-		writeNetwork(data, m_rfFrames % 128U);
-
-#if defined(DUMP_YSF)
-		writeFile(data + 2U);
-#endif
-
-		if (m_duplex) {
-			// Add the DSQ information.
-			fich.setSQL(m_sqlEnabled);
-			fich.setSQ(m_sqlValue);
-
-			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
-			fich.setDev(m_lowDeviation);
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
 			fich.encode(data + 2U);
-			writeQueueRF(data);
+
+			data[0U] = TAG_DATA;
+			data[1U] = 0x00U;
+
+			writeNetwork(data, m_rfFrames % 128U);
+
+#if defined(DUMP_YSF)
+			writeFile(data + 2U);
+#endif
+
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
+
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
+
+			m_rfFrames++;
+
+			m_display->writeFusionRSSI(m_rssi);
+
+			return true;
 		}
-
-		m_rfFrames++;
-
-		m_display->writeFusionRSSI(m_rssi);
 	} else if (valid && fi == YSF_FI_TERMINATOR) {
 		if (m_rfState == RS_RF_REJECTED) {
 			m_rfState = RS_RF_LISTENING;
@@ -319,146 +320,145 @@ bool CYSFControl::processVWData(bool valid, unsigned char *data)
 
 			writeEndRF();
 		}
-
-		return false;
 	} else {
-		if (m_rfState != RS_RF_AUDIO)
-			return false;
+		if (m_rfState == RS_RF_AUDIO) {
+			// If valid is false, update the m_lastFICH for this transmission
+			if (!valid) {
+				// XXX Check these values
+				m_lastFICH.setFT(0U);
+				m_lastFICH.setFN(0U);
+			}
 
-		// If valid is false, update the m_lastFICH for this transmission
-		if (!valid) {
-			// XXX Check these values
-			m_lastFICH.setFT(0U);
-			m_lastFICH.setFN(0U);
-		}
+			CSync::addYSFSync(data + 2U);
 
-		CSync::addYSFSync(data + 2U);
+			CYSFFICH fich = m_lastFICH;
 
-		CYSFFICH fich = m_lastFICH;
+			unsigned char fn = fich.getFN();
+			unsigned char ft = fich.getFT();
 
-		unsigned char fn = fich.getFN();
-		unsigned char ft = fich.getFT();
+			if (fn != 0U || ft != 1U) {
+				// The first packet after the header is odd, don't try and regenerate it
+				unsigned int errors = m_rfPayload.processVoiceFRModeAudio(data + 2U);
+				m_rfErrs += errors;
+				m_rfBits += 720U;
+				m_display->writeFusionBER(float(errors) / 7.2F);
+				LogDebug("YSF, V Mode 3, seq %u, AMBE FEC %u/720 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 7.2F);
+			}
 
-		if (fn != 0U || ft != 1U) {
-			// The first packet after the header is odd, don't try and regenerate it
-			unsigned int errors = m_rfPayload.processVoiceFRModeAudio(data + 2U);
-			m_rfErrs += errors;
-			m_rfBits += 720U;
-			m_display->writeFusionBER(float(errors) / 7.2F);
-			LogDebug("YSF, V Mode 3, seq %u, AMBE FEC %u/720 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 7.2F);
-		}
-
-		// Remove any DSQ information
-		fich.setSQL(false);
-		fich.setSQ(0U);
-		fich.encode(data + 2U);
-
-		data[0U] = TAG_DATA;
-		data[1U] = 0x00U;
-
-		writeNetwork(data, m_rfFrames % 128U);
-
-		if (m_duplex) {
-			// Add the DSQ information.
-			fich.setSQL(m_sqlEnabled);
-			fich.setSQ(m_sqlValue);
-
-			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
-			fich.setDev(m_lowDeviation);
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
 			fich.encode(data + 2U);
-			writeQueueRF(data);
-		}
+
+			data[0U] = TAG_DATA;
+			data[1U] = 0x00U;
+
+			writeNetwork(data, m_rfFrames % 128U);
+
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
+
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
 
 #if defined(DUMP_YSF)
-		writeFile(data + 2U);
+			writeFile(data + 2U);
 #endif
 
-		m_rfFrames++;
+			m_rfFrames++;
 
-		m_display->writeFusionRSSI(m_rssi);
+			m_display->writeFusionRSSI(m_rssi);
+
+			return true;
+		}
 	}
 
-	return true;
+	return false;
 }
 
 bool CYSFControl::processDNData(bool valid, unsigned char *data)
 {
 	unsigned char fi = m_lastFICH.getFI();
 	if (valid && fi == YSF_FI_HEADER) {
-		if (m_rfState != RS_RF_LISTENING)
-			return false;
-
-		bool valid = m_rfPayload.processHeaderData(data + 2U);
-		if (!valid)
-			return false;
-
-		CSync::addYSFSync(data + 2U);
-
-		m_rfSource = m_rfPayload.getSource();
-
-		if (m_selfOnly) {
-			bool ret = checkCallsign(m_rfSource);
-			if (!ret) {
-				LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
-				m_rfState = RS_RF_REJECTED;
+		if (m_rfState == RS_RF_LISTENING) {
+			bool valid = m_rfPayload.processHeaderData(data + 2U);
+			if (!valid)
 				return false;
+
+			m_rfSource = m_rfPayload.getSource();
+
+			if (m_selfOnly) {
+				bool ret = checkCallsign(m_rfSource);
+				if (!ret) {
+					LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
+					m_rfState = RS_RF_REJECTED;
+					return false;
+				}
 			}
-		}
 
-		unsigned char cm = m_lastFICH.getCM();
-		if (cm == YSF_CM_GROUP)
-			m_rfDest = (unsigned char*)"ALL       ";
-		else
-			m_rfDest = m_rfPayload.getDest();
+			unsigned char cm = m_lastFICH.getCM();
+			if (cm == YSF_CM_GROUP)
+				m_rfDest = (unsigned char*)"ALL       ";
+			else
+				m_rfDest = m_rfPayload.getDest();
 
-		m_rfFrames = 0U;
-		m_rfErrs = 0U;
-		m_rfBits = 1U;
-		m_rfTimeoutTimer.start();
-		m_rfPayload.reset();
-		m_rfState = RS_RF_AUDIO;
+			m_rfFrames = 0U;
+			m_rfErrs = 0U;
+			m_rfBits = 1U;
+			m_rfTimeoutTimer.start();
+			m_rfState = RS_RF_AUDIO;
 
-		m_minRSSI = m_rssi;
-		m_maxRSSI = m_rssi;
-		m_aveRSSI = m_rssi;
-		m_rssiCount = 1U;
+			m_minRSSI = m_rssi;
+			m_maxRSSI = m_rssi;
+			m_aveRSSI = m_rssi;
+			m_rssiCount = 1U;
 #if defined(DUMP_YSF)
-		openFile();
+			openFile();
 #endif
 
-		m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
-		LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
+			m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
+			LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
 
-		CYSFFICH fich = m_lastFICH;
+			CSync::addYSFSync(data + 2U);
 
-		// Remove any DSQ information
-		fich.setSQL(false);
-		fich.setSQ(0U);
-		fich.encode(data + 2U);
+			CYSFFICH fich = m_lastFICH;
 
-		data[0U] = TAG_DATA;
-		data[1U] = 0x00U;
-
-		writeNetwork(data, m_rfFrames % 128U);
-
-#if defined(DUMP_YSF)
-		writeFile(data + 2U);
-#endif
-
-		if (m_duplex) {
-			// Add the DSQ information.
-			fich.setSQL(m_sqlEnabled);
-			fich.setSQ(m_sqlValue);
-
-			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
-			fich.setDev(m_lowDeviation);
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
 			fich.encode(data + 2U);
-			writeQueueRF(data);
+
+			data[0U] = TAG_DATA;
+			data[1U] = 0x00U;
+
+			writeNetwork(data, m_rfFrames % 128U);
+
+#if defined(DUMP_YSF)
+			writeFile(data + 2U);
+#endif
+
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
+
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
+
+			m_rfFrames++;
+
+			m_display->writeFusionRSSI(m_rssi);
+
+			return true;
 		}
-
-		m_rfFrames++;
-
-		m_display->writeFusionRSSI(m_rssi);
 	} else if (valid && fi == YSF_FI_TERMINATOR) {
 		if (m_rfState == RS_RF_REJECTED) {
 			m_rfState = RS_RF_LISTENING;
@@ -503,8 +503,6 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 
 			writeEndRF();
 		}
-
-		return false;
 	} else {
 		if (m_rfState == RS_RF_AUDIO) {
 			// If valid is false, update the m_lastFICH for this transmission
@@ -578,10 +576,10 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			m_rfFrames++;
 
 			m_display->writeFusionRSSI(m_rssi);
-		} else if (valid) {
-			// Only use clean frames for late entry.
-			CSync::addYSFSync(data + 2U);
 
+			return true;
+		} else if (valid && m_rfState == RS_RF_LISTENING) {
+			// Only use clean frames for late entry.
 			unsigned char fn = m_lastFICH.getFN();
 			unsigned char dt = m_lastFICH.getDT();
 
@@ -595,22 +593,20 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 				break;
 
 			default:
+				valid = false;
 				break;
 			}
 
 			if (!valid)
 				return false;
 
-			if (m_rfDest == NULL) {
-				unsigned char cm = m_lastFICH.getCM();
-				if (cm == YSF_CM_GROUP)
-					m_rfDest = (unsigned char*)"ALL       ";
-				else
-					m_rfDest = m_rfPayload.getDest();
-			}
+			unsigned char cm = m_lastFICH.getCM();
+			if (cm == YSF_CM_GROUP)
+				m_rfDest = (unsigned char*)"ALL       ";
+			else
+				m_rfDest = m_rfPayload.getDest();
 
-			if (m_rfSource == NULL)
-				m_rfSource = m_rfPayload.getSource();
+			m_rfSource = m_rfPayload.getSource();
 
 			if (m_rfSource == NULL || m_rfDest == NULL)
 				return false;
@@ -628,7 +624,6 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			m_rfErrs = 0U;
 			m_rfBits = 1U;
 			m_rfTimeoutTimer.start();
-			m_rfPayload.reset();
 			m_rfState = RS_RF_AUDIO;
 
 			m_minRSSI = m_rssi;
@@ -654,7 +649,6 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			memcpy(csd1 + YSF_CALLSIGN_LENGTH, m_rfSource, YSF_CALLSIGN_LENGTH);
 			memset(csd2, ' ', YSF_CALLSIGN_LENGTH + YSF_CALLSIGN_LENGTH);
 
-			unsigned char cm = fich.getCM();
 			if (cm == YSF_CM_GROUP)
 				memset(csd1 + 0U, '*', YSF_CALLSIGN_LENGTH);
 			else
@@ -686,6 +680,8 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
 			LogMessage("YSF, received RF late entry from %10.10s to %10.10s", m_rfSource, m_rfDest);
 
+			CSync::addYSFSync(data + 2U);
+
 			fich = m_lastFICH;
 
 			// Remove any DSQ information
@@ -716,87 +712,89 @@ bool CYSFControl::processDNData(bool valid, unsigned char *data)
 			m_rfFrames++;
 
 			m_display->writeFusionRSSI(m_rssi);
+
+			return true;
 		}
 	}
 
-	return true;
+	return false;
 }
 
 bool CYSFControl::processFRData(bool valid, unsigned char *data)
 {
 	unsigned char fi = m_lastFICH.getFI();
 	if (valid && fi == YSF_FI_HEADER) {
-		if (m_rfState != RS_RF_LISTENING)
-			return false;
-
-		valid = m_rfPayload.processHeaderData(data + 2U);
-		if (!valid)
-			return false;
-
-		CSync::addYSFSync(data + 2U);
-
-		m_rfSource = m_rfPayload.getSource();
-
-		if (m_selfOnly) {
-			bool ret = checkCallsign(m_rfSource);
-			if (!ret) {
-				LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
-				m_rfState = RS_RF_REJECTED;
+		if (m_rfState == RS_RF_LISTENING) {
+			valid = m_rfPayload.processHeaderData(data + 2U);
+			if (!valid)
 				return false;
+
+			m_rfSource = m_rfPayload.getSource();
+
+			if (m_selfOnly) {
+				bool ret = checkCallsign(m_rfSource);
+				if (!ret) {
+					LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource);
+					m_rfState = RS_RF_REJECTED;
+					return false;
+				}
 			}
-		}
 
-		unsigned char cm = m_lastFICH.getCM();
-		if (cm == YSF_CM_GROUP)
-			m_rfDest = (unsigned char*)"ALL       ";
-		else
-			m_rfDest = m_rfPayload.getDest();
+			unsigned char cm = m_lastFICH.getCM();
+			if (cm == YSF_CM_GROUP)
+				m_rfDest = (unsigned char*)"ALL       ";
+			else
+				m_rfDest = m_rfPayload.getDest();
 
-		m_rfFrames = 0U;
-		m_rfPayload.reset();
-		m_rfState = RS_RF_DATA;
+			m_rfFrames = 0U;
+			m_rfState = RS_RF_DATA;
 
-		m_minRSSI = m_rssi;
-		m_maxRSSI = m_rssi;
-		m_aveRSSI = m_rssi;
-		m_rssiCount = 1U;
+			m_minRSSI = m_rssi;
+			m_maxRSSI = m_rssi;
+			m_aveRSSI = m_rssi;
+			m_rssiCount = 1U;
 #if defined(DUMP_YSF)
-		openFile();
+			openFile();
 #endif
 
-		m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
-		LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
+			m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", "          ");
+			LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest);
 
-		CYSFFICH fich = m_lastFICH;
+			CSync::addYSFSync(data + 2U);
 
-		// Remove any DSQ information
-		fich.setSQL(false);
-		fich.setSQ(0U);
-		fich.encode(data + 2U);
+			CYSFFICH fich = m_lastFICH;
 
-		data[0U] = TAG_DATA;
-		data[1U] = 0x00U;
-
-		writeNetwork(data, m_rfFrames % 128U);
-
-#if defined(DUMP_YSF)
-		writeFile(data + 2U);
-#endif
-
-		if (m_duplex) {
-			// Add the DSQ information.
-			fich.setSQL(m_sqlEnabled);
-			fich.setSQ(m_sqlValue);
-
-			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
-			fich.setDev(m_lowDeviation);
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
 			fich.encode(data + 2U);
-			writeQueueRF(data);
+
+			data[0U] = TAG_DATA;
+			data[1U] = 0x00U;
+
+			writeNetwork(data, m_rfFrames % 128U);
+
+#if defined(DUMP_YSF)
+			writeFile(data + 2U);
+#endif
+
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
+
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
+
+			m_rfFrames++;
+
+			m_display->writeFusionRSSI(m_rssi);
+
+			return true;
 		}
-
-		m_rfFrames++;
-
-		m_display->writeFusionRSSI(m_rssi);
 	} else if (valid && fi == YSF_FI_TERMINATOR) {
 		if (m_rfState == RS_RF_REJECTED) {
 			m_rfState = RS_RF_LISTENING;
@@ -841,62 +839,61 @@ bool CYSFControl::processFRData(bool valid, unsigned char *data)
 
 			writeEndRF();
 		}
-
-		return false;
 	} else {
-		if (m_rfState != RS_RF_DATA)
-			return false;
+		if (m_rfState == RS_RF_DATA) {
+			// If valid is false, update the m_lastFICH for this transmission
+			if (!valid) {
+				unsigned char ft = m_lastFICH.getFT();
+				unsigned char fn = m_lastFICH.getFN() + 1U;
 
-		// If valid is false, update the m_lastFICH for this transmission
-		if (!valid) {
-			unsigned char ft = m_lastFICH.getFT();
-			unsigned char fn = m_lastFICH.getFN() + 1U;
+				if (fn > ft)
+					fn = 0U;
 
-			if (fn > ft)
-				fn = 0U;
+				m_lastFICH.setFN(fn);
+			}
 
-			m_lastFICH.setFN(fn);
-		}
+			CSync::addYSFSync(data + 2U);
 
-		CSync::addYSFSync(data + 2U);
+			unsigned char fn = m_lastFICH.getFN();
 
-		unsigned char fn = m_lastFICH.getFN();
+			m_rfPayload.processDataFRModeData(data + 2U, fn);
 
-		m_rfPayload.processDataFRModeData(data + 2U, fn);
+			CYSFFICH fich = m_lastFICH;
 
-		CYSFFICH fich = m_lastFICH;
-
-		// Remove any DSQ information
-		fich.setSQL(false);
-		fich.setSQ(0U);
-		fich.encode(data + 2U);
-
-		data[0U] = TAG_DATA;
-		data[1U] = 0x00U;
-
-		writeNetwork(data, m_rfFrames % 128U);
-
-		if (m_duplex) {
-			// Add the DSQ information.
-			fich.setSQL(m_sqlEnabled);
-			fich.setSQ(m_sqlValue);
-
-			fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
-			fich.setDev(m_lowDeviation);
+			// Remove any DSQ information
+			fich.setSQL(false);
+			fich.setSQ(0U);
 			fich.encode(data + 2U);
-			writeQueueRF(data);
-		}
+
+			data[0U] = TAG_DATA;
+			data[1U] = 0x00U;
+
+			writeNetwork(data, m_rfFrames % 128U);
+
+			if (m_duplex) {
+				// Add the DSQ information.
+				fich.setSQL(m_sqlEnabled);
+				fich.setSQ(m_sqlValue);
+
+				fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY);
+				fich.setDev(m_lowDeviation);
+				fich.encode(data + 2U);
+				writeQueueRF(data);
+			}
 
 #if defined(DUMP_YSF)
-		writeFile(data + 2U);
+			writeFile(data + 2U);
 #endif
 
-		m_rfFrames++;
+			m_rfFrames++;
 
-		m_display->writeFusionRSSI(m_rssi);
+			m_display->writeFusionRSSI(m_rssi);
+
+			return true;
+		}
 	}
 
-	return true;
+	return false;
 }
 
 unsigned int CYSFControl::readModem(unsigned char* data)