From a5f3e79ad1689fc33b429e94a7f34fa7497f5d28 Mon Sep 17 00:00:00 2001
From: Michael Zillgith <michael.zillgith@mz-automation.de>
Date: Tue, 9 Mar 2021 17:11:20 +0100
Subject: [PATCH] - IED server: integrated GOOSE publisher - lock data model
 during GOOSE retransmission to avoid corrupted GOOSE data

---
 src/iec61850/inc_private/mms_goose.h          |  2 +-
 src/iec61850/server/mms_mapping/mms_goose.c   | 47 ++++++++++++-------
 src/iec61850/server/mms_mapping/mms_mapping.c |  2 +-
 3 files changed, 31 insertions(+), 20 deletions(-)

diff --git a/src/iec61850/inc_private/mms_goose.h b/src/iec61850/inc_private/mms_goose.h
index 326e35dc..ab2cdfcd 100644
--- a/src/iec61850/inc_private/mms_goose.h
+++ b/src/iec61850/inc_private/mms_goose.h
@@ -66,7 +66,7 @@ LIB61850_INTERNAL bool
 MmsGooseControlBlock_isEnabled(MmsGooseControlBlock self);
 
 LIB61850_INTERNAL void
-MmsGooseControlBlock_checkAndPublish(MmsGooseControlBlock self, uint64_t currentTime);
+MmsGooseControlBlock_checkAndPublish(MmsGooseControlBlock self, uint64_t currentTime, MmsMapping* mapping);
 
 LIB61850_INTERNAL void
 MmsGooseControlBlock_setStateChangePending(MmsGooseControlBlock self);
diff --git a/src/iec61850/server/mms_mapping/mms_goose.c b/src/iec61850/server/mms_mapping/mms_goose.c
index 4df50074..405a8e17 100644
--- a/src/iec61850/server/mms_mapping/mms_goose.c
+++ b/src/iec61850/server/mms_mapping/mms_goose.c
@@ -335,39 +335,50 @@ MmsGooseControlBlock_disable(MmsGooseControlBlock self)
 
 
 void
-MmsGooseControlBlock_checkAndPublish(MmsGooseControlBlock self, uint64_t currentTime)
+MmsGooseControlBlock_checkAndPublish(MmsGooseControlBlock self, uint64_t currentTime, MmsMapping* mapping)
 {
     if (currentTime >= self->nextPublishTime) {
 
+            IedServer_lockDataModel(mapping->iedServer);
+
+            if (currentTime >= self->nextPublishTime) {
+
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-        Semaphore_wait(self->publisherMutex);
+                Semaphore_wait(self->publisherMutex);
 #endif
 
-        GoosePublisher_publish(self->publisher, self->dataSetValues);
+                GoosePublisher_publish(self->publisher, self->dataSetValues);
+
+                if (self->retransmissionsLeft > 0) {
+                    self->nextPublishTime = currentTime + self->minTime;
 
         if (self->retransmissionsLeft > 0) {
             self->nextPublishTime = currentTime + self->minTime;
 
+                    if (self->retransmissionsLeft > 1)
+                        GoosePublisher_setTimeAllowedToLive(self->publisher, self->minTime * 3);
+                    else
+                        GoosePublisher_setTimeAllowedToLive(self->publisher, self->maxTime * 3);
 
-            if (self->retransmissionsLeft > 1)
-                GoosePublisher_setTimeAllowedToLive(self->publisher, self->minTime * 3);
-            else
-                GoosePublisher_setTimeAllowedToLive(self->publisher, self->maxTime * 3);
-
-            self->retransmissionsLeft--;
-        }
-        else {
-            GoosePublisher_setTimeAllowedToLive(self->publisher, self->maxTime * 3);
+                    self->retransmissionsLeft--;
+                }
+                else {
+                    GoosePublisher_setTimeAllowedToLive(self->publisher, self->maxTime * 3);
 
-            self->nextPublishTime = currentTime + self->maxTime;
-        }
+                    self->nextPublishTime = currentTime + self->maxTime;
+                }
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
-        Semaphore_post(self->publisherMutex);
+                Semaphore_post(self->publisherMutex);
 #endif
-    }
-    else if ((self->nextPublishTime - currentTime) > ((uint32_t) self->maxTime * 2)) {
-        self->nextPublishTime = currentTime + self->minTime;
+            }
+
+            IedServer_unlockDataModel(mapping->iedServer);
+
+        }
+        else if ((self->nextPublishTime - currentTime) > ((uint32_t) self->maxTime * 2)) {
+            self->nextPublishTime = currentTime + self->minTime;
+        }
     }
 }
 
diff --git a/src/iec61850/server/mms_mapping/mms_mapping.c b/src/iec61850/server/mms_mapping/mms_mapping.c
index 75a0031f..4677be69 100644
--- a/src/iec61850/server/mms_mapping/mms_mapping.c
+++ b/src/iec61850/server/mms_mapping/mms_mapping.c
@@ -3015,7 +3015,7 @@ GOOSE_processGooseEvents(MmsMapping* self, uint64_t currentTimeInMs)
         MmsGooseControlBlock mmsGCB = (MmsGooseControlBlock) element->data;
 
         if (MmsGooseControlBlock_isEnabled(mmsGCB)) {
-            MmsGooseControlBlock_checkAndPublish(mmsGCB, currentTimeInMs);
+            MmsGooseControlBlock_checkAndPublish(mmsGCB, currentTimeInMs, self);
         }
 
         element = LinkedList_getNext(element);