diff --git a/FM.cpp b/FM.cpp
index 4c8b45b..7eb7261 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -20,10 +20,8 @@
 #include "Globals.h"
 #include "FM.h"
 
-const uint8_t BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U};
-
-#define WRITE_BIT_AUDIO(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
-#define READ_BIT_AUDIO(p,i)    (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
+const uint16_t FM_TX_BLOCK_SIZE = 250U;
+const uint16_t FM_SERIAL_BLOCK_SIZE = 127U;
 
 CFM::CFM() :
 m_callsign(),
@@ -64,7 +62,7 @@ m_inputExtRB(2400U)   // 100ms of Audio
   insertDelay(100U);
 }
 
-void CFM::samples(bool cos, const q15_t* samples, uint8_t length)
+void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 {
   if (m_useCOS) {
     if (m_cosInvert)
@@ -149,19 +147,58 @@ void CFM::samples(bool cos, const q15_t* samples, uint8_t length)
     currentSample += m_ctcssTX.getAudio();
 
     m_outputRFRB.put(currentSample);
+    //samples[i] = currentSample;
   }
+
+  // XXX This relays audio correctly, no tones yet, process need to be commented
+  // if (m_state == FS_RELAYING_RF || m_state == FS_KERCHUNK_RF || m_state == FS_RELAYING_EXT || m_state == FS_KERCHUNK_EXT)
+  //   io.write(STATE_FM, samples, i);
+
 }
 
 void CFM::process()
 {
-  q15_t sample;
-  while (io.getSpace() >= 3U && m_outputRFRB.get(sample))
-    io.write(STATE_FM, &sample, 1U);
+  uint16_t space = io.getSpace() - 2U;
+  uint16_t length = m_outputRFRB.getData();
+  if (space > 2 && length >= FM_TX_BLOCK_SIZE ) {
 
-  uint8_t serialSample;
-  //write data to serial port
-  while (m_downsampler.getPackedData(serialSample))
-    serial.writeFMData(&serialSample, 1U);
+    if(length > FM_TX_BLOCK_SIZE)
+      length = FM_TX_BLOCK_SIZE;
+    if(space > FM_TX_BLOCK_SIZE)
+      space = FM_TX_BLOCK_SIZE;
+    if(length > space)
+      length = space;
+
+    q15_t samples[FM_TX_BLOCK_SIZE];
+    for(uint16_t i = 0U; i < length; i++) {
+      q15_t sample = 0;
+      m_outputRFRB.get(sample);
+      samples[i] = sample;
+    }
+
+    io.write(STATE_FM, samples, length);
+  }
+
+  // //Write audio to serial
+  // if (m_downsampler.getData() >= 127 || m_state != STATE_FM) {//if we just left FM mode, so write all what is left in buffer, regardless of the amoutn of data
+  //   uint16_t length = m_downsampler.getData();
+
+  //   if(length > FM_SERIAL_BLOCK_SIZE)//max message size on serial is 127
+  //     length = FM_SERIAL_BLOCK_SIZE;
+
+  //   if(length > FM_SERIAL_BLOCK_SIZE)
+  //     length = FM_SERIAL_BLOCK_SIZE;
+
+  //   uint8_t serialSamples[FM_SERIAL_BLOCK_SIZE];
+
+  //   for(uint16_t i = 0U; i < length; i++) {
+  //     uint8_t serialSample = 0U;
+  //     m_downsampler.getPackedData(serialSample);
+  //     serialSamples[i] = serialSample;
+  //   }
+
+  //   serial.writeFMData(serialSamples, length);
+  // }
 }
 
 void CFM::reset()
@@ -290,6 +327,11 @@ void CFM::stateMachine(bool validRFSignal, bool validExtSignal)
     default:
       break;
   }
+
+  if (m_state == FS_LISTENING) {
+    m_outputRFRB.reset();
+    m_downsampler.reset();
+  }
 }
 
 void CFM::clock(uint8_t length)
diff --git a/FM.h b/FM.h
index daef1b1..a4c82f9 100644
--- a/FM.h
+++ b/FM.h
@@ -51,7 +51,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();
 
@@ -117,6 +117,7 @@ private:
   void clock(uint8_t length);
 
   void sendCallsign();
+  void sendBeeps();
   void beginRelaying();
 
   void insertDelay(uint16_t ms);
diff --git a/FMDownsampler.cpp b/FMDownsampler.cpp
index c2c3635..3209778 100644
--- a/FMDownsampler.cpp
+++ b/FMDownsampler.cpp
@@ -28,7 +28,7 @@ m_samplePackPointer(NULL),
 m_packIndex(0U),
 m_downSampleIndex(0U)
 {
-    m_samplePackPointer = &m_samplePack;
+  m_samplePackPointer = &m_samplePack;
 }
 
 void CFMDownsampler::addSample(q15_t sample)
@@ -55,7 +55,7 @@ void CFMDownsampler::addSample(q15_t sample)
         break;
         }
         m_packIndex++;
-        if(m_packIndex >= 2U)//did we pack to samples ?
+        if(m_packIndex >= 2U)//did we pack two samples ?
             m_packIndex = 0U;  
     }
 
@@ -69,6 +69,11 @@ bool CFMDownsampler::getPackedData(uint8_t& data)
   return m_ringBuffer.get(data);
 }
 
+uint16_t CFMDownsampler::getData()
+{
+  return m_ringBuffer.getData();
+}
+
 void CFMDownsampler::reset()
 {
   m_downSampleIndex = 0;
diff --git a/FMDownsampler.h b/FMDownsampler.h
index 9b576c4..179493f 100644
--- a/FMDownsampler.h
+++ b/FMDownsampler.h
@@ -28,6 +28,7 @@ public:
   CFMDownsampler(uint16_t length);
   void addSample(q15_t sample);
   bool getPackedData(uint8_t& data);
+  uint16_t getData();
   void reset();
 
 private:
diff --git a/IO.cpp b/IO.cpp
index 7b000fb..c668f1f 100644
--- a/IO.cpp
+++ b/IO.cpp
@@ -264,6 +264,7 @@ void CIO::process()
   if (m_txBuffer.getData() == 0U && m_tx) {
     m_tx = false;
     setPTTInt(m_pttInvert ? true : false);
+    DEBUG1("Process TX OFF");
   }
 
   if (m_rxBuffer.getData() >= RX_BLOCK_SIZE) {
@@ -453,6 +454,7 @@ void CIO::write(MMDVM_STATE mode, q15_t* samples, uint16_t length, const uint8_t
   if (!m_tx) {
     m_tx = true;
     setPTTInt(m_pttInvert ? false : true);
+    DEBUG1("Write TX ON");
   }
 
   q15_t txLevel = 0;
diff --git a/Makefile.Arduino b/Makefile.Arduino
index 6046810..0afa96a 100644
--- a/Makefile.Arduino
+++ b/Makefile.Arduino
@@ -1,5 +1,5 @@
 #!/usr/bin/make
-# makefile for the arduino due (works with arduino IDE 1.6.11)
+# makefile for the arduino due (works with arduino IDE 1.6.12)
 #
 # The original file can be found at https://github.com/pauldreik/arduino-due-makefile
 #
@@ -34,7 +34,7 @@ OBJCOPY:=$(ADIR)/packages/arduino/tools/arm-none-eabi-gcc/4.8.3-2014q1/bin/arm-n
 
 C:=$(CC)
 #SAM:=arduino/sam/
-SAM:=$(ADIR)/packages/arduino/hardware/sam/1.6.11
+SAM:=$(ADIR)/packages/arduino/hardware/sam/1.6.12
 #CMSIS:=arduino/sam/system/CMSIS/
 #LIBSAM:=arduino/sam/system/libsam
 TMPDIR:=$(PWD)/build