diff --git a/ADF7021.cpp b/ADF7021.cpp
index baccaad..7461cd2 100644
--- a/ADF7021.cpp
+++ b/ADF7021.cpp
@@ -257,14 +257,7 @@ void IO::interrupt1()
     }
 
     m_watchdog++;
-    m_modeTimerCnt++;
     m_int1Counter++;
-
-    if (m_scanPauseCnt >= SCAN_PAUSE)
-        m_scanPauseCnt = 0U;
-
-    if (m_scanPauseCnt != 0U)
-        m_scanPauseCnt++;
 }
 
 #if defined(DUPLEX)
@@ -299,8 +292,7 @@ void IO::rf1Conf(DVM_STATE modemState, bool reset)
 
     uint32_t txFrequencyTmp, rxFrequencyTmp;
 
-    if (modemState != STATE_CW)
-        m_modemStatePrev = modemState;
+    DEBUG3("IO: rf1Conf(): configuring ADF for Tx/Rx; modemState/reset", modemState, reset);
 
 #if defined (ZUMSPOT_ADF7021) || defined(SKYBRIDGE_HS)
     io.checkBand(m_rxFrequency, m_txFrequency);
@@ -500,6 +492,8 @@ void IO::rf1Conf(DVM_STATE modemState, bool reset)
 /// <param name="reset"></param>
 void IO::rf2Conf(DVM_STATE modemState)
 {
+    DEBUG2("IO: rf2Conf(): configuring 2nd ADF for Rx; modemState", modemState);
+
     // configure ADF Tx/RX
     configureTxRx(modemState);
 
@@ -685,6 +679,8 @@ void IO::updateCal(DVM_STATE modemState)
     AD7021_CONTROL = ADF7021_REG2;
     AD7021_1_IOCTL();
 
+    DEBUG2("IO: updateCal(): updating ADF calibration; modemState", modemState);
+
     if (m_tx)
         setTX();
     else
@@ -850,6 +846,8 @@ void IO::configureBand()
         f_div = 2U;
     else
         f_div = 1U;
+
+    DEBUG3("IO: configureBand(): configuring ADF freq band; reg1/f_div", ADF7021_REG1, f_div);
 }
 
 /// <summary>
@@ -1078,6 +1076,9 @@ void IO::configureTxRx(DVM_STATE modemState)
         }
         break;
     }
+
+    DEBUG4("IO: configureTxRx(): configuring ADF Tx/Rx states; dmrSymDev/p25SymDev/rfPower", (uint16_t)((ADF7021_PFD * dmrDev) / (f_div * 65536)),
+        (uint16_t)((ADF7021_PFD * p25Dev) / (f_div * 65536)), m_rfPower);
 }
 
 /// <summary>
@@ -1127,39 +1128,4 @@ void IO::setRX(bool doSle)
     }
 }
 
-
-#if defined(ENABLE_DEBUG)
-
-uint32_t CIO::RXfreq()
-{
-  return (uint32_t)((float)(ADF7021_PFD / f_div) * ((float)((32768 * m_RX_N_divider) + m_RX_F_divider) / 32768.0)) + 100000;
-}
-
-uint32_t CIO::TXfreq()
-{
-  return (uint32_t)((float)(ADF7021_PFD / f_div) * ((float)((32768 * m_TX_N_divider) + m_TX_F_divider) / 32768.0));
-}
-
-uint16_t CIO::devDMR()
-{
-  return (uint16_t)((ADF7021_PFD * dmrDev) / (f_div * 65536));
-}
-
-uint16_t CIO::devP25()
-{
-  return (uint16_t)((ADF7021_PFD * p25Dev) / (f_div * 65536));
-}
-
-void CIO::printConf()
-{
-  DEBUG1("MMDVM_HS FW configuration:");
-  DEBUG2I("TX freq (Hz):", TXfreq());
-  DEBUG2I("RX freq (Hz):", RXfreq());
-  DEBUG2("Power set:", m_power);
-  DEBUG2("DMR +1 sym dev (Hz):", devDMR());
-  DEBUG2("P25 +1 sym dev (Hz):", devP25());
-}
-
-#endif
-
 #endif // ENABLE_ADF7021
diff --git a/FirmwareMain.cpp b/FirmwareMain.cpp
index bdeadb1..0e34f45 100644
--- a/FirmwareMain.cpp
+++ b/FirmwareMain.cpp
@@ -38,13 +38,10 @@
 // ---------------------------------------------------------------------------
 
 DVM_STATE m_modemState = STATE_IDLE;
-DVM_STATE m_modemStatePrev = STATE_IDLE;
 
 bool m_cwIdState = false;
 uint8_t m_cwIdTXLevel = 30;
 
-uint32_t m_modeTimerCnt;
-
 #ifdef ENABLE_DMR
 bool m_dmrEnable = true;
 #else
@@ -101,9 +98,9 @@ void setup()
 
 void loop()
 {
-    io.process();
-  
     serial.process();
+    
+    io.process();
 
     // The following is for transmitting
     if (m_dmrEnable && m_modemState == STATE_DMR) {
diff --git a/Globals.h b/Globals.h
index 918f0da..c3ee4b8 100644
--- a/Globals.h
+++ b/Globals.h
@@ -85,13 +85,10 @@ const uint8_t   MARK_NONE = 0x00U;
 // ---------------------------------------------------------------------------
 
 extern DVM_STATE m_modemState;
-extern DVM_STATE m_modemStatePrev;
 
 extern bool m_cwIdState;
 extern uint8_t m_cwIdTXLevel;
 
-extern uint32_t m_modeTimerCnt;
-
 extern bool m_dmrEnable;
 extern bool m_p25Enable;
 
diff --git a/IO.cpp b/IO.cpp
index 7a010e3..102457d 100644
--- a/IO.cpp
+++ b/IO.cpp
@@ -53,9 +53,6 @@ IO::IO():
     m_rxBuffer(1024U),
     m_txBuffer(1024U),
     m_ledCount(0U),
-    m_scanEnable(false),
-    m_scanPauseCnt(0U),
-    m_scanPos(0U),
     m_ledValue(true),
     m_watchdog(0U),
     m_int1Counter(0U),
@@ -83,7 +80,6 @@ IO::IO():
 #endif
 
     selfTest();
-    m_modeTimerCnt = 0U;
 }
 
 /// <summary>
@@ -91,30 +87,6 @@ IO::IO():
 /// </summary>
 void IO::start()
 {
-    m_totalModes = 0U;
-
-    if (m_dmrEnable) {
-        m_modes[m_totalModes] = STATE_DMR;
-        m_totalModes++;
-    }
-    if (m_p25Enable) {
-        m_modes[m_totalModes] = STATE_P25;
-        m_totalModes++;
-    }
-
-#if defined(ENABLE_SCAN_MODE)
-    if (m_totalModes > 1U) {
-        m_scanEnable = true;
-    }
-    else {
-        m_scanEnable = false;
-        setMode(m_modemState);
-    }
-#else
-    m_scanEnable = false;
-    setMode(m_modemState);
-#endif
-
     if (m_started)
         return;
 
@@ -129,18 +101,19 @@ void IO::start()
 void IO::process()
 {
     uint8_t bit;
-    uint32_t scantime;
+    uint32_t scanTime;
     uint8_t control;
 
     m_ledCount++;
     if (m_started) {
         // Two seconds timeout
         if (m_watchdog >= 19200U) {
+/*            
             if (m_modemState == STATE_DMR || m_modemState == STATE_P25) {
                 m_modemState = STATE_IDLE;
                 setMode(m_modemState);
             }
-
+*/
             m_watchdog = 0U;
         }
 
@@ -159,42 +132,22 @@ void IO::process()
         return;
     }
 
-    // Switch off the transmitter if needed
+    // switch off the transmitter if needed
     if (m_txBuffer.getData() == 0U && m_tx) {
         if (m_cwIdState) { 
             // check for CW ID end of transmission
             m_cwIdState = false;
-            
-            // restoring previous mode
-            if (m_totalModes) {
-                io.rf1Conf(m_modemStatePrev, true);
-            }
+            io.rf1Conf(m_modemState, true);
         }
 
         setRX(false);
     }
 
-    if (m_modemStatePrev == STATE_DMR)
-        scantime = SCAN_TIME * 2U;
-    else if (m_modemStatePrev == STATE_P25)
-        scantime = SCAN_TIME;
-    else
-        scantime = SCAN_TIME;
-
-    if (m_modeTimerCnt >= scantime) {
-        m_modeTimerCnt = 0U;
-        if ((m_modemState == STATE_IDLE) && (m_scanPauseCnt == 0U) && m_scanEnable && !m_cwIdState) {
-            m_scanPos = (m_scanPos + 1U) % m_totalModes;
-
-            setMode(m_modes[m_scanPos]);
-            io.rf1Conf(m_modes[m_scanPos], true);
-        }
-    }
-
     if (m_rxBuffer.getData() >= 1U) {
         m_rxBuffer.get(bit, control);
 
-        if (m_modemStatePrev == STATE_DMR) {
+        if (m_modemState == STATE_DMR) {
+            /** Digital Mobile Radio */
 #if defined(DUPLEX)
             if (m_duplex) {
                 if (m_tx)
@@ -208,7 +161,8 @@ void IO::process()
             dmrDMORX.databit(bit);
 #endif
         }
-        else if (m_modemStatePrev == STATE_P25) {
+        else if (m_modemState == STATE_P25) {
+            /** Project 25 */
             p25RX.databit(bit);
         }
     }
@@ -255,10 +209,8 @@ uint16_t IO::getSpace() const
 /// <param name="dcd"></param>
 void IO::setDecode(bool dcd)
 {
-    if (dcd != m_dcd) {
-        m_scanPauseCnt = 1U;
+    if (dcd != m_dcd)
         setCOSInt(dcd ? true : false);
-    }
 
     m_dcd = dcd;
 }
@@ -270,16 +222,12 @@ void IO::setMode(DVM_STATE modemState)
 {
     DVM_STATE relativeState = modemState;
 
-    if ((modemState != STATE_IDLE) && (m_modemStatePrev != modemState)) {
-        DEBUG2("IO: setMode(): setting RF hardware", modemState);
-        if (serial.isCalState(modemState)) {
-            relativeState = serial.calRelativeState(modemState);
-        }
-    }
-    else {
-        return;
+    if (serial.isCalState(modemState)) {
+        relativeState = serial.calRelativeState(modemState);
     }
 
+    DEBUG3("IO: setMode(): setting RF hardware", modemState, relativeState);
+
     rf1Conf(relativeState, true);
 
     setDMRInt(relativeState == STATE_DMR);
diff --git a/IO.h b/IO.h
index 4a03747..c1fd010 100644
--- a/IO.h
+++ b/IO.h
@@ -41,9 +41,6 @@
 //  Constants
 // ---------------------------------------------------------------------------
 
-#define SCAN_TIME 1920
-#define SCAN_PAUSE 20000
-
 #if defined(DUPLEX)
 #if defined(STM32_USB_HOST)
 #define CAL_DLY_LOOP 98950U
@@ -179,13 +176,6 @@ private:
     BitBuffer m_txBuffer;
 
     uint32_t m_ledCount;
-
-    bool m_scanEnable;
-    uint32_t m_scanPauseCnt;
-    uint8_t m_scanPos;
-
-    uint8_t m_totalModes;
-    DVM_STATE m_modes[6];
     bool m_ledValue;
 
     volatile uint32_t m_watchdog;
diff --git a/SerialPort.cpp b/SerialPort.cpp
index 3cd38f4..6217a6a 100644
--- a/SerialPort.cpp
+++ b/SerialPort.cpp
@@ -370,13 +370,14 @@ void SerialPort::process()
 bool SerialPort::isCalState(DVM_STATE state)
 {
     // calibration mode check
-    if (state != STATE_P25_CAL_1K &&
-        state != STATE_DMR_DMO_CAL_1K && state != STATE_DMR_CAL_1K &&
-        state != STATE_DMR_LF_CAL && state != STATE_P25_LF_CAL &&
-        state != STATE_RSSI_CAL &&
-        state != STATE_P25_CAL && state != STATE_DMR_CAL &&
-        state != STATE_INT_CAL)
+    if (state == STATE_P25_CAL_1K ||
+        state == STATE_DMR_DMO_CAL_1K || state == STATE_DMR_CAL_1K ||
+        state == STATE_DMR_LF_CAL || state == STATE_P25_LF_CAL ||
+        state == STATE_RSSI_CAL ||
+        state == STATE_P25_CAL || state == STATE_DMR_CAL ||
+        state == STATE_INT_CAL) {
         return true;
+    }
     
     return false;
 }
@@ -393,7 +394,7 @@ DVM_STATE SerialPort::calRelativeState(DVM_STATE state)
             state == STATE_DMR_LF_CAL || state == STATE_DMR_CAL ||
             state == STATE_RSSI_CAL || state == STATE_INT_CAL) {
             return STATE_DMR;
-        } else if(state != STATE_P25_CAL_1K && state != STATE_P25_LF_CAL &&
+        } else if(state == STATE_P25_CAL_1K || state == STATE_P25_LF_CAL ||
             state == STATE_P25_CAL) {
             return STATE_P25;
         }
@@ -943,6 +944,10 @@ uint8_t SerialPort::setConfig(const uint8_t* data, uint8_t length)
 
     m_dmrEnable = dmrEnable;
     m_p25Enable = p25Enable;
+
+    if (m_dmrEnable && m_p25Enable)
+        return RSN_HS_NO_DUAL_MODE;
+
     m_duplex = !simplex;
 
 #if !defined(DUPLEX)
@@ -977,6 +982,8 @@ uint8_t SerialPort::setConfig(const uint8_t* data, uint8_t length)
         io.updateCal(calRelativeState(m_modemState));
     }
 
+    setMode(m_modemState);
+
     io.start();
 
     return RSN_OK;
diff --git a/SerialPort.h b/SerialPort.h
index 3054e3c..0ea5795 100644
--- a/SerialPort.h
+++ b/SerialPort.h
@@ -121,6 +121,8 @@ enum CMD_REASON_CODE {
 
     RSN_INVALID_P25_CORR_COUNT = 16U,
 
+    RSN_HS_NO_DUAL_MODE = 32U,
+
     RSN_DMR_DISABLED = 63U,
     RSN_P25_DISABLED = 64U,
 };
@@ -128,6 +130,7 @@ enum CMD_REASON_CODE {
 const uint8_t DVM_FRAME_START = 0xFEU;
 
 #define SERIAL_SPEED 115200
+#define STATE_SCAN_MAX 3
 
 // ---------------------------------------------------------------------------
 //  Class Declaration
diff --git a/dmr/DMRDMORX.cpp b/dmr/DMRDMORX.cpp
index 5f80a04..a84011b 100644
--- a/dmr/DMRDMORX.cpp
+++ b/dmr/DMRDMORX.cpp
@@ -259,8 +259,6 @@ void DMRDMORX::correlateSync()
         if (m_endPtr >= DMO_BUFFER_LENGTH_BITS)
             m_endPtr -= DMO_BUFFER_LENGTH_BITS;
 
-        m_modeTimerCnt = 0;
-
         DEBUG4("DMRDMORX: correlateSync(): dataPtr/startPtr/endPtr", m_dataPtr, m_startPtr, m_endPtr);
     }
     else if ((countBits64((m_bitBuffer & DMR_SYNC_BITS_MASK) ^ DMR_MS_VOICE_SYNC_BITS) <= MAX_SYNC_BYTES_ERRS) || \
@@ -276,8 +274,6 @@ void DMRDMORX::correlateSync()
         if (m_endPtr >= DMO_BUFFER_LENGTH_BITS)
             m_endPtr -= DMO_BUFFER_LENGTH_BITS;
 
-        m_modeTimerCnt = 0;
-
         DEBUG4("DMRDMORX: correlateSync(): dataPtr/startPtr/endPtr", m_dataPtr, m_startPtr, m_endPtr);
     }
 }