diff --git a/DMRSlot.cpp b/DMRSlot.cpp
index de5784a..624ee06 100644
--- a/DMRSlot.cpp
+++ b/DMRSlot.cpp
@@ -85,7 +85,7 @@ void CDMRSlot::writeModem(unsigned char *data)
 		return;
 	}
 
-	if (data[0U] == TAG_LOST && m_state == RS_LATE_ENTRY) {
+	if (data[0U] == TAG_LOST) {
 		m_state = RS_LISTENING;
 		return;
 	}
diff --git a/DStarControl.cpp b/DStarControl.cpp
index b0b8527..fd02dc6 100644
--- a/DStarControl.cpp
+++ b/DStarControl.cpp
@@ -78,7 +78,7 @@ CDStarControl::~CDStarControl()
 	delete[] m_lastFrame;
 }
 
-void CDStarControl::writeModem(unsigned char *data)
+bool CDStarControl::writeModem(unsigned char *data)
 {
 	unsigned char type = data[0U];
 
@@ -87,30 +87,30 @@ void CDStarControl::writeModem(unsigned char *data)
 		LogMessage("D-Star, transmission lost, %.1f seconds, BER: %.1f%%", float(m_frames) / 50.0F, float(m_errs * 100U) / float(m_bits));
 		m_ackTimer.start();
 		writeEndOfTransmission();
-		return;
+		return false;
 	}
 
-	if (type == TAG_LOST && (m_state == RS_LATE_ENTRY || m_state == RS_RELAYING_RF_DATA)) {
+	if (type == TAG_LOST && m_state != RS_RELAYING_NETWORK_AUDIO) {
 		m_state = RS_LISTENING;
-		return;
+		return false;
 	}
 
 	if (type == TAG_HEADER) {
 		if (m_state == RS_RELAYING_RF_AUDIO)
-			return;
+			return false;
 
 		CDStarHeader header(data + 1U);
 
 		// Is this a transmission destined for a repeater?
 		if (!header.isRepeater())
-			return;
+			return false;
 
 		unsigned char callsign[DSTAR_LONG_CALLSIGN_LENGTH];
 		header.getRPTCall1(callsign);
 
 		// Is it for us?
 		if (::memcmp(callsign, m_callsign, DSTAR_LONG_CALLSIGN_LENGTH) != 0)
-			return;
+			return false;
 
 		unsigned char gateway[DSTAR_LONG_CALLSIGN_LENGTH];
 		header.getRPTCall2(gateway);
@@ -178,6 +178,8 @@ void CDStarControl::writeModem(unsigned char *data)
 			}
 
 			LogMessage("D-Star, received RF busy header from %8.8s/%4.4s to %8.8s", my1, my2, your);
+
+			return false;
 		}
 	} else if (type == TAG_EOT) {
 		if (m_state == RS_RELAYING_RF_AUDIO) {
@@ -203,6 +205,8 @@ void CDStarControl::writeModem(unsigned char *data)
 					writeNetworkData(DSTAR_END_PATTERN_BYTES, 0U, true, true);
 			}
 		}
+
+		return false;
 	} else {
 		if (m_state == RS_LISTENING) {
 			unsigned int bits = matchSync(data + 1U);
@@ -210,6 +214,8 @@ void CDStarControl::writeModem(unsigned char *data)
 				m_slowData.start();
 				m_state = RS_LATE_ENTRY;
 			}
+
+			return false;
 		} else if (m_state == RS_RELAYING_RF_AUDIO) {
 			unsigned int errors = m_fec.regenerateDStar(data + 1U);
 			m_errs += errors;
@@ -239,27 +245,29 @@ void CDStarControl::writeModem(unsigned char *data)
 
 			if (m_net)
 				writeNetworkData(data, 0U, false, true);
+
+			return false;
 		} else if (m_state == RS_LATE_ENTRY) {
 			unsigned int bits = matchSync(data + 1U);
 			if (bits <= MAX_SYNC_BIT_ERRORS) {
 				m_slowData.reset();
-				return;
+				return false;
 			}
 
 			CDStarHeader* header = m_slowData.add(data + 1U);
 			if (header == NULL)
-				return;
+				return false;
 
 			// Is this a transmission destined for a repeater?
 			if (!header->isRepeater())
-				return;
+				return false;
 
 			unsigned char callsign[DSTAR_LONG_CALLSIGN_LENGTH];
 			header->getRPTCall1(callsign);
 
 			// Is it for us?
 			if (::memcmp(callsign, m_callsign, DSTAR_LONG_CALLSIGN_LENGTH) != 0)
-				return;
+				return false;
 
 			unsigned char gateway[DSTAR_LONG_CALLSIGN_LENGTH];
 			header->getRPTCall2(gateway);
@@ -341,6 +349,8 @@ void CDStarControl::writeModem(unsigned char *data)
 			LogMessage("D-Star, received RF late entry from %8.8s/%4.4s to %8.8s", my1, my2, your);
 		}
 	}
+
+	return true;
 }
 
 unsigned int CDStarControl::readModem(unsigned char* data)
diff --git a/DStarControl.h b/DStarControl.h
index c709451..32c3b12 100644
--- a/DStarControl.h
+++ b/DStarControl.h
@@ -38,7 +38,7 @@ public:
 	CDStarControl(const std::string& callsign, const std::string& module, CDStarNetwork* network, IDisplay* display, unsigned int timeout, bool duplex);
 	~CDStarControl();
 
-	void writeModem(unsigned char* data);
+	bool writeModem(unsigned char* data);
 
 	unsigned int readModem(unsigned char* data);
 
diff --git a/MMDVMHost.cpp b/MMDVMHost.cpp
index cf2a1b5..acddefe 100644
--- a/MMDVMHost.cpp
+++ b/MMDVMHost.cpp
@@ -75,6 +75,8 @@ m_modem(NULL),
 m_dstarNetwork(NULL),
 m_dmrNetwork(NULL),
 m_display(NULL),
+m_mode(MODE_IDLE),
+m_modeTimer(1000U),
 m_dstarEnabled(false),
 m_dmrEnabled(false),
 m_ysfEnabled(false)
@@ -167,10 +169,9 @@ int CMMDVMHost::run()
 	if (m_ysfEnabled)
 		ysf = new CYSFEcho(2U, 10000U);
 
-	unsigned char mode = MODE_IDLE;
-	CTimer modeTimer(1000U, m_conf.getModeHang());
+	m_modeTimer.setTimeout(m_conf.getModeHang());
 
-	m_display->setIdle();
+	setMode(MODE_IDLE);
 
 	while (!m_killed) {
 		unsigned char data[200U];
@@ -179,103 +180,68 @@ int CMMDVMHost::run()
 
 		len = m_modem->readDStarData(data);
 		if (dstar != NULL && len > 0U) {
-			if (mode == MODE_IDLE)
+			if (m_mode == MODE_IDLE) {
+				bool ret = dstar->writeModem(data);
+				if (ret)
+					setMode(MODE_DSTAR);
+			} else if (m_mode == MODE_DSTAR) {
 				dstar->writeModem(data);
-
-			if (mode == MODE_DSTAR) {
-				dstar->writeModem(data);
-				modeTimer.start();
+				m_modeTimer.start();
 			}
 		}
 
 		len = m_modem->readDMRData1(data);
 		if (dmr != NULL && len > 0U) {
-			if (mode == MODE_IDLE) {
+			if (m_mode == MODE_IDLE) {
 				bool ret = dmr->processWakeup(data);
-				if (ret) {
-					LogMessage("Mode set to DMR");
-					mode = MODE_DMR;
-					m_display->setDMR();
-					// This sets the mode to DMR within the modem
-					m_modem->writeDMRStart(true);
-					if (m_dstarNetwork != NULL)
-						m_dstarNetwork->enable(false);
-					modeTimer.start();
-				}
-			} else if (mode == MODE_DMR) {
+				if (ret)
+					setMode(MODE_DMR);
+			} else if (m_mode == MODE_DMR) {
 				dmr->writeModemSlot1(data);
 				dmrBeaconTimer.stop();
-				modeTimer.start();
+				m_modeTimer.start();
 			}
 		}
 
 		len = m_modem->readDMRData2(data);
 		if (dmr != NULL && len > 0U) {
-			if (mode == MODE_IDLE) {
+			if (m_mode == MODE_IDLE) {
 				bool ret = dmr->processWakeup(data);
-				if (ret) {
-					LogMessage("Mode set to DMR");
-					mode = MODE_DMR;
-					m_display->setDMR();
-					// This sets the mode to DMR within the modem
-					m_modem->writeDMRStart(true);
-					if (m_dstarNetwork != NULL)
-						m_dstarNetwork->enable(false);
-					modeTimer.start();
-				}
-			} else if (mode == MODE_DMR) {
+				if (ret)
+					setMode(MODE_DMR);
+			} else if (m_mode == MODE_DMR) {
 				dmr->writeModemSlot2(data);
 				dmrBeaconTimer.stop();
-				modeTimer.start();
+				m_modeTimer.start();
 			}
 		}
 
 		len = m_modem->readYSFData(data);
 		if (ysf != NULL && len > 0U) {
-			if (mode == MODE_IDLE)
+			if (m_mode == MODE_IDLE) {
+				bool ret = ysf->writeData(data, len);
+				if (ret)
+					setMode(MODE_YSF);
+			} else if (m_mode == MODE_YSF) {
 				ysf->writeData(data, len);
-
-			if (mode == MODE_YSF) {
-				ysf->writeData(data, len);
-				modeTimer.start();
+				m_modeTimer.start();
 			}
 		}
 
-		if (modeTimer.isRunning() && modeTimer.hasExpired()) {
-			LogMessage("Mode set to Idle");
-
-			if (mode == MODE_DMR)
-				m_modem->writeDMRStart(false);
-
-			mode = MODE_IDLE;
-			m_display->setIdle();
-			m_modem->setMode(MODE_IDLE);
-
-			if (m_dmrNetwork != NULL)
-				m_dmrNetwork->enable(true);
-			if (m_dstarNetwork != NULL)
-				m_dstarNetwork->enable(true);
-
-			modeTimer.stop();
-		}
+		if (m_modeTimer.isRunning() && m_modeTimer.hasExpired())
+			setMode(MODE_IDLE);
 
 		if (dstar != NULL) {
 			ret = m_modem->hasDStarSpace();
 			if (ret) {
 				len = dstar->readModem(data);
 
-				if (len > 0U && mode == MODE_IDLE) {
-					LogMessage("Mode set to D-Star");
-					mode = MODE_DSTAR;
-					m_display->setDStar();
-					m_modem->setMode(MODE_DSTAR);
-					if (m_dmrNetwork != NULL)
-						m_dmrNetwork->enable(false);
-				}
+				if (len > 0U && m_mode == MODE_IDLE)
+					setMode(MODE_DSTAR);
 
-				if (len > 0U && mode == MODE_DSTAR) {
+				if (len > 0U && m_mode == MODE_DSTAR) {
 					m_modem->writeDStarData(data, len);
-					modeTimer.start();
+					m_modeTimer.start();
 				}
 			}
 		}
@@ -285,19 +251,13 @@ int CMMDVMHost::run()
 			if (ret) {
 				len = dmr->readModemSlot1(data);
 
-				if (len > 0U && mode == MODE_IDLE) {
-					LogMessage("Mode set to DMR");
-					m_modem->setMode(MODE_DMR);
-					m_display->setDMR();
-					mode = MODE_DMR;
-					if (m_dstarNetwork != NULL)
-						m_dstarNetwork->enable(false);
-				}
+				if (len > 0U && m_mode == MODE_IDLE)
+					setMode(MODE_DMR);
 
-				if (len > 0U && mode == MODE_DMR) {
+				if (len > 0U && m_mode == MODE_DMR) {
 					m_modem->writeDMRData1(data, len);
 					dmrBeaconTimer.stop();
-					modeTimer.start();
+					m_modeTimer.start();
 				}
 			}
 
@@ -305,19 +265,13 @@ int CMMDVMHost::run()
 			if (ret) {
 				len = dmr->readModemSlot2(data);
 
-				if (len > 0U && mode == MODE_IDLE) {
-					LogMessage("Mode set to DMR");
-					m_modem->setMode(MODE_DMR);
-					m_display->setDMR();
-					mode = MODE_DMR;
-					if (m_dstarNetwork != NULL)
-						m_dstarNetwork->enable(false);
-				}
+				if (len > 0U && m_mode == MODE_IDLE)
+					setMode(MODE_DMR);
 
-				if (len > 0U && mode == MODE_DMR) {
+				if (len > 0U && m_mode == MODE_DMR) {
 					m_modem->writeDMRData2(data, len);
 					dmrBeaconTimer.stop();
-					modeTimer.start();
+					m_modeTimer.start();
 				}
 			}
 		}
@@ -327,20 +281,12 @@ int CMMDVMHost::run()
 			if (ret) {
 				len = ysf->readData(data);
 
-				if (len > 0U && mode == MODE_IDLE) {
-					LogMessage("Mode set to System Fusion");
-					mode = MODE_YSF;
-					m_display->setFusion();
-					m_modem->setMode(MODE_YSF);
-					if (m_dmrNetwork != NULL)
-						m_dmrNetwork->enable(false);
-					if (m_dstarNetwork != NULL)
-						m_dstarNetwork->enable(false);
-				}
+				if (len > 0U && m_mode == MODE_IDLE)
+					setMode(MODE_YSF);
 
-				if (len > 0U && mode == MODE_YSF) {
+				if (len > 0U && m_mode == MODE_YSF) {
 					m_modem->writeYSFData(data, len);
-					modeTimer.start();
+					m_modeTimer.start();
 				}
 			}
 		}
@@ -348,8 +294,8 @@ int CMMDVMHost::run()
 		if (m_dmrNetwork != NULL) {
 			bool run = m_dmrNetwork->wantsBeacon();
 
-			if (dmrBeaconsEnabled && run && mode == MODE_IDLE) {
-				mode = MODE_DMR;
+			if (dmrBeaconsEnabled && run && m_mode == MODE_IDLE) {
+				m_mode = MODE_DMR;
 				m_modem->writeDMRStart(true);
 				dmrBeaconTimer.start();
 			}
@@ -357,7 +303,7 @@ int CMMDVMHost::run()
 
 		unsigned int ms = stopWatch.elapsed();
 		m_modem->clock(ms);
-		modeTimer.clock(ms);
+		m_modeTimer.clock(ms);
 		if (dstar != NULL)
 			dstar->clock(ms);
 		if (dmr != NULL)
@@ -374,7 +320,7 @@ int CMMDVMHost::run()
 		if (dmrBeaconTimer.isRunning() && dmrBeaconTimer.hasExpired()) {
 			dmrBeaconTimer.stop();
 			m_modem->writeDMRStart(false);
-			mode = MODE_IDLE;
+			m_mode = MODE_IDLE;
 		}
 
 		if (ms < 5U) {
@@ -388,7 +334,7 @@ int CMMDVMHost::run()
 
 	LogMessage("MMDVMHost is exiting on receipt of SIGHUP1");
 
-	m_display->setIdle();
+	setMode(MODE_IDLE);
 
 	m_modem->close();
 	delete m_modem;
@@ -561,3 +507,57 @@ void CMMDVMHost::createDisplay()
 		m_display = new CNullDisplay;
 	}
 }
+
+void CMMDVMHost::setMode(unsigned char mode)
+{
+	assert(m_modem != NULL);
+	assert(m_display != NULL);
+
+	switch (mode) {
+	case MODE_DSTAR:
+		LogMessage("Mode set to D-Star");
+		if (m_dmrNetwork != NULL)
+			m_dmrNetwork->enable(false);
+		m_display->setDStar();
+		m_modem->setMode(MODE_DSTAR);
+		m_mode = MODE_DSTAR;
+		m_modeTimer.start();
+		break;
+
+	case MODE_DMR:
+		LogMessage("Mode set to DMR");
+		if (m_dstarNetwork != NULL)
+			m_dstarNetwork->enable(false);
+		m_display->setDMR();
+		m_modem->writeDMRStart(true);
+		m_mode = MODE_DMR;
+		m_modeTimer.start();
+		break;
+
+	case MODE_YSF:
+		LogMessage("Mode set to System Fusion");
+		if (m_dstarNetwork != NULL)
+			m_dstarNetwork->enable(false);
+		if (m_dmrNetwork != NULL)
+			m_dmrNetwork->enable(false);
+		m_display->setFusion();
+		m_modem->setMode(MODE_YSF);
+		m_mode = MODE_YSF;
+		m_modeTimer.start();
+		break;
+
+	default:
+		LogMessage("Mode set to Idle");
+		if (m_dstarNetwork != NULL)
+			m_dstarNetwork->enable(true);
+		if (m_dmrNetwork != NULL)
+			m_dmrNetwork->enable(true);
+		if (m_mode == MODE_DMR)
+			m_modem->writeDMRStart(false);
+		m_display->setIdle();
+		m_modem->setMode(MODE_IDLE);
+		m_mode = MODE_IDLE;
+		m_modeTimer.stop();
+		break;
+	}
+}
diff --git a/MMDVMHost.h b/MMDVMHost.h
index ed76697..bb5e412 100644
--- a/MMDVMHost.h
+++ b/MMDVMHost.h
@@ -22,6 +22,7 @@
 #include "HomebrewDMRIPSC.h"
 #include "DStarNetwork.h"
 #include "Display.h"
+#include "Timer.h"
 #include "Modem.h"
 #include "Conf.h"
 
@@ -41,6 +42,8 @@ private:
   CDStarNetwork*    m_dstarNetwork;
   CHomebrewDMRIPSC* m_dmrNetwork;
   IDisplay*         m_display;
+  unsigned char     m_mode;
+  CTimer            m_modeTimer;
   bool              m_dstarEnabled;
   bool              m_dmrEnabled;
   bool              m_ysfEnabled;
@@ -50,6 +53,8 @@ private:
   bool createDStarNetwork();
   bool createDMRNetwork();
   void createDisplay();
+
+  void setMode(unsigned char mode);
 };
 
 #endif
diff --git a/Makefile b/Makefile
index 75d10f5..33e9876 100644
--- a/Makefile
+++ b/Makefile
@@ -88,7 +88,7 @@ Log.o:	Log.cpp Log.h
 		$(CC) $(CFLAGS) -c Log.cpp
 
 MMDVMHost.o:	MMDVMHost.cpp MMDVMHost.h Conf.h Log.h Version.h Modem.h StopWatch.h Defines.h DMRSync.h DStarControl.h YSFEcho.h DMRControl.h HomebrewDMRIPSC.h \
-							Display.h TFTSerial.h NullDisplay.h DStarNetwork.h
+							Display.h TFTSerial.h NullDisplay.h DStarNetwork.h Timer.h
 		$(CC) $(CFLAGS) -c MMDVMHost.cpp
 
 Modem.o:	Modem.cpp Modem.h Log.h SerialController.h Timer.h RingBuffer.h Utils.o DMRDefines.h DStarDefines.h YSFDefines.h Defines.h