diff --git a/CHANGELOG b/CHANGELOG
index 541a268bd..ecd71382b 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -1,5 +1,8 @@
 Changes to version 1.4.1
 ------------------------
+- MMS server: refactored connection handling; more efficient use of HandleSet
+- linux/bsd socket layer: replaced select by poll
+- removed header dependencies from API headers
 - IEC 61850 server: added support for transient data objects
 - .NET API: added MmsValue methods BitStringToUInt32BigEndian and BitStringFromUInt32BigEndian
 - fixed compilation problem when CONFIG_MMS_THREADLESS_STACK is defined
diff --git a/src/iec61850/inc/iec61850_server.h b/src/iec61850/inc/iec61850_server.h
index 6190f0137..f34e67ea2 100644
--- a/src/iec61850/inc/iec61850_server.h
+++ b/src/iec61850/inc/iec61850_server.h
@@ -457,6 +457,16 @@ IedServer_getDataModel(IedServer self);
 LIB61850_API bool
 IedServer_isRunning(IedServer self);
 
+/**
+ * \brief Get number of open MMS connections
+ *
+ * \param self the instance of IedServer to operate on
+ *
+ * \return the number of open and accepted MMS connections
+ */
+LIB61850_API int
+IedServer_getNumberOfOpenConnections(IedServer self);
+
 /**
  * \brief Get access to the underlying MmsServer instance.
  *
diff --git a/src/iec61850/server/impl/ied_server.c b/src/iec61850/server/impl/ied_server.c
index 4bbe92df4..2f950ac91 100644
--- a/src/iec61850/server/impl/ied_server.c
+++ b/src/iec61850/server/impl/ied_server.c
@@ -578,14 +578,10 @@ singleThreadedServerThread(void* parameter)
         printf("IED_SERVER: server thread started!\n");
 
     while (running) {
-
-        if (IedServer_waitReady(self, 25) > 0)
-            MmsServer_handleIncomingMessages(self->mmsServer);
+        MmsServer_handleIncomingMessages(self->mmsServer);
 
         IedServer_performPeriodicTasks(self);
 
-        Thread_sleep(1);
-
         running = mmsMapping->reportThreadRunning;
     }
 
@@ -613,7 +609,6 @@ IedServer_start(IedServer self, int tcpPort)
 
         Thread_start(self->serverThread);
 #else
-
         MmsServer_startListening(self->mmsServer, tcpPort);
         MmsMapping_startEventWorkerThread(self->mmsMapping);
 #endif
@@ -683,6 +678,12 @@ IedServer_startThreadless(IedServer self, int tcpPort)
     }
 }
 
+int
+IedServer_getNumberOfOpenConnections(IedServer self)
+{
+    return MmsServer_getConnectionCounter(self->mmsServer);
+}
+
 int
 IedServer_waitReady(IedServer self, unsigned int timeoutMs)
 {
diff --git a/src/iec61850/server/mms_mapping/mms_mapping.c b/src/iec61850/server/mms_mapping/mms_mapping.c
index 811e0fe68..a9f756152 100644
--- a/src/iec61850/server/mms_mapping/mms_mapping.c
+++ b/src/iec61850/server/mms_mapping/mms_mapping.c
@@ -3004,8 +3004,6 @@ GOOSE_processGooseEvents(MmsMapping* self, uint64_t currentTimeInMs)
 
 #endif /* (CONFIG_INCLUDE_GOOSE_SUPPORT == 1) */
 
-
-
 static void
 processPeriodicTasks(MmsMapping* self)
 {
diff --git a/src/mms/inc_private/iso_server.h b/src/mms/inc_private/iso_server.h
index e75cb30bc..4b39987fd 100644
--- a/src/mms/inc_private/iso_server.h
+++ b/src/mms/inc_private/iso_server.h
@@ -147,6 +147,9 @@ IsoServer_startListeningThreadless(IsoServer self);
 LIB61850_INTERNAL void
 IsoServer_processIncomingMessages(IsoServer self);
 
+LIB61850_INTERNAL int
+IsoServer_getConnectionCounter(IsoServer self);
+
 LIB61850_INTERNAL int
 IsoServer_waitReady(IsoServer self, unsigned int timeoutMs);
 
diff --git a/src/mms/inc_private/iso_server_private.h b/src/mms/inc_private/iso_server_private.h
index 53f34b911..dac94c727 100644
--- a/src/mms/inc_private/iso_server_private.h
+++ b/src/mms/inc_private/iso_server_private.h
@@ -1,7 +1,7 @@
 /*
  *  iso_server_private.h
  *
- *  Copyright 2013-2018 Michael Zillgith
+ *  Copyright 2013-2020 Michael Zillgith
  *
  *  This file is part of libIEC61850.
  *
@@ -28,7 +28,7 @@
 #include "hal_socket.h"
 
 LIB61850_INTERNAL IsoConnection
-IsoConnection_create(Socket socket, IsoServer isoServer);
+IsoConnection_create(Socket socket, IsoServer isoServer, bool isSingleThread);
 
 LIB61850_INTERNAL void
 IsoConnection_start(IsoConnection self);
@@ -37,7 +37,17 @@ LIB61850_INTERNAL void
 IsoConnection_destroy(IsoConnection self);
 
 LIB61850_INTERNAL void
-IsoConnection_handleTcpConnection(IsoConnection self);
+IsoConnection_callTickHandler(IsoConnection self);
+
+LIB61850_INTERNAL void
+IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread);
+
+#define ISO_CON_STATE_TERMINATED 2 /* connection has terminated and is ready to be destroyed */
+#define ISO_CON_STATE_RUNNING 1 /* connection is newly started */
+#define ISO_CON_STATE_STOPPED 0 /* connection is being stopped */
+
+LIB61850_INTERNAL int
+IsoConnection_getState(IsoConnection self);
 
 /**
  * \brief Add the connection socket to the given HandleSet instance
@@ -45,6 +55,12 @@ IsoConnection_handleTcpConnection(IsoConnection self);
 LIB61850_INTERNAL void
 IsoConnection_addToHandleSet(const IsoConnection self, HandleSet handles);
 
+/**
+ * \brief Remove the connection socket from the given HandleSet instance
+ */
+LIB61850_INTERNAL void
+IsoConnection_removeFromHandleSet(const IsoConnection self, HandleSet handles);
+
 LIB61850_INTERNAL void
 private_IsoServer_increaseConnectionCounter(IsoServer self);
 
diff --git a/src/mms/inc_private/mms_server_libinternal.h b/src/mms/inc_private/mms_server_libinternal.h
index 6c03b9af6..b516513ac 100644
--- a/src/mms/inc_private/mms_server_libinternal.h
+++ b/src/mms/inc_private/mms_server_libinternal.h
@@ -166,6 +166,16 @@ MmsServer_handleIncomingMessages(MmsServer self);
 LIB61850_INTERNAL void
 MmsServer_handleBackgroundTasks(MmsServer self);
 
+/**
+ * \brief Get number of open connections
+ *
+ * \param self the MmsServer instance to operate on
+ *
+ * \return the number of open MMS connections
+ */
+LIB61850_INTERNAL int
+MmsServer_getConnectionCounter(MmsServer self);
+
 /**
  * \brief Stop the server (for non-threaded operation mode)
  *
diff --git a/src/mms/iso_mms/server/mms_server.c b/src/mms/iso_mms/server/mms_server.c
index 792381a0f..b5af1737b 100644
--- a/src/mms/iso_mms/server/mms_server.c
+++ b/src/mms/iso_mms/server/mms_server.c
@@ -446,7 +446,7 @@ isoConnectionIndicationHandler(IsoConnectionIndication indication,
     }
 }
 
-#if (CONFIG_MMS_THREADLESS_STACK != 1)
+#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED != 1)
 void
 MmsServer_startListening(MmsServer server, int tcpPort)
 {
@@ -463,7 +463,7 @@ MmsServer_stopListening(MmsServer server)
 {
     IsoServer_stopListening(server->isoServer);
 }
-#endif /* (CONFIG_MMS_THREADLESS_STACK != 1)*/
+#endif /* (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED != 1)*/
 
 void
 MmsServer_startListeningThreadless(MmsServer self, int tcpPort)
@@ -504,6 +504,12 @@ MmsServer_handleBackgroundTasks(MmsServer self)
 #endif /* (MMS_OBTAIN_FILE_SERVICE == 1) */
 }
 
+int
+MmsServer_getConnectionCounter(MmsServer self)
+{
+    return IsoServer_getConnectionCounter(self->isoServer);
+}
+
 void
 MmsServer_callConnectionHandler(MmsServer self, MmsServerConnection connection)
 {
diff --git a/src/mms/iso_server/iso_connection.c b/src/mms/iso_server/iso_connection.c
index 03868ca01..9347c1939 100644
--- a/src/mms/iso_server/iso_connection.c
+++ b/src/mms/iso_server/iso_connection.c
@@ -1,7 +1,7 @@
 /*
  *  iso_connection.c
  *
- *  Copyright 2013-2018 Michael Zillgith
+ *  Copyright 2013-2020 Michael Zillgith
  *
  *  This file is part of libIEC61850.
  *
@@ -48,9 +48,6 @@
 
 #define TPKT_RFC1006_HEADER_SIZE 4
 
-#define ISO_CON_STATE_RUNNING 1
-#define ISO_CON_STATE_STOPPED 0
-
 struct sIsoConnection
 {
     uint8_t* receiveBuffer;
@@ -89,6 +86,10 @@ struct sIsoConnection
     Thread thread;
     Semaphore conMutex;
 #endif
+
+#if (CONFIG_MMS_SINGLE_THREADED != 1) || (CONFIG_MMS_THREADLESS_STACK == 1)
+    HandleSet handleSet;
+#endif
 };
 
 static void
@@ -97,15 +98,19 @@ finalizeIsoConnection(IsoConnection self)
     if (DEBUG_ISO_SERVER)
         printf("ISO_SERVER: finalizeIsoConnection (%p)--> close transport connection\n", self);
 
-    IsoServer_closeConnection(self->isoServer, self);
-
 #if (CONFIG_MMS_SUPPORT_TLS == 1)
     if (self->tlsSocket)
         TLSSocket_close(self->tlsSocket);
 #endif
 
-    if (self->socket != NULL)
-        Socket_destroy(self->socket);
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+#if (CONFIG_MMS_SINGLE_THREADED != 1)
+    if (self->handleSet) {
+        Handleset_destroy(self->handleSet);
+        self->handleSet = NULL;
+    }
+#endif
+#endif
 
     GLOBAL_FREEMEM(self->session);
     GLOBAL_FREEMEM(self->presentation);
@@ -126,7 +131,7 @@ finalizeIsoConnection(IsoConnection self)
     GLOBAL_FREEMEM(self->clientAddress);
     GLOBAL_FREEMEM(self->localAddress);
     IsoServer isoServer = self->isoServer;
-    GLOBAL_FREEMEM(self);
+
     if (DEBUG_ISO_SERVER)
         printf("ISO_SERVER: connection %p closed\n", self);
 
@@ -140,17 +145,34 @@ IsoConnection_addToHandleSet(const IsoConnection self, HandleSet handles)
 }
 
 void
-IsoConnection_handleTcpConnection(IsoConnection self)
+IsoConnection_removeFromHandleSet(const IsoConnection self, HandleSet handles)
+{
+    Handleset_removeSocket(handles, self->socket);
+}
+
+void
+IsoConnection_callTickHandler(IsoConnection self)
 {
-    /* call tick handler */
     if (self->tickHandler) {
         self->tickHandler(self->handlerParameter);
     }
+}
 
-#if (CONFIG_MMS_SINGLE_THREADED == 0)
-    if (IsoServer_waitReady(self->isoServer, 10) < 1)
-        goto exit_function;
-#endif /* (CONFIG_MMS_SINGLE_THREADED == 0) */
+void
+IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread)
+{
+#if (CONFIG_MMS_SINGLE_THREADED != 1)
+    if (isSingleThread == false) {
+
+        /* call tick handler */
+        if (self->tickHandler) {
+            self->tickHandler(self->handlerParameter);
+        }
+
+        if (Handleset_waitReady(self->handleSet, 10) < 1)
+            goto exit_function;
+    }
+#endif
 
     TpktState tpktState = CotpConnection_readToTpktBuffer(self->cotpConnection);
 
@@ -439,20 +461,25 @@ IsoConnection_handleTcpConnection(IsoConnection self)
 }
 
 #if ((CONFIG_MMS_SINGLE_THREADED == 0) && (CONFIG_MMS_THREADLESS_STACK == 0))
+/* only for multi-thread mode */
 static void
 handleTcpConnection(void* parameter)
 {
     IsoConnection self = (IsoConnection) parameter;
 
     while(self->state == ISO_CON_STATE_RUNNING)
-        IsoConnection_handleTcpConnection(self);
+        IsoConnection_handleTcpConnection(self, false);
+
+    IsoServer_closeConnection(self->isoServer, self);
 
     finalizeIsoConnection(self);
+
+    self->state = ISO_CON_STATE_TERMINATED;
 }
 #endif /* (CONFIG_MMS_SINGLE_THREADED == 0) */
 
 IsoConnection
-IsoConnection_create(Socket socket, IsoServer isoServer)
+IsoConnection_create(Socket socket, IsoServer isoServer, bool isSingleThread)
 {
     IsoConnection self = (IsoConnection) GLOBAL_CALLOC(1, sizeof(struct sIsoConnection));
     self->socket = socket;
@@ -524,13 +551,23 @@ IsoConnection_create(Socket socket, IsoServer isoServer)
 
 #if (CONFIG_MMS_SINGLE_THREADED == 0)
 #if (CONFIG_MMS_THREADLESS_STACK == 0)
-    self->thread = Thread_create((ThreadExecutionFunction) handleTcpConnection, self, true);
+    if (isSingleThread == false) {
+        self->handleSet = Handleset_new();
+        Handleset_addSocket(self->handleSet, self->socket);
+        self->thread = Thread_create((ThreadExecutionFunction) handleTcpConnection, self, false);
+    }
 #endif
 #endif
 
     return self;
 }
 
+int
+IsoConnection_getState(IsoConnection self)
+{
+    return self->state;
+}
+
 void
 IsoConnection_start(IsoConnection self)
 {
@@ -544,10 +581,15 @@ IsoConnection_start(IsoConnection self)
 void
 IsoConnection_destroy(IsoConnection self)
 {
-    if (DEBUG_ISO_SERVER)
-        printf("ISO_SERVER: destroy called for IsoConnection.\n");
+#if (CONFIG_MMS_THREADLESS_STACK == 0) && (CONFIG_MMS_SINGLE_THREADED == 0)
+    if (self->thread)
+        Thread_destroy(self->thread);
+#endif
 
-    finalizeIsoConnection(self);
+    if (self->socket != NULL)
+        Socket_destroy(self->socket);
+
+    GLOBAL_FREEMEM(self);
 }
 
 char*
@@ -627,19 +669,23 @@ IsoConnection_sendMessage(IsoConnection self, ByteBuffer* message)
 void
 IsoConnection_close(IsoConnection self)
 {
-    if (self->state != ISO_CON_STATE_STOPPED) {
-        Socket socket = self->socket;
+    if (self->state != ISO_CON_STATE_TERMINATED) {
         self->state = ISO_CON_STATE_STOPPED;
-        self->socket = NULL;
 
-#if (CONFIG_MMS_SUPPORT_TLS == 1)
-        if (self->tlsSocket) {
-            TLSSocket_close(self->tlsSocket);
-            self->tlsSocket = NULL;
+#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED != 1)
+        /* wait for connection thread to terminate */
+        if (self->thread) {
+            Thread_destroy(self->thread);
+            self->thread = NULL;
+        }
+        else {
+            finalizeIsoConnection(self);
+            self->state = ISO_CON_STATE_TERMINATED;
         }
+#else
+        finalizeIsoConnection(self);
+        self->state = ISO_CON_STATE_TERMINATED;
 #endif
-
-        Socket_destroy(socket);
     }
 }
 
diff --git a/src/mms/iso_server/iso_server.c b/src/mms/iso_server/iso_server.c
index 2549836d3..d07aa5de3 100644
--- a/src/mms/iso_server/iso_server.c
+++ b/src/mms/iso_server/iso_server.c
@@ -1,7 +1,7 @@
 /*
  *  iso_server.c
  *
- *  Copyright 2013-2018 Michael Zillgith
+ *  Copyright 2013-2020 Michael Zillgith
  *
  *  This file is part of libIEC61850.
  *
@@ -66,6 +66,8 @@ struct sIsoServer {
     Thread serverThread;
 #endif
 
+    HandleSet handleset;
+
     Socket serverSocket;
     int tcpPort;
     const char* localIpAddress;
@@ -147,7 +149,7 @@ addClientConnection(IsoServer self, IsoConnection connection)
 
     self->connectionCounter++;
     if (DEBUG_ISO_SERVER)
-        printf("IsoServer: increase connection counter to %i!\n", self->connectionCounter);
+        printf("ISO_SERVER: increase connection counter to %i!\n", self->connectionCounter);
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
     lockClientConnections(self);
@@ -183,11 +185,6 @@ addClientConnection(IsoServer self, IsoConnection connection)
 static void
 removeClientConnection(IsoServer self, IsoConnection connection)
 {
-
-#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
-    lockClientConnections(self);
-#endif
-
 #if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
 
 
@@ -205,6 +202,10 @@ removeClientConnection(IsoServer self, IsoConnection connection)
     for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++) {
         if (self->openClientConnections[i] == connection) {
 
+#if (CONFIG_MMS_SINGLE_THREADED == 1)
+            IsoConnection_removeFromHandleSet(connection, self->handleset);
+#endif
+
             if (DEBUG_ISO_SERVER)
                 printf("ISO_SERVER: removed connection (%p) index:%i\n", connection, i);
 
@@ -213,10 +214,72 @@ removeClientConnection(IsoServer self, IsoConnection connection)
         }
     }
 #endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
+}
+
+static void
+removeTerminatedConnections(IsoServer self, bool isSingleThread)
+{
+#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
+    lockClientConnections(self);
+#endif
+
+#if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
+
+    LinkedList openConnection = LinkedList_getNext(self->openClientConnections);
+    while (openConnection != NULL) {
+        IsoConnection isoConnection = (IsoConnection) openConnection->data;
+
+        if (isSingleThread) {
+            if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_STOPPED) {
+
+                self->connectionHandler(ISO_CONNECTION_CLOSED, self->connectionHandlerParameter,
+                        isoConnection);
+
+                IsoConnection_close(isoConnection);
+            }
+        }
+
+        if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_TERMINATED) {
+            removeClientConnection(self, isoConnection);
+            IsoConnection_destroy(isoConnection);
+            openConnection = LinkedList_getNext(self->openClientConnections);
+        }
+        else
+            openConnection = LinkedList_getNext(openConnection);
+    }
+
+#else
+    int i;
+
+    for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++) {
+        if (self->openClientConnections[i] != NULL) {
+
+            IsoConnection isoConnection = self->openClientConnections[i];
+
+            if (isSingleThread) {
+                if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_STOPPED) {
+
+                    self->connectionHandler(ISO_CONNECTION_CLOSED, self->connectionHandlerParameter,
+                            isoConnection);
+
+                    IsoConnection_close(isoConnection);
+
+                    IsoConnection_removeFromHandleSet(isoConnection, self->handleset);
+                }
+            }
+
+            if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_TERMINATED) {
+                removeClientConnection(self, isoConnection);
+                IsoConnection_destroy(isoConnection);
+            }
+        }
+    }
+#endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
     unlockClientConnections(self);
 #endif
+
 }
 
 static void
@@ -234,11 +297,7 @@ closeAllOpenClientConnections(IsoServer self)
         IsoConnection isoConnection = (IsoConnection) openConnection->data;
 
         IsoConnection_close(isoConnection);
-
-#if (CONFIG_MMS_SINGLE_THREADED == 1)
-            /* if CONFIG_MMS_SINGLE_THREADED == 0 connection instance will be destroyed by connection thread. */
         IsoConnection_destroy(isoConnection);
-#endif
 
         openConnection = LinkedList_getNext(openConnection);
     }
@@ -253,14 +312,43 @@ closeAllOpenClientConnections(IsoServer self)
 
     for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++) {
         if (self->openClientConnections[i] != NULL) {
-
             IsoConnection_close(self->openClientConnections[i]);
-
-#if (CONFIG_MMS_SINGLE_THREADED == 1)
-            /* if CONFIG_MMS_SINGLE_THREADED == 0 connection instance will be destroyed by connection thread. */
             IsoConnection_destroy(self->openClientConnections[i]);
+        }
+    }
+#endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
+    unlockClientConnections(self);
 #endif
 
+}
+
+static void
+callTickHandlerForClientConnections(IsoServer self)
+{
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
+    lockClientConnections(self);
+#endif
+
+#if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
+
+    LinkedList openConnection = LinkedList_getNext(self->openClientConnections);
+    while (openConnection != NULL) {
+        IsoConnection isoConnection = (IsoConnection) openConnection->data;
+
+        IsoConnection_callTickHandler(isoConnection);
+
+        openConnection = LinkedList_getNext(openConnection);
+    }
+
+#else
+    int i;
+
+    for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++) {
+        if (self->openClientConnections[i] != NULL) {
+            IsoConnection_callTickHandler(self->openClientConnections[i]);
         }
     }
 #endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
@@ -287,7 +375,7 @@ handleClientConnections(IsoServer self)
         IsoConnection isoConnection = (IsoConnection) openConnection->data;
 
         if (IsoConnection_isRunning(isoConnection))
-            IsoConnection_handleTcpConnection(isoConnection);
+            IsoConnection_handleTcpConnection(isoConnection, true);
         else {
             IsoConnection_destroy(isoConnection);
 
@@ -315,7 +403,7 @@ handleClientConnections(IsoServer self)
         if (self->openClientConnections[i] != NULL) {
             if (IsoConnection_isRunning(self->openClientConnections[i])) {
 
-                IsoConnection_handleTcpConnection(self->openClientConnections[i]);
+                IsoConnection_handleTcpConnection(self->openClientConnections[i], true);
             }
             else {
                 IsoConnection_destroy(self->openClientConnections[i]);
@@ -330,6 +418,8 @@ handleClientConnections(IsoServer self)
     unlockClientConnections(self);
 #endif
 
+    removeTerminatedConnections(self, true);
+
 #endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
 }
 
@@ -347,6 +437,9 @@ setupIsoServer(IsoServer self)
         goto exit_function;
     }
 
+    self->handleset = Handleset_new();
+    Handleset_addSocket(self->handleset, self->serverSocket);
+
 #if (CONFIG_ACTIVATE_TCP_KEEPALIVE == 1)
     Socket_activateTcpKeepAlive(self->serverSocket,
             CONFIG_TCP_KEEPALIVE_IDLE,
@@ -365,67 +458,24 @@ setupIsoServer(IsoServer self)
 }
 
 
-#if (CONFIG_MMS_THREADLESS_STACK == 0)
-/* used by single and multi-threaded versions */
+/** used by single and multi-threaded versions
+ *
+ * \param isSingleThread when true server is running in single thread or non-thread mode
+ */
 static void
-handleIsoConnections(IsoServer self)
-{
-    Socket connectionSocket;
-
-    if ((connectionSocket = ServerSocket_accept((ServerSocket) self->serverSocket)) != NULL) {
-
-#if (CONFIG_MMS_SERVER_CONFIG_SERVICES_AT_RUNTIME == 1)
-        if (self->maxConnections > -1) {
-            if (private_IsoServer_getConnectionCounter(self) >= self->maxConnections) {
-                if (DEBUG_ISO_SERVER)
-                    printf("ISO_SERVER: maximum number of connections reached -> reject connection attempt.\n");
-
-                Socket_destroy(connectionSocket);
-
-                return;
-            }
-        }
-#endif /* (CONFIG_MMS_SERVER_CONFIG_SERVICES_AT_RUNTIME == 1) */
-
-#if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS != -1)
-        if (private_IsoServer_getConnectionCounter(self) >= CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS) {
-            if (DEBUG_ISO_SERVER)
-                printf("ISO_SERVER: maximum number of connections reached -> reject connection attempt.\n");
-
-            Socket_destroy(connectionSocket);
-
-#if (CONFIG_MMS_SINGLE_THREADED == 1)
-            handleClientConnections(self);
-#endif
-
-            return;
-        }
-#endif
-
-        IsoConnection isoConnection = IsoConnection_create(connectionSocket, self);
-
-        if (isoConnection) {
-
-            addClientConnection(self, isoConnection);
-
-            self->connectionHandler(ISO_CONNECTION_OPENED, self->connectionHandlerParameter,
-                    isoConnection);
-
-            IsoConnection_start(isoConnection);
-        }
-
+handleIsoConnections(IsoServer self, bool isSingleThread)
+{
+    if (isSingleThread) {
+        /*
+         * NOTE: when running in multi thread mode the tick handler is called
+         * by the connection thread.
+         */
+        callTickHandlerForClientConnections(self);
     }
 
-#if (CONFIG_MMS_SINGLE_THREADED == 1)
-    handleClientConnections(self);
-#endif
-}
-#endif /* (CONFIG_MMS_THREADLESS_STACK == 0) */
+    if (Handleset_waitReady(self->handleset, 1) < 1)
+        return;
 
-/* used by non-threaded version */
-static void
-handleIsoConnectionsThreadless(IsoServer self)
-{
     Socket connectionSocket;
 
     if ((connectionSocket = ServerSocket_accept((ServerSocket) self->serverSocket)) != NULL) {
@@ -450,28 +500,35 @@ handleIsoConnectionsThreadless(IsoServer self)
 
             Socket_destroy(connectionSocket);
 
-            handleClientConnections(self);
+            if (isSingleThread)
+                handleClientConnections(self);
 
             return;
         }
 #endif
 
-        IsoConnection isoConnection = IsoConnection_create(connectionSocket, self);
+        IsoConnection isoConnection = IsoConnection_create(connectionSocket, self, isSingleThread);
 
         if (isoConnection) {
-
             addClientConnection(self, isoConnection);
 
+            if (isSingleThread)
+                IsoConnection_addToHandleSet(isoConnection, self->handleset);
+
             self->connectionHandler(ISO_CONNECTION_OPENED, self->connectionHandlerParameter,
                     isoConnection);
 
+            if (isSingleThread == false)
+                IsoConnection_start(isoConnection);
         }
+
     }
 
-    handleClientConnections(self);
+    if (isSingleThread)
+        handleClientConnections(self);
 }
 
-#if (CONFIG_MMS_THREADLESS_STACK != 1)
+#if (CONFIG_MMS_SINGLE_THREADED == 0) && (CONFIG_MMS_THREADLESS_STACK == 0)
 /* only required for multi-threaded server! */
 static void
 isoServerThread(void* isoServerParam)
@@ -490,9 +547,9 @@ isoServerThread(void* isoServerParam)
 
     while (self->state == ISO_SVR_STATE_RUNNING)
     {
-        handleIsoConnections(self);
+        removeTerminatedConnections(self, false);
 
-        Thread_sleep(1);
+        handleIsoConnections(self, false);
     }
 
     self->state = ISO_SVR_STATE_STOPPED;
@@ -501,8 +558,7 @@ isoServerThread(void* isoServerParam)
     self->serverSocket = NULL;
 
     if (DEBUG_ISO_SERVER)
-           printf("ISO_SERVER: isoServerThread %p stopped\n", &isoServerParam);
-
+        printf("ISO_SERVER: isoServerThread %p stopped\n", &isoServerParam);
 }
 #endif
 
@@ -593,7 +649,7 @@ IsoServer_getTLSConfiguration(IsoServer self)
     return self->tlsConfiguration;
 }
 
-#if (CONFIG_MMS_THREADLESS_STACK != 1)
+#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED != 1)
 void
 IsoServer_startListening(IsoServer self)
 {
@@ -620,7 +676,7 @@ IsoServer_startListening(IsoServer self)
 exit_function:
     return;
 }
-#endif /* (CONFIG_MMS_THREADLESS_STACK != 1) */
+#endif /* (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED != 1) */
 
 void
 IsoServer_startListeningThreadless(IsoServer self)
@@ -645,66 +701,15 @@ IsoServer_waitReady(IsoServer self, unsigned int timeoutMs)
    int result;
 
    if (getState(self) == ISO_SVR_STATE_RUNNING) {
-       HandleSet handles = Handleset_new();
-
-       if (handles) {
-
-#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
-           lockClientConnections(self);
-#endif
-
-#if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
 
-           LinkedList openConnection = LinkedList_getNext(self->openClientConnections);
-           LinkedList lastConnection = self->openClientConnections;
-
-           while (openConnection != NULL) {
-               IsoConnection isoConnection = (IsoConnection) openConnection->data;
-
-               if (IsoConnection_isRunning(isoConnection)) {
-                   IsoConnection_addToHandleSet(isoConnection, handles);
-                   openConnection = LinkedList_getNext(openConnection);
-               } else {
-#if ((CONFIG_MMS_SINGLE_THREADED == 1) || (CONFIG_MMS_THREADLESS_STACK == 1))
-                   IsoConnection_destroy(isoConnection);
-#endif
-                   lastConnection->next = openConnection->next;
-                   GLOBAL_FREEMEM(openConnection);
-                   openConnection = lastConnection->next;
-               }
-
-               lastConnection = lastConnection->next;
-           }
-
-#else
-           int i;
-
-           for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++) {
-               if (self->openClientConnections[i] != NULL) {
-                   if (IsoConnection_isRunning(self->openClientConnections[i])) {
-                       IsoConnection_addToHandleSet(self->openClientConnections[i], handles);
-                   }
-                   else {
-#if ((CONFIG_MMS_SINGLE_THREADED == 1) || (CONFIG_MMS_THREADLESS_STACK == 1))
-                       IsoConnection_destroy(self->openClientConnections[i]);
-#endif
-                       self->openClientConnections[i] = NULL;
-                   }
-               }
-           }
-
-#endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
-
-#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
-           unlockClientConnections(self);
-#endif
-
-           Handleset_addSocket(handles, self->serverSocket);
-           result = Handleset_waitReady(handles, timeoutMs);
-           Handleset_destroy(handles);
-       } else {
-           result = -1;
+       if (self->handleset) {
+           result = Handleset_waitReady(self->handleset, 10);
+       }
+       else {
+           if (DEBUG_ISO_SERVER)
+               printf("ISO_SERVER: internal error - no handleset!\n");
        }
+
    } else {
        result = -1;
    }
@@ -712,12 +717,17 @@ IsoServer_waitReady(IsoServer self, unsigned int timeoutMs)
    return result;
 }
 
-
 void
 IsoServer_processIncomingMessages(IsoServer self)
 {
     if (getState(self) == ISO_SVR_STATE_RUNNING)
-        handleIsoConnectionsThreadless(self);
+        handleIsoConnections(self, true);
+}
+
+int
+IsoServer_getConnectionCounter(IsoServer self)
+{
+    return private_IsoServer_getConnectionCounter(self);
 }
 
 static void
@@ -769,8 +779,6 @@ IsoServer_closeConnection(IsoServer self, IsoConnection isoConnection)
         self->connectionHandler(ISO_CONNECTION_CLOSED, self->connectionHandlerParameter,
                 isoConnection);
     }
-
-    removeClientConnection(self, isoConnection);
 }
 
 void
@@ -811,6 +819,9 @@ IsoServer_destroy(IsoServer self)
     Semaphore_destroy(self->openClientConnectionsMutex);
 #endif
 
+    if (self->handleset)
+        Handleset_destroy(self->handleset);
+
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
     Semaphore_destroy(self->stateLock);
 #endif