- IED server: Goose publisher - set NdsCom when data set not configured or GoEna fails because of large data set

src/iec61850/client/client_goose_control.c

@@ -569,6 +569,8 @@ IedConnection_setGoCBValues(IedConnection self, IedClientError* error, ClientGoo
     if (singleRequest) {
         LinkedList accessResults = NULL;
 
+        *error = IED_ERROR_OK;
+
         MmsConnection_writeMultipleVariables(self->connection, &mmsError, domainId, itemIds, values, &accessResults);
 
         if (accessResults != NULL) {
@@ -577,8 +579,12 @@ IedConnection_setGoCBValues(IedConnection self, IedClientError* error, ClientGoo
             while (element != NULL) {
                 MmsValue* accessResult = (MmsValue*) element->data;
 
+                MmsDataAccessError resErr = MmsValue_getDataAccessError(accessResult);
+
                 if (MmsValue_getDataAccessError(accessResult) != DATA_ACCESS_ERROR_SUCCESS) {
-                    mmsError = MMS_ERROR_ACCESS_OBJECT_NON_EXISTENT;
+
+                    *error = iedConnection_mapDataAccessErrorToIedError(resErr);
+
                     break;
                 }
 
@@ -588,8 +594,6 @@ IedConnection_setGoCBValues(IedConnection self, IedClientError* error, ClientGoo
             LinkedList_destroyDeep(accessResults, (LinkedListValueDeleteFunction) MmsValue_delete);
         }
 
-        *error = iedConnection_mapMmsErrorToIedError(mmsError);
-
         goto exit_function;
     }
     else {

src/iec61850/inc/iec61850_server.h

@@ -1607,6 +1607,9 @@ MmsGooseControlBlock_getMaxTime(MmsGooseControlBlock self);
 LIB61850_API bool
 MmsGooseControlBlock_getFixedOffs(MmsGooseControlBlock self);
 
+LIB61850_API bool
+MmsGooseControlBlock_getNdsCom(MmsGooseControlBlock self);
+
 /**@}*/
 
 /**

src/iec61850/inc_private/mms_goose.h

@@ -63,7 +63,7 @@ MmsGooseControlBlock_setStateChangePending(MmsGooseControlBlock self);
 LIB61850_INTERNAL void
 MmsGooseControlBlock_publishNewState(MmsGooseControlBlock self);
 
-LIB61850_INTERNAL void
+LIB61850_INTERNAL bool
 MmsGooseControlBlock_enable(MmsGooseControlBlock self, MmsMapping* mmsMapping);
 
 LIB61850_INTERNAL void

src/iec61850/server/mms_mapping/mms_goose.c

@@ -1,7 +1,7 @@
 /*
  *  mms_goose.c
  *
- *  Copyright 2013-2020 Michael Zillgith
+ *  Copyright 2013-2021 Michael Zillgith
  *
  *  This file is part of libIEC61850.
  *
@@ -25,6 +25,8 @@
 
 #if (CONFIG_INCLUDE_GOOSE_SUPPORT == 1)
 
+#define GOOSE_MAX_MESSAGE_SIZE 1518
+
 #include "libiec61850_platform_includes.h"
 #include "mms_mapping.h"
 #include "linked_list.h"
@@ -80,6 +82,35 @@ struct sMmsGooseControlBlock {
     bool stateChangePending;
 };
 
+static void
+setNdsCom(MmsGooseControlBlock mmsGCB, bool value)
+{
+    MmsValue* ndsComValue = MmsValue_getElement(mmsGCB->mmsValue, 4);
+
+    if (ndsComValue) {
+        MmsValue_setBoolean(ndsComValue, value);
+    }
+}
+
+static bool
+getNdsCom(MmsGooseControlBlock mmsGCB)
+{
+    bool ndsCom = true;
+
+    MmsValue* ndsComValue = MmsValue_getElement(mmsGCB->mmsValue, 4);
+
+    if (ndsComValue)
+        ndsCom = MmsValue_getBoolean(ndsComValue);
+
+    return ndsCom;
+}
+
+bool
+MmsGooseControlBlock_getNdsCom(MmsGooseControlBlock self)
+{
+    return getNdsCom(self);
+}
+
 bool
 MmsGooseControlBlock_getGoEna(MmsGooseControlBlock self)
 {
@@ -341,9 +372,27 @@ MmsGooseControlBlock_isEnabled(MmsGooseControlBlock self)
     return self->goEna;
 }
 
-void
+static int
+calculateMaxDataSetSize(DataSet* dataSet)
+{
+    int dataSetSize = 0;
+
+    DataSetEntry* dataSetEntry = dataSet->fcdas;
+
+    while (dataSetEntry) {
+        dataSetSize +=  MmsValue_getMaxEncodedSize(dataSetEntry->value);
+
+        dataSetEntry = dataSetEntry->sibling;
+    }
+
+    return dataSetSize;
+}
+
+bool
 MmsGooseControlBlock_enable(MmsGooseControlBlock self, MmsMapping* mmsMapping)
 {
+    bool retVal = false;
+
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
     Semaphore_wait(self->publisherMutex);
 #endif
@@ -388,6 +437,28 @@ MmsGooseControlBlock_enable(MmsGooseControlBlock self, MmsMapping* mmsMapping)
 
         if (self->dataSet != NULL) {
 
+            int dataSetSize = calculateMaxDataSetSize(self->dataSet);
+
+            /*  Calculate maximum GOOSE message size */
+            int maxGooseMessageSize =  26 + 51 + 6;
+            maxGooseMessageSize += strlen(self->goCBRef);
+            if (self->goId)
+                maxGooseMessageSize += strlen(self->goId);
+            else
+                maxGooseMessageSize += strlen(self->goCBRef);
+            maxGooseMessageSize += strlen(self->dataSetRef);
+
+            maxGooseMessageSize += dataSetSize;
+
+            if (maxGooseMessageSize > GOOSE_MAX_MESSAGE_SIZE) {
+                setNdsCom(self, true);
+
+#if (CONFIG_IEC61850_SERVICE_TRACKING == 1)
+                copyGCBValuesToTrackingObject(self);
+                updateGenericTrackingObjectValues(self, IEC61850_SERVICE_TYPE_SET_GOCB_VALUES, DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID);
+#endif /* (CONFIG_IEC61850_SERVICE_TRACKING == 1) */
+            }
+            else {
                 MmsValue* goEna = MmsValue_getElement(self->mmsValue, 0);
 
                 MmsValue_setBoolean(goEna, true);
@@ -451,22 +522,27 @@ MmsGooseControlBlock_enable(MmsGooseControlBlock self, MmsMapping* mmsMapping)
 
                 self->goEna = true;
 
+                retVal = true;
+
 #if (CONFIG_IEC61850_SERVICE_TRACKING == 1)
-            MmsDataAccessError retVal = DATA_ACCESS_ERROR_SUCCESS;
                 copyGCBValuesToTrackingObject(self);
-            updateGenericTrackingObjectValues(self, IEC61850_SERVICE_TYPE_SET_GOCB_VALUES, retVal);
+                updateGenericTrackingObjectValues(self, IEC61850_SERVICE_TYPE_SET_GOCB_VALUES, DATA_ACCESS_ERROR_SUCCESS);
 #endif /* (CONFIG_IEC61850_SERVICE_TRACKING == 1) */
+            }
 
         }
 
     }
     else {
+        if (DEBUG_IED_SERVER)
             printf("GoCB already enabled!\n");
     }
 
 #if (CONFIG_MMS_THREADLESS_STACK != 1)
     Semaphore_post(self->publisherMutex);
 #endif
+
+    return retVal;
 }
 
 void
@@ -754,11 +830,13 @@ GOOSE_createGOOSEControlBlocks(MmsMapping* self, MmsDomain* domain,
             mmsGCB->goId = StringUtils_copyString(gooseControlBlock->appId);
         }
 
-        if ((gooseControlBlock->dataSetName != NULL) && (gooseControlBlock->dataSetName[0] != 0))
+        if ((gooseControlBlock->dataSetName != NULL) && (gooseControlBlock->dataSetName[0] != 0)) {
         	mmsGCB->dataSetRef = createDataSetReference(MmsDomain_getName(domain),
         			logicalNode->name, gooseControlBlock->dataSetName);
-        else
+        }
+        else {
         	mmsGCB->dataSetRef = NULL;
+        }
 
         MmsValue* dataSetRef = MmsValue_getElement(gseValues, 2);
 
@@ -823,6 +901,11 @@ GOOSE_createGOOSEControlBlocks(MmsMapping* self, MmsDomain* domain,
         MmsValue* maxTime = MmsValue_getElement(gseValues, 7);
         MmsValue_setUint32(maxTime, mmsGCB->maxTime);
 
+        if (mmsGCB->dataSetRef)
+            setNdsCom(mmsGCB, false);
+        else
+            setNdsCom(mmsGCB, true);
+
         mmsGCB->mmsMapping = self;
 
         mmsGCB->stateChangePending = false;

src/iec61850/server/mms_mapping/mms_mapping.c

@@ -2276,12 +2276,18 @@ writeAccessGooseControlBlock(MmsMapping* self, MmsDomain* domain, char* variable
         if (MmsValue_getType(value) != MMS_BOOLEAN)
             return DATA_ACCESS_ERROR_TYPE_INCONSISTENT;
 
-        if (MmsValue_getBoolean(value)) {
+        if (MmsGooseControlBlock_getNdsCom(mmsGCB))
-            MmsGooseControlBlock_enable(mmsGCB, self);
+            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
 
+        if (MmsValue_getBoolean(value)) {
+            if (MmsGooseControlBlock_enable(mmsGCB, self)) {
                 if (self->goCbHandler)
                     self->goCbHandler(mmsGCB, IEC61850_GOCB_EVENT_ENABLE, self->goCbHandlerParameter);
             }
+            else {
+                return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
+            }
+        }
         else {
             MmsGooseControlBlock_disable(mmsGCB, self);
 
@@ -3618,7 +3624,6 @@ MmsMapping_triggerReportObservers(MmsMapping* self, MmsValue* value, int flag)
             }
 
             if (DataSet_isMemberValue(rc->dataSet, value, &index)) {
-
                 ReportControl_valueUpdated(rc, index, flag, modelLocked);
             }
         }
@@ -3665,7 +3670,10 @@ MmsMapping_enableGoosePublishing(MmsMapping* self)
     while (element) {
         MmsGooseControlBlock gcb = (MmsGooseControlBlock) LinkedList_getData(element);
 
-        MmsGooseControlBlock_enable(gcb, self);
+        if (MmsGooseControlBlock_enable(gcb, self) == false) {
+            if (DEBUG_IED_SERVER)
+                printf("IED_SERVER: failed to enable GoCB %s\n", MmsGooseControlBlock_getName(gcb));
+        }
 
         element = LinkedList_getNext(element);
     }

src/mms/inc/mms_value.h

@@ -985,6 +985,24 @@ MmsValue_decodeMmsData(uint8_t* buffer, int bufPos, int bufferLength, int* endBu
 LIB61850_API int
 MmsValue_encodeMmsData(MmsValue* self, uint8_t* buffer, int bufPos, bool encode);
 
+/**
+ * \brief Get the maximum possible BER encoded size of the MMS data element
+ *
+ * \param self the MmsValue instance
+ *
+ * \return the maximum encoded size in bytes of the MMS data element
+ */
+LIB61850_API int
+MmsValue_getMaxEncodedSize(MmsValue* self);
+
+/**
+ * \brief Calculate the maximum encoded size of a variable of this type
+ *
+ * \param self the MMS variable specification instance
+ */
+LIB61850_API int
+MmsVariableSpecification_getMaxEncodedSize(MmsVariableSpecification* self);
+
 /**@}*/
 
 /**@}*/

src/mms/iso_mms/server/mms_access_result.c

@@ -334,6 +334,168 @@ exit_with_error:
     return NULL;
 }
 
+static int
+MmsValue_getMaxStructSize(MmsValue* self)
+{
+    int componentsSize = 0;
+    int i;
+    int size;
+
+    int componentCount = self->value.structure.size;
+
+    MmsValue** components = self->value.structure.components;
+
+    for (i = 0; i < componentCount; i++)
+        componentsSize += MmsValue_getMaxEncodedSize(components[i]);
+
+    size = 1 + componentsSize + BerEncoder_determineLengthSize(componentsSize);
+
+    return size;
+}
+
+int
+MmsValue_getMaxEncodedSize(MmsValue* self)
+{
+    int size = 0;
+    int elementSize = 0;
+
+    switch (self->type)
+    {
+    case MMS_STRUCTURE:
+        size = MmsValue_getMaxStructSize(self);
+        break;
+    case MMS_ARRAY:
+        size = MmsValue_getMaxStructSize(self);
+        break;
+    case MMS_BOOLEAN:
+        size = 3;
+        break;
+    case MMS_DATA_ACCESS_ERROR:
+        size = 7; /* TL * size of uint32 max */
+        break;
+    case MMS_VISIBLE_STRING:
+        elementSize = abs(self->value.visibleString.size);
+        size = 1 + elementSize + BerEncoder_determineLengthSize(elementSize);
+        break;
+    case MMS_UNSIGNED:
+        size = 2 + self->value.integer->maxSize;
+        break;
+    case MMS_INTEGER:
+        size = 2 + self->value.integer->maxSize;
+        break;
+    case MMS_UTC_TIME:
+        size = 10;
+        break;
+    case MMS_BIT_STRING:
+        elementSize = abs(self->value.bitString.size);
+        size = BerEncoder_determineEncodedBitStringSize(elementSize);
+        break;
+    case MMS_BINARY_TIME:
+        size = 2 + self->value.binaryTime.size;
+        break;
+    case MMS_OCTET_STRING:
+        elementSize = abs(self->value.octetString.maxSize);
+        size = 1 + BerEncoder_determineLengthSize(elementSize) + elementSize;
+        break;
+    case MMS_FLOAT:
+        elementSize = (self->value.floatingPoint.formatWidth / 8) + 1;
+        size = elementSize + 2; /* 2 for tag and length */
+        break;
+    case MMS_STRING:
+        elementSize = abs(self->value.visibleString.size);
+        size = 1 + elementSize + BerEncoder_determineLengthSize(elementSize);
+        break;
+    default:
+        if (DEBUG_MMS_SERVER)
+            printf("MmsVariableSpecification_getMaxEncodedSize: error unsupported type!\n");
+        break;
+    }
+
+    return size;
+}
+
+static int
+getMaxStructSize(MmsVariableSpecification* variable)
+{
+    int componentsSize = 0;
+    int i;
+    int size;
+
+    int componentCount = variable->typeSpec.structure.elementCount;
+
+    MmsVariableSpecification** components = variable->typeSpec.structure.elements;
+
+    for (i = 0; i < componentCount; i++)
+        componentsSize += MmsVariableSpecification_getMaxEncodedSize(components[i]);
+
+    size = 1 + componentsSize + BerEncoder_determineLengthSize(componentsSize);
+
+    return size;
+}
+
+int
+MmsVariableSpecification_getMaxEncodedSize(MmsVariableSpecification* self)
+{
+    int size = 0;
+    int elementSize = 0;
+
+    switch (self->type)
+    {
+    case MMS_STRUCTURE:
+        size = getMaxStructSize(self);
+        break;
+    case MMS_ARRAY:
+        elementSize = MmsVariableSpecification_getMaxEncodedSize(self->typeSpec.array.elementTypeSpec)
+            * self->typeSpec.array.elementCount;
+        size = 1 + elementSize + BerEncoder_determineLengthSize(elementSize);
+        break;
+    case MMS_BOOLEAN:
+        size = 3;
+        break;
+    case MMS_DATA_ACCESS_ERROR:
+        size = 7; /* TL * size of uint32 max */
+        break;
+    case MMS_VISIBLE_STRING:
+        elementSize = abs(self->typeSpec.visibleString);
+        size = 1 + elementSize + BerEncoder_determineLengthSize(elementSize);
+        break;
+    case MMS_UNSIGNED:
+        size = 2 + (self->typeSpec.unsignedInteger / 8) + 1;
+        break;
+    case MMS_INTEGER:
+        size = 2 + (self->typeSpec.integer / 8) + 1;
+        break;
+    case MMS_UTC_TIME:
+        size = 10;
+        break;
+    case MMS_BIT_STRING:
+        elementSize = abs(self->typeSpec.bitString);
+        size = BerEncoder_determineEncodedBitStringSize(elementSize);
+        break;
+    case MMS_BINARY_TIME:
+        size = 2 + self->typeSpec.binaryTime;
+        break;
+    case MMS_OCTET_STRING:
+        elementSize = abs(self->typeSpec.octetString);
+        size = 1 + BerEncoder_determineLengthSize(elementSize) + elementSize;
+        break;
+    case MMS_FLOAT:
+        elementSize = (self->typeSpec.floatingpoint.formatWidth / 8) + 1;
+        size = elementSize + 2; /* 2 for tag and length */
+        break;
+    case MMS_STRING:
+        elementSize = abs(self->typeSpec.mmsString);
+        size = 1 + elementSize + BerEncoder_determineLengthSize(elementSize);
+        break;
+    default:
+        if (DEBUG_MMS_SERVER)
+            printf("MmsVariableSpecification_getMaxEncodedSize: error unsupported type!\n");
+        break;
+    }
+
+    return size;
+}
+
 int
 MmsValue_encodeMmsData(MmsValue* self, uint8_t* buffer, int bufPos, bool encode)
 {
@@ -444,7 +606,7 @@ MmsValue_encodeMmsData(MmsValue* self, uint8_t* buffer, int bufPos, bool encode)
         break;
     default:
         if (DEBUG_MMS_SERVER)
-            printf("encodeAccessResult: error unsupported type!\n");
+            printf("MmsValue_encodeMmsData: error unsupported type!\n");
         size = 0;
         break;
     }