Merge branch 'v1.6_develop_488' into v1.6_develop

dotnet/IEC61850forCSharp/IEC61850ServerAPI.cs

@@ -1756,7 +1756,7 @@ namespace IEC61850
             }
         }
 
-        public class ClientConnection 
+        public class ClientConnection : IDisposable
         {
             [DllImport("iec61850", CallingConvention = CallingConvention.Cdecl)]
             static extern IntPtr ClientConnection_getPeerAddress(IntPtr self);
@@ -1764,31 +1764,90 @@ namespace IEC61850
             [DllImport("iec61850", CallingConvention = CallingConvention.Cdecl)]
             static extern IntPtr ClientConnection_getLocalAddress(IntPtr self);
 
+            [DllImport("iec61850", CallingConvention = CallingConvention.Cdecl)]
+            [return: MarshalAs(UnmanagedType.I1)]
+            static extern bool ClientConnection_abort(IntPtr self);
+
+            [DllImport("iec61850", CallingConvention = CallingConvention.Cdecl)]
+            static extern IntPtr ClientConnection_claimOwnership(IntPtr self);
+
+            [DllImport("iec61850", CallingConvention = CallingConvention.Cdecl)]
+            static extern void ClientConnection_release(IntPtr self);
+
             internal IntPtr self;
 
-            internal ClientConnection (IntPtr self) {
-                this.self = self;
+            internal ClientConnection(IntPtr self)
+            {
+                this.self = ClientConnection_claimOwnership(self);
             }
 
             public string GetPeerAddress()
             {
-                IntPtr peerAddrPtr = ClientConnection_getPeerAddress (self);
+                lock (this)
+                {
+                    if (self != IntPtr.Zero)
+                    {
+                        IntPtr peerAddrPtr = ClientConnection_getPeerAddress(self);
 
-                if (peerAddrPtr != IntPtr.Zero)
-                    return Marshal.PtrToStringAnsi (peerAddrPtr);
-                else
-                    return null;
+                        if (peerAddrPtr != IntPtr.Zero)
+                            return Marshal.PtrToStringAnsi(peerAddrPtr);
+                    }
+                }
+
+                return null;
             }
 
             public string GetLocalAddress()
             {
-                IntPtr localAddrPtr = ClientConnection_getLocalAddress(self);
+                lock (this)
+                {
+                    if (self != IntPtr.Zero)
+                    {
+                        IntPtr localAddrPtr = ClientConnection_getLocalAddress(self);
 
-                if (localAddrPtr != IntPtr.Zero)
-                    return Marshal.PtrToStringAnsi(localAddrPtr);
-                else
-                    return null;
+                        if (localAddrPtr != IntPtr.Zero)
+                            return Marshal.PtrToStringAnsi(localAddrPtr);
+                    }
+                }
+
+                return null;
             }
+
+            /// <summary>
+            /// Abort/Close the client connection
+            /// </summary>
+            /// <returns>true, when connection has been closed, false when connection was already close</returns>
+            public bool Abort()
+            {
+                lock (this)
+                {
+                    if (self != IntPtr.Zero)
+                    {
+                        return ClientConnection_abort(self);
+                    }
+                }
+
+                return false;
+            }
+
+            public void Dispose()
+            {
+                lock (this)
+                {
+                    if (self != IntPtr.Zero)
+                    {
+                        ClientConnection_release(self);
+
+                        self = IntPtr.Zero;
+                    }
+                }
+            }
+
+            ~ClientConnection ()
+            {
+                Dispose();
+            }
+
         }
 
         public class MmsGooseControlBlock

dotnet/server1/Program.cs

@@ -101,22 +101,16 @@ namespace server1
             void ConnectionCallBack(IedServer server, ClientConnection clientConnection, bool connected, object parameter)
             {
                 string allowedIp = parameter as string;
-                string ipAddress = clientConnection.GetPeerAddress();
-                if (allowedIp == ipAddress)
+                string ipAddress = clientConnection.GetLocalAddress();
+                if (allowedIp != ipAddress)
                 {
-
+                    clientConnection.Abort();
                 }
-                else
-                {
-
-                }
-
-
             }
 
             var connectionCallBack = new ConnectionIndicationHandler(ConnectionCallBack);
 
-            iedServer.SetConnectionIndicationHandler(connectionCallBack, "127.0.0.2");
+            iedServer.SetConnectionIndicationHandler(connectionCallBack, "127.0.0.1:103");
 
             iedServer.Start(102);
 

src/iec61850/inc/iec61850_server.h

@@ -825,6 +825,44 @@ ClientConnection_getLocalAddress(ClientConnection self);
 LIB61850_API void*
 ClientConnection_getSecurityToken(ClientConnection self);
 
+/**
+ * \brief Abort/Close the client connection
+ *
+ * \note After calling this function the ClientConnection instance can no longer be used.
+ *
+ * \param self the ClientConnection instance
+ *
+ * \return true, when the client connection has been closed, false otherwise
+ */
+LIB61850_API bool
+ClientConnection_abort(ClientConnection self);
+
+/**
+ * \brief Claim ownership of the ClientConnection instance to store and use it outside of a callback
+ *
+ * \note The application has to call \ref ClientConnection_release when the instance is no longer used
+ *       order to avoid memory leaks
+ *
+ * \param self the ClientConnection instance
+ *
+ * \return the connection instance when ownership is granted
+ */
+LIB61850_API ClientConnection
+ClientConnection_claimOwnership(ClientConnection self);
+
+/**
+ * \brief Release the ownership of the ClientConnection instance.
+ *
+ * This function will also destroy the ClientConnection instance when no other owners exist.
+ *
+ * \note The application should only call this function once for each call for \ref ClientConnection_claimOwnership
+ *       for a specific ClientConnection instance. The reference must not be used after calling this function!
+ *
+ * \param self the ClientConnection instance
+ */
+LIB61850_API void
+ClientConnection_release(ClientConnection self);
+
 /**
  * \brief User provided callback function that is invoked whenever a new client connects or an existing connection is closed
  *        or detected as lost.

src/iec61850/inc_private/ied_server_private.h

@@ -96,7 +96,7 @@ LIB61850_INTERNAL ClientConnection
 private_ClientConnection_create(void* serverConnectionHandle);
 
 LIB61850_INTERNAL void
-private_ClientConnection_destroy(ClientConnection self);
+private_ClientConnection_invalidate(ClientConnection self);
 
 LIB61850_INTERNAL int
 private_ClientConnection_getTasksCount(ClientConnection self);

src/iec61850/server/impl/client_connection.c

@@ -1,7 +1,7 @@
 /*
  *  client_connection.c
  *
- *  Copyright 2013-2024 Michael Zillgith
+ *  Copyright 2013-2025 Michael Zillgith
  *
  *  This file is part of libIEC61850.
  *
@@ -32,16 +32,45 @@
 
 #include "libiec61850_platform_includes.h"
 
+#if __STDC_VERSION__ >= 201112L
+#include <stdatomic.h>
+#else
+#define _TLS_OWN_CNT_SEM 1
+#endif
+
 struct sClientConnection
 {
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-    Semaphore tasksCountMutex;
+    Semaphore accessMutex;
 #endif
 
-    int tasksCount;
-    void* serverConnectionHandle;
+    int tasksCount; /* protected by accessMutex */
+    void* serverConnectionHandle; /* protected by accessMutex */
+
+#ifdef _TLS_OWN_CNT_SEM
+    #if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore ownerCountMutex;
+    #endif /*#if (CONFIG_MMS_THREADLESS_STACK != 1) */
+    int ownerCount;
+#else
+    _Atomic(int) ownerCount;
+#endif /* _TLS_OWN_CNT_SEM */
 };
 
+void
+private_ClientConnection_invalidate(ClientConnection self)
+{
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->accessMutex);
+#endif
+
+    self->serverConnectionHandle = NULL;
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->accessMutex);
+#endif
+}
+
 ClientConnection
 private_ClientConnection_create(void* serverConnectionHandle)
 {
@@ -50,9 +79,14 @@ private_ClientConnection_create(void* serverConnectionHandle)
     if (self)
     {
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-        self->tasksCountMutex = Semaphore_create(1);
+        self->accessMutex = Semaphore_create(1);
+
+        #ifdef _TLS_OWN_CNT_SEM
+        self->ownerCount = Semaphore_create(1);
+        #endif
 #endif
 
+        self->ownerCount = 1;
         self->tasksCount = 0;
         self->serverConnectionHandle = serverConnectionHandle;
     }
@@ -60,13 +94,17 @@ private_ClientConnection_create(void* serverConnectionHandle)
     return self;
 }
 
-void
+static void
 private_ClientConnection_destroy(ClientConnection self)
 {
     if (self)
     {
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-        Semaphore_destroy(self->tasksCountMutex);
+        Semaphore_destroy(self->accessMutex);
+
+        #ifdef _TLS_OWN_CNT_SEM
+        Semaphore_destroy(self->ownerCountMutex);
+        #endif
 #endif
 
         GLOBAL_FREEMEM(self);
@@ -79,13 +117,13 @@ private_ClientConnection_getTasksCount(ClientConnection self)
     int tasksCount;
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-    Semaphore_wait(self->tasksCountMutex);
+    Semaphore_wait(self->accessMutex);
 #endif
 
     tasksCount = self->tasksCount;
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-    Semaphore_post(self->tasksCountMutex);
+    Semaphore_post(self->accessMutex);
 #endif
 
     return tasksCount;
@@ -95,13 +133,13 @@ void
 private_ClientConnection_increaseTasksCount(ClientConnection self)
 {
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-    Semaphore_wait(self->tasksCountMutex);
+    Semaphore_wait(self->accessMutex);
 #endif
 
     self->tasksCount++;
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-    Semaphore_post(self->tasksCountMutex);
+    Semaphore_post(self->accessMutex);
 #endif
 }
 
@@ -109,43 +147,183 @@ void
 private_ClientConnection_decreaseTasksCount(ClientConnection self)
 {
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-    Semaphore_wait(self->tasksCountMutex);
+    Semaphore_wait(self->accessMutex);
 #endif
 
     self->tasksCount--;
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-    Semaphore_post(self->tasksCountMutex);
+    Semaphore_post(self->accessMutex);
 #endif
 }
 
 void*
 private_ClientConnection_getServerConnectionHandle(ClientConnection self)
 {
-    return self->serverConnectionHandle;
+    void* handle = NULL;
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->accessMutex);
+#endif
+
+    handle = self->serverConnectionHandle;
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->accessMutex);
+#endif
+
+    return handle;
 }
 
 const char*
 ClientConnection_getPeerAddress(ClientConnection self)
 {
-    MmsServerConnection mmsConnection = (MmsServerConnection) self->serverConnectionHandle;
+    char* peerAddress = NULL;
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->accessMutex);
+#endif
+
+    if (self->serverConnectionHandle)
+    {
+        MmsServerConnection mmsConnection = (MmsServerConnection) self->serverConnectionHandle;
+
+        peerAddress = MmsServerConnection_getClientAddress(mmsConnection);
+    }
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->accessMutex);
+#endif
 
-    return MmsServerConnection_getClientAddress(mmsConnection);
+    return peerAddress;
 }
 
 const char*
 ClientConnection_getLocalAddress(ClientConnection self)
 {
-    MmsServerConnection mmsConnection = (MmsServerConnection) self->serverConnectionHandle;
+    char* localAddress = NULL;
 
-    return MmsServerConnection_getLocalAddress(mmsConnection);
-}
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->accessMutex);
+#endif
 
+    if (self->serverConnectionHandle)
+    {
+        MmsServerConnection mmsConnection = (MmsServerConnection) self->serverConnectionHandle;
+
+        localAddress = MmsServerConnection_getLocalAddress(mmsConnection);
+    }
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->accessMutex);
+#endif
+
+    return localAddress;
+}
 
 void*
 ClientConnection_getSecurityToken(ClientConnection self)
 {
-    MmsServerConnection mmsConnection = (MmsServerConnection) self->serverConnectionHandle;
+    void* secToken = NULL;
 
-    return MmsServerConnection_getSecurityToken(mmsConnection);
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->accessMutex);
+#endif
+
+    if (self->serverConnectionHandle)
+    {
+        MmsServerConnection mmsConnection = (MmsServerConnection) self->serverConnectionHandle;
+
+        secToken =  MmsServerConnection_getSecurityToken(mmsConnection);
+    }
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->accessMutex);
+#endif
+
+    return secToken;
+}
+
+bool
+ClientConnection_abort(ClientConnection self)
+{
+    bool aborted = false;
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->accessMutex);
+#endif
+
+    if (self->serverConnectionHandle)
+    {
+        MmsServerConnection mmsConnection = (MmsServerConnection) self->serverConnectionHandle;
+
+        if (mmsConnection)
+        {
+            MmsServer mmsServer = MmsServerConnection_getServer(mmsConnection);
+
+            aborted = MmsServer_abortConnection(mmsServer, mmsConnection);
+
+            if (aborted)
+            {
+                /* remove reference to underlying connection. Instance cannot be used any longer */
+                self->serverConnectionHandle = NULL;
+            }
+        }
+    }
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->accessMutex);
+#endif
+
+    return aborted;
+}
+
+ClientConnection
+ClientConnection_claimOwnership(ClientConnection self)
+{
+#ifdef _TLS_OWN_CNT_SEM
+    #if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->ownerCountMutex);
+    #endif
+    self->ownerCount++;
+    #if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->ownerCountMutex);
+    #endif
+#else
+    atomic_fetch_add(&(self->ownerCount), 1);
+#endif
+
+    return self;
+}
+
+void
+ClientConnection_release(ClientConnection self)
+{
+    if (self)
+    {
+#ifdef _TLS_OWN_CNT_SEM
+
+        int cnt;
+
+        #if (CONFIG_MMS_THREADLESS_STACK != 1)
+        Semaphore_wait(self->ownerCountMutex);
+        #endif
+
+        cnt = self->ownerCount;
+        self->ownerCount--;
+
+        #if (CONFIG_MMS_THREADLESS_STACK != 1)
+        Semaphore_post(self->ownerCountMutex);
+        #endif
+
+        if (cnt == 1) {
+            private_ClientConnection_destroy(self);
+        }
+#else
+        if (atomic_fetch_sub(&(self->ownerCount), 1) == 1)
+        {
+            private_ClientConnection_destroy(self);
+        }
+#endif /* #ifdef _TLS_OWN_CNT_SEM */
+    }
 }

src/iec61850/server/impl/ied_server.c

@@ -808,7 +808,7 @@ IedServer_destroy(IedServer self)
         if (self->mmsMapping)
             MmsMapping_destroy(self->mmsMapping);
 
-        LinkedList_destroyDeep(self->clientConnections, (LinkedListValueDeleteFunction) private_ClientConnection_destroy);
+        LinkedList_destroyDeep(self->clientConnections, (LinkedListValueDeleteFunction) ClientConnection_release);
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
         Semaphore_destroy(self->dataModelLock);

src/iec61850/server/mms_mapping/mms_mapping.c

@@ -3354,7 +3354,9 @@ mmsConnectionHandler(void* parameter, MmsServerConnection connection, MmsServerE
         unselectAllSettingGroups(self, connection);
 #endif
 
-        private_ClientConnection_destroy(clientConnection);
+        private_ClientConnection_invalidate(clientConnection);
+
+        ClientConnection_release(clientConnection);
     }
     else if (event == MMS_SERVER_NEW_CONNECTION) {
         /* call user provided handler function */

src/mms/inc/mms_server.h

@@ -411,7 +411,10 @@ LIB61850_INTERNAL void*
 MmsServerConnection_getSecurityToken(MmsServerConnection self);
 
 LIB61850_INTERNAL void
-MmsServer_ignoreClientRequests(MmsServer self, bool enable);;
+MmsServer_ignoreClientRequests(MmsServer self, bool enable);
+
+LIB61850_INTERNAL bool
+MmsServer_abortConnection(MmsServer self, MmsServerConnection con);
 
 /**@}*/
 

src/mms/inc_private/iso_server.h

@@ -157,7 +157,11 @@ LIB61850_INTERNAL void
 IsoServer_stopListeningThreadless(IsoServer self);
 
 LIB61850_INTERNAL void
-IsoServer_closeConnection(IsoServer self, IsoConnection isoConnection);
+IsoServer_closeConnectionIndication(IsoServer self, IsoConnection isoConnection);
+
+/* Used by MmsServer instance to actively close a connection */
+LIB61850_INTERNAL bool
+IsoServer_closeConnection(IsoServer self, IsoConnection con);
 
 LIB61850_INTERNAL void
 IsoServer_destroy(IsoServer self);

src/mms/inc_private/mms_server_connection.h

@@ -112,6 +112,9 @@ MmsServerConnection_getNextRequestInvokeId(MmsServerConnection self);
 LIB61850_INTERNAL const char*
 MmsServerConnection_getFilesystemBasepath(MmsServerConnection self);
 
+LIB61850_INTERNAL MmsServer
+MmsServerConnection_getServer(MmsServerConnection self);
+
 #ifdef __cplusplus
 }
 #endif

src/mms/iso_mms/server/mms_server.c

@@ -926,3 +926,26 @@ MmsServer_ignoreClientRequests(MmsServer self, bool enable)
 {
     self->blockRequests = enable;
 }
+
+bool
+MmsServer_abortConnection(MmsServer self, MmsServerConnection con)
+{
+    if (self->isoServerList)
+    {
+        LinkedList elem = LinkedList_getNext(self->isoServerList);
+
+        while (elem)
+        {
+            IsoServer isoServer = (IsoServer) LinkedList_getData(elem);
+
+            if (IsoServer_closeConnection(isoServer, con->isoConnection))
+            {
+                return true;
+            }
+
+            elem = LinkedList_getNext(elem);
+        }
+    }
+
+    return false;
+}

src/mms/iso_mms/server/mms_server_connection.c

@@ -818,7 +818,6 @@ MmsServerConnection_init(MmsServerConnection connection, MmsServer server, IsoCo
 void
 MmsServerConnection_destroy(MmsServerConnection self)
 {
-
 #if (MMS_FILE_SERVICE == 1)
     int frsmIndex = 0;
 
@@ -925,3 +924,9 @@ MmsServerConnection_getFilesystemBasepath(MmsServerConnection self)
     return CONFIG_VIRTUAL_FILESTORE_BASEPATH;
 #endif
 }
+
+MmsServer
+MmsServerConnection_getServer(MmsServerConnection self)
+{
+    return self->server;
+}

src/mms/iso_server/iso_connection.c

@@ -479,7 +479,7 @@ handleTcpConnection(void* parameter)
     while(self->state == ISO_CON_STATE_RUNNING)
         IsoConnection_handleTcpConnection(self, false);
 
-    IsoServer_closeConnection(self->isoServer, self);
+    IsoServer_closeConnectionIndication(self->isoServer, self);
 
     finalizeIsoConnection(self);
 

src/mms/iso_server/iso_server.c

@@ -177,24 +177,31 @@ addClientConnection(IsoServer self, IsoConnection connection)
 
 }
 
-static void
+static bool
 removeClientConnection(IsoServer self, IsoConnection connection)
 {
+    bool removed = false;
+
 #if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
 
-    LinkedList_remove(self->openClientConnections, connection);
+    removed = LinkedList_remove(self->openClientConnections, connection);
 
 #if (CONFIG_MMS_SINGLE_THREADED == 1)
-    IsoConnection_removeFromHandleSet(connection, self->handleset);
+    if (removed)
+    {
+        IsoConnection_removeFromHandleSet(connection, self->handleset);
+    }
 #endif
 
-
 #else
 
     int i;
 
-    for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++) {
-        if (self->openClientConnections[i] == connection) {
+    for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++)
+    {
+        if (self->openClientConnections[i] == connection)
+        {
+            removed = true;
 
 #if (CONFIG_MMS_SINGLE_THREADED == 1)
             IsoConnection_removeFromHandleSet(connection, self->handleset);
@@ -208,6 +215,8 @@ removeClientConnection(IsoServer self, IsoConnection connection)
         }
     }
 #endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
+
+    return removed;
 }
 
 static void
@@ -221,12 +230,14 @@ removeTerminatedConnections(IsoServer self, bool isSingleThread)
 
     LinkedList openConnection = LinkedList_getNext(self->openClientConnections);
 
-    while (openConnection) {
+    while (openConnection)
+    {
         IsoConnection isoConnection = (IsoConnection) openConnection->data;
 
-        if (isSingleThread) {
-            if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_STOPPED) {
-
+        if (isSingleThread)
+        {
+            if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_STOPPED)
+            {
                 self->connectionHandler(ISO_CONNECTION_CLOSED, self->connectionHandlerParameter,
                         isoConnection);
 
@@ -234,7 +245,8 @@ removeTerminatedConnections(IsoServer self, bool isSingleThread)
             }
         }
 
-        if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_TERMINATED) {
+        if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_TERMINATED)
+        {
             removeClientConnection(self, isoConnection);
             IsoConnection_destroy(isoConnection);
             openConnection = LinkedList_getNext(self->openClientConnections);
@@ -246,14 +258,16 @@ removeTerminatedConnections(IsoServer self, bool isSingleThread)
 #else
     int i;
 
-    for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++) {
-        if (self->openClientConnections[i] != NULL) {
-
+    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) {
-
+            if (isSingleThread)
+            {
+                if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_STOPPED)
+                {
                     self->connectionHandler(ISO_CONNECTION_CLOSED, self->connectionHandlerParameter,
                             isoConnection);
 
@@ -263,7 +277,8 @@ removeTerminatedConnections(IsoServer self, bool isSingleThread)
                 }
             }
 
-            if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_TERMINATED) {
+            if (IsoConnection_getState(isoConnection) == ISO_CON_STATE_TERMINATED)
+            {
                 removeClientConnection(self, isoConnection);
                 IsoConnection_destroy(isoConnection);
             }
@@ -271,10 +286,34 @@ removeTerminatedConnections(IsoServer self, bool isSingleThread)
     }
 #endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
 
+#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
+    unlockClientConnections(self);
+#endif
+}
+
+bool
+IsoServer_closeConnection(IsoServer self, IsoConnection con)
+{
+    bool closed = false;
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
+    lockClientConnections(self);
+#endif
+
+    /*  check if the connection is handled by this IsoServer instance */
+    if (removeClientConnection(self, con))
+    {
+        closed = true;
+        printf("ISO_SERVER (%p): Connection (%p) removed\n", self, con);
+        IsoConnection_close(con);
+        IsoConnection_destroy(con);
+    }
+
 #if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED == 0)
     unlockClientConnections(self);
 #endif
 
+    return closed;
 }
 
 static void
@@ -800,7 +839,7 @@ IsoServer_stopListening(IsoServer self)
 #endif
 
 void
-IsoServer_closeConnection(IsoServer self, IsoConnection isoConnection)
+IsoServer_closeConnectionIndication(IsoServer self, IsoConnection isoConnection)
 {
     if (getState(self) != ISO_SVR_STATE_IDLE) {
         self->connectionHandler(ISO_CONNECTION_CLOSED, self->connectionHandlerParameter,