diff --git a/IO.cpp b/IO.cpp
index d2d041f..d5e38b1 100644
--- a/IO.cpp
+++ b/IO.cpp
@@ -140,12 +140,14 @@ void CIO::process()
     if(m_cwid_state) { // check for CW ID end of transmission
       m_cwid_state = false;
       // Restoring previous mode
-      io.ifConf(m_modemState_prev, true);
+      if (m_TotalModes)
+        io.ifConf(m_modemState_prev, true);
     }
     if(m_pocsag_state) { // check for POCSAG end of transmission
       m_pocsag_state = false;
       // Restoring previous mode
-      io.ifConf(m_modemState_prev, true);
+      if (m_TotalModes)
+        io.ifConf(m_modemState_prev, true);
     }
     setRX(false);
   }
diff --git a/SerialPort.cpp b/SerialPort.cpp
index 8ee44b5..5ee99e7 100644
--- a/SerialPort.cpp
+++ b/SerialPort.cpp
@@ -330,6 +330,8 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length)
       io.ifConf(STATE_P25, true);
     else if(m_nxdnEnable)
       io.ifConf(STATE_NXDN, true);
+    else if(m_pocsagEnable)
+      io.ifConf(STATE_POCSAG, true);
   }
 
   io.start();