From c98a2b0baa5d7ad5be0a9d1fb0c08651b5382183 Mon Sep 17 00:00:00 2001
From: Michael Zillgith <michael.zillgith@mz-automation.de>
Date: Tue, 26 Mar 2019 12:06:02 +0100
Subject: [PATCH] - MMS server: fixed connection handling problem in
 multi-threaded mode

---
 src/iec61850/inc_private/ied_server_private.h |  1 +
 src/iec61850/server/impl/ied_server.c         | 26 +++++++++++++++++++
 src/mms/inc_private/iso_server_private.h      |  3 +++
 src/mms/iso_mms/server/mms_server.c           |  1 -
 src/mms/iso_server/iso_connection.c           | 14 +++++++---
 src/mms/iso_server/iso_server.c               |  1 +
 6 files changed, 42 insertions(+), 4 deletions(-)

diff --git a/src/iec61850/inc_private/ied_server_private.h b/src/iec61850/inc_private/ied_server_private.h
index 79f83c90..ce780b5a 100644
--- a/src/iec61850/inc_private/ied_server_private.h
+++ b/src/iec61850/inc_private/ied_server_private.h
@@ -49,6 +49,7 @@ struct sIedServer
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
     Semaphore dataModelLock;
+    Semaphore clientConnectionsLock;
 #endif
 
 #if (CONFIG_MMS_SERVER_CONFIG_SERVICES_AT_RUNTIME == 1)
diff --git a/src/iec61850/server/impl/ied_server.c b/src/iec61850/server/impl/ied_server.c
index 5ca28a5b..5cf79de2 100644
--- a/src/iec61850/server/impl/ied_server.c
+++ b/src/iec61850/server/impl/ied_server.c
@@ -427,6 +427,7 @@ IedServer_createWithConfig(IedModel* dataModel, TLSConfiguration tlsConfiguratio
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
         self->dataModelLock = Semaphore_create(1);
+        self->clientConnectionsLock = Semaphore_create(1);
 #endif /* (CONFIG_MMS_SERVER_CONFIG_SERVICES_AT_RUNTIME == 1) */
 
 #if (CONFIG_IEC61850_REPORT_SERVICE == 1)
@@ -539,6 +540,7 @@ IedServer_destroy(IedServer self)
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
     Semaphore_destroy(self->dataModelLock);
+    Semaphore_destroy(self->clientConnectionsLock);
 #endif
 
     GLOBAL_FREEMEM(self);
@@ -1495,6 +1497,10 @@ IedServer_setLogStorage(IedServer self, const char* logRef, LogStorage logStorag
 ClientConnection
 private_IedServer_getClientConnectionByHandle(IedServer self, void* serverConnectionHandle)
 {
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->clientConnectionsLock);
+#endif
+
     LinkedList element = LinkedList_getNext(self->clientConnections);
     ClientConnection matchingConnection = NULL;
 
@@ -1509,19 +1515,39 @@ private_IedServer_getClientConnectionByHandle(IedServer self, void* serverConnec
         element = LinkedList_getNext(element);
     }
 
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->clientConnectionsLock);
+#endif
+
     return matchingConnection;
 }
 
 void
 private_IedServer_addNewClientConnection(IedServer self, ClientConnection newClientConnection)
 {
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->clientConnectionsLock);
+#endif
+
     LinkedList_add(self->clientConnections, (void*) newClientConnection);
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->clientConnectionsLock);
+#endif
 }
 
 void
 private_IedServer_removeClientConnection(IedServer self, ClientConnection clientConnection)
 {
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_wait(self->clientConnectionsLock);
+#endif
+
     LinkedList_remove(self->clientConnections, clientConnection);
+
+#if (CONFIG_MMS_THREADLESS_STACK != 1)
+    Semaphore_post(self->clientConnectionsLock);
+#endif
 }
 
 
diff --git a/src/mms/inc_private/iso_server_private.h b/src/mms/inc_private/iso_server_private.h
index dfe0d17a..4e9e45d7 100644
--- a/src/mms/inc_private/iso_server_private.h
+++ b/src/mms/inc_private/iso_server_private.h
@@ -30,6 +30,9 @@
 LIB61850_INTERNAL IsoConnection
 IsoConnection_create(Socket socket, IsoServer isoServer);
 
+LIB61850_INTERNAL void
+IsoConnection_start(IsoConnection self);
+
 LIB61850_INTERNAL void
 IsoConnection_destroy(IsoConnection self);
 
diff --git a/src/mms/iso_mms/server/mms_server.c b/src/mms/iso_mms/server/mms_server.c
index dfdee698..b21499af 100644
--- a/src/mms/iso_mms/server/mms_server.c
+++ b/src/mms/iso_mms/server/mms_server.c
@@ -410,7 +410,6 @@ isoConnectionIndicationHandler(IsoConnectionIndication indication,
     if (indication == ISO_CONNECTION_OPENED) {
         MmsServerConnection mmsCon = MmsServerConnection_init(0, self, connection);
 
-
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
         Semaphore_wait(self->openConnectionsLock);
 #endif
diff --git a/src/mms/iso_server/iso_connection.c b/src/mms/iso_server/iso_connection.c
index 6ad6c7e9..beb8330d 100644
--- a/src/mms/iso_server/iso_connection.c
+++ b/src/mms/iso_server/iso_connection.c
@@ -93,7 +93,7 @@ static void
 finalizeIsoConnection(IsoConnection self)
 {
     if (DEBUG_ISO_SERVER)
-        printf("ISO_SERVER: finalizeIsoConnection --> close transport connection\n");
+        printf("ISO_SERVER: finalizeIsoConnection (%p)--> close transport connection\n", self);
 
     IsoServer_closeConnection(self->isoServer, self);
 
@@ -515,14 +515,22 @@ 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);
-
-    Thread_start(self->thread);
 #endif
 #endif
 
     return self;
 }
 
+void
+IsoConnection_start(IsoConnection self)
+{
+#if (CONFIG_MMS_SINGLE_THREADED == 0)
+#if (CONFIG_MMS_THREADLESS_STACK == 0)
+    Thread_start(self->thread);
+#endif
+#endif
+}
+
 void
 IsoConnection_destroy(IsoConnection self)
 {
diff --git a/src/mms/iso_server/iso_server.c b/src/mms/iso_server/iso_server.c
index 65d434ec..413ac7e2 100644
--- a/src/mms/iso_server/iso_server.c
+++ b/src/mms/iso_server/iso_server.c
@@ -410,6 +410,7 @@ handleIsoConnections(IsoServer self)
             self->connectionHandler(ISO_CONNECTION_OPENED, self->connectionHandlerParameter,
                     isoConnection);
 
+            IsoConnection_start(isoConnection);
         }
 
     }