diff --git a/FM.cpp b/FM.cpp
index 08e4766..fd0a4f2 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -39,8 +39,38 @@ m_hangTimer()
 {
 }
 
-void CFM::samples(bool cos, const q15_t* samples, uint8_t length)
+void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 {
+  // De-emphasis
+
+  bool ctcss = m_goertzel.process(samples, length);
+
+  bool validSignal = ctcss && cos;
+
+  stateMachine(validSignal);
+
+  if (m_modemState != STATE_FM)
+    return;
+
+  // Only let audio through when relaying audio
+  if (m_state != FS_RELAYING) {
+    for (uint8_t i = 0U; i < length; i++)
+      samples[i] = 0;
+  }
+
+  m_rfAck.getAudio(samples, length);
+
+  m_callsign.getAudio(samples, length);
+
+  m_timeoutTone.getAudio(samples, length);
+
+  // Band-pass filter
+
+  m_ctcss.getAudio(samples, length);
+
+  // Pre-emphasis
+
+  io.write(STATE_FM, samples, length);
 }
 
 void CFM::process()
@@ -79,3 +109,207 @@ void CFM::setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFrequency
   m_kerchunkTimer.setTimeout(kerchunkTime);
   m_hangTimer.setTimeout(hangTime);
 }
+
+void CFM::stateMachine(bool validSignal)
+{
+  m_callsignTimer.clock();
+  m_timeoutTimer.clock();
+  m_holdoffTimer.clock();
+  m_kerchunkTimer.clock();
+  m_ackMinTimer.clock();
+  m_ackDelayTimer.clock();
+  m_hangTimer.clock();
+
+  switch (m_state) {
+    case FS_LISTENING:
+      listeningState(validSignal);
+      break;
+    case FS_KERCHUNK:
+      kerchunkState(validSignal);
+      break;
+    case FS_RELAYING:
+      relayingState(validSignal);
+      break;
+    case FS_RELAYING_WAIT:
+      relayingWaitState(validSignal);
+      break;
+    case FS_TIMEOUT:
+      timeoutState(validSignal);
+      break;
+    case FS_TIMEOUT_WAIT:
+      timeoutWaitState(validSignal);
+      break;
+    case FS_HANG:
+      hangState(validSignal);
+      break;
+    default:
+      break;
+  }
+}
+
+void CFM::listeningState(bool validSignal)
+{
+  if (m_kerchunkTimer.getTimeout() > 0U) {
+    m_state = FS_KERCHUNK;
+    m_kerchunkTimer.start();
+  } else {
+    m_state = FS_RELAYING;
+    if (m_callsignAtStart)
+      sendCallsign();
+  }
+
+  beginRelaying();
+
+  m_callsignTimer.start();
+
+  m_modemState = STATE_FM;
+}
+
+void CFM::kerchunkState(bool validSignal)
+{
+  if (validSignal) {
+    if (m_kerchunkTimer.hasExpired()) {
+      m_state = FS_RELAYING;
+      m_kerchunkTimer.stop();
+    }
+  } else {
+    m_state = FS_LISTENING;
+    m_kerchunkTimer.stop();
+    m_timeoutTimer.stop();
+    m_ackMinTimer.stop();
+    m_callsignTimer.stop();
+    m_holdoffTimer.stop();
+    m_modemState = STATE_IDLE;
+  }
+}
+
+void CFM::relayingState(bool validSignal)
+{
+  if (validSignal) {
+    if (m_timeoutTimer.isRunning() && m_timeoutTimer.hasExpired()) {
+      m_state = FS_TIMEOUT;
+      m_ackMinTimer.stop();
+      m_timeoutTimer.stop();
+      m_timeoutTone.start();
+    }
+  } else {
+    m_state = FS_RELAYING_WAIT;
+    m_ackDelayTimer.start();
+  }
+
+  if (m_callsignTimer.isRunning() && m_callsignTimer.hasExpired()) {
+    sendCallsign();
+    m_callsignTimer.start();
+  }
+}
+
+void CFM::relayingWaitState(bool validSignal)
+{
+  if (validSignal) {
+    m_state = FS_RELAYING;
+    m_ackDelayTimer.stop();
+  } else {
+    if (m_ackDelayTimer.isRunning() && m_ackDelayTimer.hasExpired()) {
+      m_state = FS_HANG;
+
+      if (m_ackMinTimer.isRunning()) {
+        if (m_ackMinTimer.hasExpired()) {
+          m_rfAck.start();
+          m_ackMinTimer.stop();
+        }
+      } else {
+          m_rfAck.start();
+          m_ackMinTimer.stop();
+      }
+
+      m_ackDelayTimer.stop();
+      m_timeoutTimer.stop();
+      m_hangTimer.start();
+    }
+  }
+
+  if (m_callsignTimer.isRunning() && m_callsignTimer.hasExpired()) {
+    sendCallsign();
+    m_callsignTimer.start();
+  }
+}
+
+void CFM::hangState(bool validSignal)
+{
+  if (validSignal) {
+    m_state = FS_RELAYING;
+    m_rfAck.stop();
+    beginRelaying();
+  } else {
+    if (m_hangTimer.isRunning() && m_hangTimer.hasExpired()) {
+      m_state = FS_LISTENING;
+      m_hangTimer.stop();
+
+      if (m_callsignAtEnd)
+        sendCallsign();
+
+      m_callsignTimer.stop();
+      m_holdoffTimer.stop();
+      m_modemState = STATE_IDLE;
+    }
+  }
+
+  if (m_callsignTimer.isRunning() && m_callsignTimer.hasExpired()) {
+    sendCallsign();
+    m_callsignTimer.start();
+  }
+}
+
+void CFM::timeoutState(bool validSignal)
+{
+  if (!validSignal) {
+    m_state = FS_TIMEOUT_WAIT;
+    m_ackDelayTimer.start();
+  }
+
+  if (m_callsignTimer.isRunning() && m_callsignTimer.hasExpired()) {
+    sendCallsign();
+    m_callsignTimer.start();
+  }
+}
+
+void CFM::timeoutWaitState(bool validSignal)
+{
+  if (validSignal) {
+    m_state = FS_TIMEOUT;
+    m_ackDelayTimer.stop();
+  } else {
+    if (m_ackDelayTimer.isRunning() && m_ackDelayTimer.hasExpired()) {
+      m_state = FS_HANG;
+      m_timeoutTone.stop();
+      m_rfAck.start();
+      m_ackDelayTimer.stop();
+      m_ackMinTimer.stop();
+      m_timeoutTimer.stop();
+      m_hangTimer.start();
+    }
+  }
+
+  if (m_callsignTimer.isRunning() && m_callsignTimer.hasExpired()) {
+    sendCallsign();
+    m_callsignTimer.start();
+  }
+}
+
+void CFM::sendCallsign()
+{
+  if (m_holdoffTimer.isRunning()) {
+    if (m_holdoffTimer.hasExpired()) {
+      m_callsign.start();
+      m_holdoffTimer.start();
+    }
+  } else {
+    m_callsign.start();
+  }
+}
+
+void CFM::beginRelaying()
+{
+  m_timeoutTimer.start();
+  m_ackMinTimer.start();
+}
diff --git a/FM.h b/FM.h
index 2ee6aa8..fdc38a7 100644
--- a/FM.h
+++ b/FM.h
@@ -30,10 +30,10 @@
 enum FM_STATE {
   FS_LISTENING,
   FS_KERCHUNK,
-  FS_RELAYING_RF,
-  FS_RELAYING_WAIT_RF,
-  FS_TIMEOUT_RF,
-  FS_TIMEOUT_WAIT_RF,
+  FS_RELAYING,
+  FS_RELAYING_WAIT,
+  FS_TIMEOUT,
+  FS_TIMEOUT_WAIT,
   FS_HANG
 };
 
@@ -41,7 +41,7 @@ class CFM {
 public:
   CFM();
 
-  void samples(bool cos, const q15_t* samples, uint8_t length);
+  void samples(bool cos, q15_t* samples, uint8_t length);
 
   void process();
 
@@ -67,6 +67,18 @@ private:
   CFMTimer    m_ackMinTimer;
   CFMTimer    m_ackDelayTimer;
   CFMTimer    m_hangTimer;
+
+  void stateMachine(bool validSignal);
+  void listeningState(bool validSignal);
+  void kerchunkState(bool validSignal);
+  void relayingState(bool validSignal);
+  void relayingWaitState(bool validSignal);
+  void timeoutState(bool validSignal);
+  void timeoutWaitState(bool validSignal);
+  void hangState(bool validSignal);
+
+  void sendCallsign();
+  void beginRelaying();
 };
 
 #endif
diff --git a/FMTimeout.cpp b/FMTimeout.cpp
index 06cf1ee..c0bcea0 100644
--- a/FMTimeout.cpp
+++ b/FMTimeout.cpp
@@ -21,7 +21,8 @@
 #include "FMTimeout.h"
 
 CFMTimeout::CFMTimeout() :
-m_level(128 * 128)
+m_level(128 * 128),
+m_running(false)
 {
 }
 
@@ -33,3 +34,13 @@ void CFMTimeout::setParams(uint8_t level)
 void CFMTimeout::getAudio(q15_t* samples, uint8_t length)
 {
 }
+
+void CFMTimeout::start()
+{
+  m_running = true;
+}
+
+void CFMTimeout::stop()
+{
+  m_running = false;
+}
diff --git a/FMTimeout.h b/FMTimeout.h
index 48c179a..c89b80b 100644
--- a/FMTimeout.h
+++ b/FMTimeout.h
@@ -27,10 +27,14 @@ public:
 
   void setParams(uint8_t level);
 
+  void start();
+  void stop();
+
   void getAudio(q15_t* samples, uint8_t length);
 
 private:
   q15_t m_level;
+  bool  m_running;
 };
 
 #endif
diff --git a/FMTimer.cpp b/FMTimer.cpp
index b839db4..4711b8f 100644
--- a/FMTimer.cpp
+++ b/FMTimer.cpp
@@ -20,12 +20,19 @@
 #include "Globals.h"
 #include "FMTimer.h"
 
-CFMTimer::CFMTimer()
+CFMTimer::CFMTimer() :
+m_timeout(0U)
 {
 }
 
-void CFMTimer::setTimeout(uint16_t time)
+void CFMTimer::setTimeout(uint16_t timeout)
 {
+  m_timeout = timeout;
+}
+
+uint16_t CFMTimer::getTimeout() const
+{
+  return m_timeout;
 }
 
 void CFMTimer::start()
diff --git a/FMTimer.h b/FMTimer.h
index a8b91c0..1fb0911 100644
--- a/FMTimer.h
+++ b/FMTimer.h
@@ -25,7 +25,9 @@ class CFMTimer {
 public:
   CFMTimer();
 
-  void setTimeout(uint16_t time);
+  void setTimeout(uint16_t timeout);
+
+  uint16_t getTimeout() const;
 
   void start();
   
@@ -38,6 +40,7 @@ public:
   bool hasExpired() const;
 
 private:
+  uint16_t m_timeout;
 };
 
 #endif