diff --git a/DMRDMORX.cpp b/DMRDMORX.cpp
index 90003f1..5eefa23 100644
--- a/DMRDMORX.cpp
+++ b/DMRDMORX.cpp
@@ -76,17 +76,17 @@ void CDMRDMORX::reset()
   m_rssiCount = 0U;
 }
 
-void CDMRDMORX::samples(const q15_t* samples, uint8_t length)
+void CDMRDMORX::samples(const q15_t* samples, const uint16_t* rssi, uint8_t length)
 {
   bool dcd = false;
 
   for (uint8_t i = 0U; i < length; i++)
-    dcd = processSample(samples[i]);
+    dcd = processSample(samples[i], rssi[i]);
 
   io.setDecode(dcd);
 }
 
-bool CDMRDMORX::processSample(q15_t sample)
+bool CDMRDMORX::processSample(q15_t sample, uint16_t rssi)
 {
   m_buffer[m_dataPtr] = sample;
 
@@ -97,11 +97,10 @@ bool CDMRDMORX::processSample(q15_t sample)
   if (m_state == DMORXS_NONE) {
     correlateSync(true);
   } else {
-#if defined(SEND_RSSI_DATA)
     // Grab the RSSI data during the frame
-    if (m_state == DMORXS_VOICE && m_dataPtr == m_syncPtr && m_rssiCount == 2U)
-      m_rssi = io.getRSSIValue();
-#endif
+    if (m_state == DMORXS_VOICE && m_dataPtr == m_syncPtr)
+      m_rssi = rssi;
+
     uint16_t min  = m_syncPtr + DMO_BUFFER_LENGTH_SAMPLES - 1U;
     uint16_t max  = m_syncPtr + 1U;
 
diff --git a/DMRDMORX.h b/DMRDMORX.h
index 138333d..1049fd1 100644
--- a/DMRDMORX.h
+++ b/DMRDMORX.h
@@ -34,7 +34,7 @@ class CDMRDMORX {
 public:
   CDMRDMORX();
 
-  void samples(const q15_t* samples, uint8_t length);
+  void samples(const q15_t* samples, const uint16_t* rssi, uint8_t length);
 
   void setColorCode(uint8_t colorCode);
 
@@ -61,7 +61,7 @@ private:
   uint16_t    m_rssiCount;
   uint16_t    m_rssi;
 
-  bool processSample(q15_t sample);
+  bool processSample(q15_t sample, uint16_t rssi);
   void correlateSync(bool first);
   void samplesToBits(uint16_t start, uint8_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold);
 };
diff --git a/DMRRX.cpp b/DMRRX.cpp
index 9137b94..64320e8 100644
--- a/DMRRX.cpp
+++ b/DMRRX.cpp
@@ -26,7 +26,7 @@ m_slot2RX(true)
 {
 }
 
-void CDMRRX::samples(const q15_t* samples, const uint8_t* control, uint8_t length)
+void CDMRRX::samples(const q15_t* samples, const uint16_t* rssi, const uint8_t* control, uint8_t length)
 {
   bool dcd1 = false;
   bool dcd2 = false;
@@ -43,8 +43,8 @@ void CDMRRX::samples(const q15_t* samples, const uint8_t* control, uint8_t lengt
         break;
     }
 
-    dcd1 = m_slot1RX.processSample(samples[i]);
-    dcd2 = m_slot2RX.processSample(samples[i]);
+    dcd1 = m_slot1RX.processSample(samples[i], rssi[i]);
+    dcd2 = m_slot2RX.processSample(samples[i], rssi[i]);
   }
 
   io.setDecode(dcd1 || dcd2);
diff --git a/DMRRX.h b/DMRRX.h
index 2f8810d..6c99cd0 100644
--- a/DMRRX.h
+++ b/DMRRX.h
@@ -26,7 +26,7 @@ class CDMRRX {
 public:
   CDMRRX();
 
-  void samples(const q15_t* samples, const uint8_t* control, uint8_t length);
+  void samples(const q15_t* samples, const uint16_t* rssi, const uint8_t* control, uint8_t length);
 
   void setColorCode(uint8_t colorCode);
   void setDelay(uint8_t delay);
diff --git a/DMRSlotRX.cpp b/DMRSlotRX.cpp
index 58ad5d4..6abe38d 100644
--- a/DMRSlotRX.cpp
+++ b/DMRSlotRX.cpp
@@ -94,7 +94,7 @@ void CDMRSlotRX::reset()
   m_rssiCount = 0U;
 }
 
-bool CDMRSlotRX::processSample(q15_t sample)
+bool CDMRSlotRX::processSample(q15_t sample, uint16_t rssi)
 {
   m_delayPtr++;
   if (m_delayPtr < m_delay)
@@ -114,11 +114,10 @@ bool CDMRSlotRX::processSample(q15_t sample)
     if (m_dataPtr >= SCAN_START && m_dataPtr <= SCAN_END)
       correlateSync(true);
   } else {
-#if defined(SEND_RSSI_DATA)
     // Grab the RSSI data during the frame
-    if (m_state == DMRRXS_VOICE && m_dataPtr == m_startPtr && m_rssiCount == 2U)
-      m_rssi = io.getRSSIValue();
-#endif
+    if (m_state == DMRRXS_VOICE && m_dataPtr == m_syncPtr)
+      m_rssi = rssi;
+
     uint16_t min = m_syncPtr - 1U;
     uint16_t max = m_syncPtr + 1U;
     if (m_dataPtr >= min && m_dataPtr <= max)
diff --git a/DMRSlotRX.h b/DMRSlotRX.h
index 12482d0..7be5552 100644
--- a/DMRSlotRX.h
+++ b/DMRSlotRX.h
@@ -34,7 +34,7 @@ public:
 
   void start();
 
-  bool processSample(q15_t sample);
+  bool processSample(q15_t sample, uint16_t rssi);
 
   void setColorCode(uint8_t colorCode);
   void setDelay(uint8_t delay);
diff --git a/IO.cpp b/IO.cpp
index 1599592..233c950 100644
--- a/IO.cpp
+++ b/IO.cpp
@@ -101,6 +101,7 @@ CIO::CIO() :
 m_started(false),
 m_rxBuffer(RX_RINGBUFFER_SIZE),
 m_txBuffer(TX_RINGBUFFER_SIZE),
+m_rssiBuffer(RX_RINGBUFFER_SIZE),
 m_C4FSKFilter(),
 m_GMSKFilter(),
 m_C4FSKState(),
@@ -262,14 +263,16 @@ void CIO::process()
   }
 
   if (m_rxBuffer.getData() >= RX_BLOCK_SIZE) {
-    q15_t   samples[RX_BLOCK_SIZE + 1U];
-    uint8_t control[RX_BLOCK_SIZE + 1U];
+    q15_t    samples[RX_BLOCK_SIZE + 1U];
+    uint8_t  control[RX_BLOCK_SIZE + 1U];
+    uint16_t rssi[RX_BLOCK_SIZE + 1U];
 
     uint8_t blockSize = RX_BLOCK_SIZE;
 
     for (uint16_t i = 0U; i < RX_BLOCK_SIZE; i++) {
       uint16_t sample;
       m_rxBuffer.get(sample, control[i]);
+      m_rssiBuffer.get(rssi[i]);
 
       // Detect ADC overflow
       if (m_detect && (sample == 0U || sample == 4095U))
@@ -319,7 +322,7 @@ void CIO::process()
           if (m_duplex)
             dmrIdleRX.samples(C4FSKVals, blockSize);
           else
-            dmrDMORX.samples(C4FSKVals, blockSize);
+            dmrDMORX.samples(C4FSKVals, rssi, blockSize);
         }
 
         if (m_ysfEnable)
@@ -343,11 +346,11 @@ void CIO::process()
         if (m_duplex) {
           // If the transmitter isn't on, use the DMR idle RX to detect the wakeup CSBKs
           if (m_tx)
-            dmrRX.samples(C4FSKVals, control, blockSize);
+            dmrRX.samples(C4FSKVals, rssi, control, blockSize);
           else
             dmrIdleRX.samples(C4FSKVals, blockSize);
         } else {
-          dmrDMORX.samples(C4FSKVals, blockSize);
+          dmrDMORX.samples(C4FSKVals, rssi, blockSize);
         }
       }
     } else if (m_modemState == STATE_YSF) {
@@ -440,6 +443,7 @@ void CIO::interrupt()
 #endif
 
   m_rxBuffer.put(sample, control);
+  m_rssiBuffer.put(0U);
 
   m_watchdog++;
 }
@@ -551,9 +555,3 @@ bool CIO::hasLockout() const
   return m_lockout;
 }
 
-#if defined(SEND_RSSI_DATA)
-uint16_t CIO::getRSSIValue()
-{
-  return 0U;
-}
-#endif
diff --git a/IO.h b/IO.h
index 53321f7..a8f9c7e 100644
--- a/IO.h
+++ b/IO.h
@@ -22,6 +22,7 @@
 #include "Globals.h"
 
 #include "SampleRB.h"
+#include "RSSIRB.h"
 
 class CIO {
 public:
@@ -52,15 +53,12 @@ public:
 
   void resetWatchdog();
 
-#if defined(SEND_RSSI_DATA)
-  uint16_t getRSSIValue();
-#endif
-
 private:
   bool                 m_started;
 
   CSampleRB            m_rxBuffer;
   CSampleRB            m_txBuffer;
+  CRSSIRB              m_rssiBuffer;
 
   arm_fir_instance_q15 m_C4FSKFilter;
   arm_fir_instance_q15 m_GMSKFilter;
diff --git a/RSSIRB.cpp b/RSSIRB.cpp
new file mode 100644
index 0000000..98a519b
--- /dev/null
+++ b/RSSIRB.cpp
@@ -0,0 +1,104 @@
+/*
+TX fifo control - Copyright (C) KI6ZUM 2015
+Copyright (C) 2015,2016 by Jonathan Naylor G4KLX
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Library General Public
+License as published by the Free Software Foundation; either
+version 2 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Library General Public License for more details.
+
+You should have received a copy of the GNU Library General Public
+License along with this library; if not, write to the
+Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
+Boston, MA  02110-1301, USA.
+*/
+
+#include "RSSIRB.h"
+
+CRSSIRB::CRSSIRB(uint16_t length) :
+m_length(length),
+m_rssi(NULL),
+m_head(0U),
+m_tail(0U),
+m_full(false),
+m_overflow(false)
+{
+  m_rssi = new uint16_t[length];
+}
+
+uint16_t CRSSIRB::getSpace() const
+{
+  uint16_t n = 0U;
+
+  if (m_tail == m_head)
+    n = m_full ? 0U : m_length;
+  else if (m_tail < m_head)
+    n = m_length - m_head + m_tail;
+  else
+    n = m_tail - m_head;
+
+  if (n > m_length)
+    n = 0U;
+
+  return n;
+}
+
+uint16_t CRSSIRB::getData() const
+{
+  if (m_tail == m_head)
+    return m_full ? m_length : 0U;
+  else if (m_tail < m_head)
+    return m_head - m_tail;
+  else
+    return m_length - m_tail + m_head;
+}
+
+bool CRSSIRB::put(uint16_t rssi)
+{
+  if (m_full) {
+    m_overflow = true;
+    return false;
+  }
+
+  m_rssi[m_head] = rssi;
+
+  m_head++;
+  if (m_head >= m_length)
+    m_head = 0U;
+
+  if (m_head == m_tail)
+    m_full = true;
+
+  return true;
+}
+
+bool CRSSIRB::get(uint16_t& rssi)
+{
+  if (m_head == m_tail && !m_full)
+    return false;
+
+  rssi  = m_rssi[m_tail];
+
+  m_full = false;
+
+  m_tail++;
+  if (m_tail >= m_length)
+    m_tail = 0U;
+
+  return true;
+}
+
+bool CRSSIRB::hasOverflowed()
+{
+  bool overflow = m_overflow;
+
+  m_overflow = false;
+
+  return overflow;
+}
+
diff --git a/RSSIRB.h b/RSSIRB.h
new file mode 100644
index 0000000..41e6b8c
--- /dev/null
+++ b/RSSIRB.h
@@ -0,0 +1,50 @@
+/*
+Serial fifo control - Copyright (C) KI6ZUM 2015
+Copyright (C) 2015,2016 by Jonathan Naylor G4KLX
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Library General Public
+License as published by the Free Software Foundation; either
+version 2 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Library General Public License for more details.
+
+You should have received a copy of the GNU Library General Public
+License along with this library; if not, write to the
+Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
+Boston, MA  02110-1301, USA.
+*/
+
+#if !defined(RSSIRB_H)
+#define  RSSIRB_H
+
+#include <Arduino.h>
+
+class CRSSIRB {
+public:
+  CRSSIRB(uint16_t length);
+  
+  uint16_t getSpace() const;
+  
+  uint16_t getData() const;
+
+  bool put(uint16_t rssi);
+
+  bool get(uint16_t& rssi);
+
+  bool hasOverflowed();
+
+private:
+  uint16_t           m_length;
+  volatile uint16_t* m_rssi;
+  volatile uint16_t  m_head;
+  volatile uint16_t  m_tail;
+  volatile bool      m_full;
+  bool               m_overflow;
+};
+
+#endif
+