diff --git a/ADF7021.cpp b/ADF7021.cpp
index 05da485..0024c9b 100644
--- a/ADF7021.cpp
+++ b/ADF7021.cpp
@@ -1,6 +1,9 @@
 /*
  *   Copyright (C) 2016 by Jim McLaughlin KI6ZUM
  *   Copyright (C) 2016, 2017 by Andy Uribe CA6JAU
+ * 
+ *   Some of the code is based on work of Guus Van Dooren PE1PLM:
+ *   https://github.com/ki6zum/gmsk-dstar/blob/master/firmware/dvmega/dvmega.ino
  *
  *   This program is free software; you can redistribute it and/or modify
  *   it under the terms of the GNU General Public License as published by
@@ -29,12 +32,6 @@
 volatile uint32_t  AD7021_control_byte;
 volatile int       AD7021_counter;
 
-void dlybit(void)
-{
-  volatile unsigned int delay;
-  for(delay = 0;delay<5;delay++);
-}
-
 void Send_AD7021_control()
 {
   for(AD7021_counter = 31; AD7021_counter >= 0; AD7021_counter--) {
@@ -43,14 +40,14 @@ void Send_AD7021_control()
     else
       io.SDATA_pin(LOW);
      
+    io.dlybit();
     io.SCLK_pin(HIGH);
-    dlybit();
+    io.dlybit();
     io.SCLK_pin(LOW);
-    dlybit();
   }
   
   io.SLE_pin(HIGH);
-  dlybit();
+  io.dlybit();
   io.SLE_pin(LOW);    
   io.SDATA_pin(LOW);
 }
diff --git a/ADF7021.h b/ADF7021.h
index 477f6bf..cb355db 100644
--- a/ADF7021.h
+++ b/ADF7021.h
@@ -28,7 +28,6 @@
 
 #define bitRead(value, bit) (((value) >> (bit)) & 0x01)
 
-void dlybit(void);
 void Send_AD7021_control(void);
 void Send_REG0_RX(void);
 void Send_REG0_TX(void);
diff --git a/IO.h b/IO.h
index 797da19..3f66c61 100644
--- a/IO.h
+++ b/IO.h
@@ -73,13 +73,15 @@ public:
   void  ifConf();
   void  ifInit();
 
+  // Misc functions
+  void  dlybit(void);
+  void  delay_rx(void);
+   
 private:
   bool      m_started;
   CBitRB    m_rxBuffer;
   CBitRB    m_txBuffer;
 
-  void  delay_rx(void);
-
 };
 
 #endif
diff --git a/IOArduino.cpp b/IOArduino.cpp
index bce310f..fdd72ff 100644
--- a/IOArduino.cpp
+++ b/IOArduino.cpp
@@ -73,6 +73,12 @@ void CIO::delay_rx() {
   delayMicroseconds(1);
 }
 
+void CIO::dlybit(void)
+{
+  volatile unsigned int delay;
+  delayMicroseconds(1);
+}
+
 void CIO::Init()
 {
 #if defined (__STM32F1__) 
diff --git a/IOSTM.cpp b/IOSTM.cpp
index 515f4cf..2af5d45 100644
--- a/IOSTM.cpp
+++ b/IOSTM.cpp
@@ -85,6 +85,12 @@ void CIO::delay_rx() {
   for(delay = 0;delay<512;delay++);
 }
 
+void CIO::dlybit(void)
+{
+  volatile unsigned int delay;
+  for(delay = 0;delay<5;delay++);
+}
+
 void CIO::Init()
 {
   // USB Conf IO: