diff --git a/DMRTX.cpp b/DMRTX.cpp
index a3b9e7c..1588dfd 100644
--- a/DMRTX.cpp
+++ b/DMRTX.cpp
@@ -218,10 +218,11 @@ void CDMRTX::writeByte(uint8_t c, uint8_t control)
     }
   }
 
-  uint8_t blockSize = DMR_RADIO_SYMBOL_LENGTH * 4U;
+  uint16_t blockSize = DMR_RADIO_SYMBOL_LENGTH * 4U;
 
   uint8_t controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U + 1U];
   ::memset(controlBuffer, MARK_NONE, (DMR_RADIO_SYMBOL_LENGTH * 4U + 1U) * sizeof(uint8_t));
+  controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 2U] = control;  
 
   // Handle the case of the oscillator not being accurate enough
   if (m_sampleCount > 0U) {
@@ -230,17 +231,18 @@ void CDMRTX::writeByte(uint8_t c, uint8_t control)
     if (m_count >= m_sampleCount) {
       if (m_sampleInsert) {
         inBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U] = inBuffer[DMR_RADIO_SYMBOL_LENGTH * 4U - 1U];
-        blockSize = DMR_RADIO_SYMBOL_LENGTH * 4U + 1U;
-        controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 2U + 1U] = control;  
+        for (int8_t i = DMR_RADIO_SYMBOL_LENGTH * 4U - 1; i >= 0; i--)
+          controlBuffer[i + 1] = controlBuffer[i];
+        blockSize++;
       } else {
-        blockSize = DMR_RADIO_SYMBOL_LENGTH * 4U - 1U;
         controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 2U - 1U] = control;  
+        for (uint8_t i = 0U; i < (DMR_RADIO_SYMBOL_LENGTH * 4U - 1U); i++)
+          controlBuffer[i] = controlBuffer[i + 1U];
+        blockSize--;
       }
 
       m_count -= m_sampleCount;
     }
-  } else {
-    controlBuffer[DMR_RADIO_SYMBOL_LENGTH * 2U] = control;  
   }
 
   ::arm_fir_fast_q15(&m_modFilter, inBuffer, outBuffer, blockSize);
diff --git a/DStarTX.cpp b/DStarTX.cpp
index 1bbf95c..6338773 100644
--- a/DStarTX.cpp
+++ b/DStarTX.cpp
@@ -424,7 +424,7 @@ void CDStarTX::writeByte(uint8_t c)
     mask <<= 1;
   }
 
-  uint8_t blockSize = DSTAR_RADIO_BIT_LENGTH * 8U;
+  uint16_t blockSize = DSTAR_RADIO_BIT_LENGTH * 8U;
 
   // Handle the case of the oscillator not being accurate enough
   if (m_sampleCount > 0U) {
@@ -433,9 +433,9 @@ void CDStarTX::writeByte(uint8_t c)
     if (m_count >= m_sampleCount) {
       if (m_sampleInsert) {
         inBuffer[DSTAR_RADIO_BIT_LENGTH * 8U] = inBuffer[DSTAR_RADIO_BIT_LENGTH * 8U - 1U];
-        blockSize = DSTAR_RADIO_BIT_LENGTH * 8U + 1U;
+        blockSize++;
       } else {
-        blockSize = DSTAR_RADIO_BIT_LENGTH * 8U - 1U;
+        blockSize--;
       }
 
       m_count -= m_sampleCount;
diff --git a/IO.cpp b/IO.cpp
index 16eb272..57bf777 100644
--- a/IO.cpp
+++ b/IO.cpp
@@ -283,12 +283,12 @@ void CIO::process()
 
       if (m_count >= m_sampleCount) {
         if (m_sampleInsert) {
-          blockSize = RX_BLOCK_SIZE + 1U;
+          blockSize++;
           samples[RX_BLOCK_SIZE] = 0;
           for (int8_t i = RX_BLOCK_SIZE - 1; i >= 0; i--)
             control[i + 1] = control[i];
         } else {
-          blockSize = RX_BLOCK_SIZE - 1U;
+          blockSize--;
           for (uint8_t i = 0U; i < (RX_BLOCK_SIZE - 1U); i++)
             control[i] = control[i + 1U];
         }
diff --git a/SerialPort.cpp b/SerialPort.cpp
index 394ed9f..5536a68 100644
--- a/SerialPort.cpp
+++ b/SerialPort.cpp
@@ -210,11 +210,11 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length)
   uint8_t dmrDelay = data[7U];
 
   int8_t oscOffset = int8_t(data[8U]) - 128;
-  if (oscOffset < 0) {
-    m_sampleCount = 1000000U / uint32_t(-oscOffset);
-    m_sampleInsert = true;
-  } else if (oscOffset > 0) {
+  if (oscOffset > 0) {
     m_sampleCount = 1000000U / uint32_t(oscOffset);
+    m_sampleInsert = true;
+  } else if (oscOffset < 0) {
+    m_sampleCount = 1000000U / uint32_t(-oscOffset);
     m_sampleInsert = false;
   } else {
     m_sampleCount = 0U;
diff --git a/YSFTX.cpp b/YSFTX.cpp
index 74bc398..34c9299 100644
--- a/YSFTX.cpp
+++ b/YSFTX.cpp
@@ -133,7 +133,7 @@ void CYSFTX::writeByte(uint8_t c)
     }
   }
 
-  uint8_t blockSize = YSF_RADIO_SYMBOL_LENGTH * 4U;
+  uint16_t blockSize = YSF_RADIO_SYMBOL_LENGTH * 4U;
 
   // Handle the case of the oscillator not being accurate enough
   if (m_sampleCount > 0U) {
@@ -142,9 +142,9 @@ void CYSFTX::writeByte(uint8_t c)
     if (m_count >= m_sampleCount) {
       if (m_sampleInsert) {
         inBuffer[YSF_RADIO_SYMBOL_LENGTH * 4U] = inBuffer[YSF_RADIO_SYMBOL_LENGTH * 4U - 1U];
-        blockSize = YSF_RADIO_SYMBOL_LENGTH * 4U + 1U;
+        blockSize++;
       } else {
-        blockSize = YSF_RADIO_SYMBOL_LENGTH * 4U - 1U;
+        blockSize--;
       }
 
       m_count -= m_sampleCount;