diff --git a/config/stack_config.h b/config/stack_config.h
index 405a5a1a..c21730e5 100644
--- a/config/stack_config.h
+++ b/config/stack_config.h
@@ -36,7 +36,7 @@
  * 0 ==> server runs in multi-threaded mode (one thread for each connection and
  * one server background thread )
  */
-#define CONFIG_MMS_SINGLE_THREADED 1
+#define CONFIG_MMS_SINGLE_THREADED 0
 
 /*
  * Optimize stack for threadless operation - don't use semaphores
diff --git a/src/iec61850/server/impl/ied_server.c b/src/iec61850/server/impl/ied_server.c
index 83a3a9ef..1a236485 100644
--- a/src/iec61850/server/impl/ied_server.c
+++ b/src/iec61850/server/impl/ied_server.c
@@ -485,15 +485,12 @@ singleThreadedServerThread(void* parameter)
 
     while (running) {
 
-        if (IedServer_waitReady(self, 25) > 0) {
+        if (IedServer_waitReady(self, 25) > 0)
             MmsServer_handleIncomingMessages(self->mmsServer);
-            IedServer_performPeriodicTasks(self);
-        }
-        else {
-            IedServer_performPeriodicTasks(self);
-        }
 
-        Thread_sleep(1);
+        IedServer_performPeriodicTasks(self);
+
+        //Thread_sleep(1);
 
         running = mmsMapping->reportThreadRunning;
     }
diff --git a/src/iec61850/server/mms_mapping/reporting.c b/src/iec61850/server/mms_mapping/reporting.c
index 2ce1a3ff..238b9dbc 100644
--- a/src/iec61850/server/mms_mapping/reporting.c
+++ b/src/iec61850/server/mms_mapping/reporting.c
@@ -1,7 +1,7 @@
 /*
  *  reporting.c
  *
- *  Copyright 2013, 2014 Michael Zillgith
+ *  Copyright 2013 - 2016 Michael Zillgith
  *
  *  This file is part of libIEC61850.
  *
diff --git a/src/mms/iso_server/iso_connection.c b/src/mms/iso_server/iso_connection.c
index 8ae5168c..f52970fd 100644
--- a/src/mms/iso_server/iso_connection.c
+++ b/src/mms/iso_server/iso_connection.c
@@ -130,6 +130,11 @@ IsoConnection_addHandleSet(const IsoConnection self, HandleSet handles)
 void
 IsoConnection_handleTcpConnection(IsoConnection self)
 {
+#if (CONFIG_MMS_SINGLE_THREADED == 0)
+    if (IsoServer_waitReady(self->isoServer, 10) < 1)
+        return;
+#endif /* (CONFIG_MMS_SINGLE_THREADED == 0) */
+
     TpktState tpktState = CotpConnection_readToTpktBuffer(self->cotpConnection);
 
     if (tpktState == TPKT_ERROR)
@@ -430,7 +435,7 @@ handleTcpConnection(void* parameter)
 
     while(self->state == ISO_CON_STATE_RUNNING) {
         IsoConnection_handleTcpConnection(self);
-        Thread_sleep(1);
+       // Thread_sleep(1);
     }
 
     finalizeIsoConnection(self);