diff --git a/src/iec61850/client/client_goose_control.c b/src/iec61850/client/client_goose_control.c index 7fa44444..f6e0743a 100644 --- a/src/iec61850/client/client_goose_control.c +++ b/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 { diff --git a/src/iec61850/inc/iec61850_server.h b/src/iec61850/inc/iec61850_server.h index 92e55429..098bc1f2 100644 --- a/src/iec61850/inc/iec61850_server.h +++ b/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); + /**@}*/ /** diff --git a/src/iec61850/inc_private/mms_goose.h b/src/iec61850/inc_private/mms_goose.h index 65faa077..98923c2b 100644 --- a/src/iec61850/inc_private/mms_goose.h +++ b/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 diff --git a/src/iec61850/server/mms_mapping/mms_goose.c b/src/iec61850/server/mms_mapping/mms_goose.c index 5a9ead7d..df7a79ac 100644 --- a/src/iec61850/server/mms_mapping/mms_goose.c +++ b/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,85 +437,112 @@ MmsGooseControlBlock_enable(MmsGooseControlBlock self, MmsMapping* mmsMapping) if (self->dataSet != NULL) { - MmsValue* goEna = MmsValue_getElement(self->mmsValue, 0); + int dataSetSize = calculateMaxDataSetSize(self->dataSet); - MmsValue_setBoolean(goEna, true); + /* 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; - MmsValue* dstAddress = MmsValue_getElement(self->mmsValue, 5); + if (maxGooseMessageSize > GOOSE_MAX_MESSAGE_SIZE) { + setNdsCom(self, true); - CommParameters commParameters; - commParameters.appId = MmsValue_toInt32(MmsValue_getElement(dstAddress, 3)); - commParameters.vlanId = MmsValue_toInt32(MmsValue_getElement(dstAddress, 2)); - commParameters.vlanPriority = MmsValue_toInt32(MmsValue_getElement(dstAddress, 1)); +#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* macAddress = MmsValue_getElement(dstAddress, 0); + MmsValue_setBoolean(goEna, true); - memcpy(commParameters.dstAddress, MmsValue_getOctetStringBuffer(macAddress), 6); - if (mmsMapping->useIntegratedPublisher) { + MmsValue* dstAddress = MmsValue_getElement(self->mmsValue, 5); - if (self->gooseInterfaceId) - self->publisher = GoosePublisher_createEx(&commParameters, self->gooseInterfaceId, self->useVlanTag); - else - self->publisher = GoosePublisher_createEx(&commParameters, self->mmsMapping->gooseInterfaceId, self->useVlanTag); + CommParameters commParameters; + commParameters.appId = MmsValue_toInt32(MmsValue_getElement(dstAddress, 3)); + commParameters.vlanId = MmsValue_toInt32(MmsValue_getElement(dstAddress, 2)); + commParameters.vlanPriority = MmsValue_toInt32(MmsValue_getElement(dstAddress, 1)); - if (self->publisher) { - self->minTime = MmsValue_toUint32(MmsValue_getElement(self->mmsValue, 6)); - self->maxTime = MmsValue_toUint32(MmsValue_getElement(self->mmsValue, 7)); + MmsValue* macAddress = MmsValue_getElement(dstAddress, 0); - GoosePublisher_setTimeAllowedToLive(self->publisher, self->maxTime * 3); + memcpy(commParameters.dstAddress, MmsValue_getOctetStringBuffer(macAddress), 6); + + if (mmsMapping->useIntegratedPublisher) { - GoosePublisher_setDataSetRef(self->publisher, self->dataSetRef); + if (self->gooseInterfaceId) + self->publisher = GoosePublisher_createEx(&commParameters, self->gooseInterfaceId, self->useVlanTag); + else + self->publisher = GoosePublisher_createEx(&commParameters, self->mmsMapping->gooseInterfaceId, self->useVlanTag); - GoosePublisher_setGoCbRef(self->publisher, self->goCBRef); + if (self->publisher) { + self->minTime = MmsValue_toUint32(MmsValue_getElement(self->mmsValue, 6)); + self->maxTime = MmsValue_toUint32(MmsValue_getElement(self->mmsValue, 7)); - uint32_t confRev = MmsValue_toUint32(MmsValue_getElement(self->mmsValue, 3)); + GoosePublisher_setTimeAllowedToLive(self->publisher, self->maxTime * 3); - GoosePublisher_setConfRev(self->publisher, confRev); + GoosePublisher_setDataSetRef(self->publisher, self->dataSetRef); - bool needsCom = MmsValue_getBoolean(MmsValue_getElement(self->mmsValue, 4)); + GoosePublisher_setGoCbRef(self->publisher, self->goCBRef); - GoosePublisher_setNeedsCommission(self->publisher, needsCom); + uint32_t confRev = MmsValue_toUint32(MmsValue_getElement(self->mmsValue, 3)); - if (self->goId != NULL) - GoosePublisher_setGoID(self->publisher, self->goId); + GoosePublisher_setConfRev(self->publisher, confRev); - /* prepare data set values */ - self->dataSetValues = LinkedList_create(); + bool needsCom = MmsValue_getBoolean(MmsValue_getElement(self->mmsValue, 4)); - DataSetEntry* dataSetEntry = self->dataSet->fcdas; + GoosePublisher_setNeedsCommission(self->publisher, needsCom); - while (dataSetEntry != NULL) { - LinkedList_add(self->dataSetValues, dataSetEntry->value); - dataSetEntry = dataSetEntry->sibling; - } + if (self->goId != NULL) + GoosePublisher_setGoID(self->publisher, self->goId); + /* prepare data set values */ + self->dataSetValues = LinkedList_create(); + + DataSetEntry* dataSetEntry = self->dataSet->fcdas; + + while (dataSetEntry != NULL) { + LinkedList_add(self->dataSetValues, dataSetEntry->value); + dataSetEntry = dataSetEntry->sibling; + } + + } + else { + if (DEBUG_IED_SERVER) + printf("IED_SERVER: Failed to create GOOSE publisher!\n"); + } } - else { - if (DEBUG_IED_SERVER) - printf("IED_SERVER: Failed to create GOOSE publisher!\n"); - } - } - self->goEna = true; + 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); + copyGCBValuesToTrackingObject(self); + updateGenericTrackingObjectValues(self, IEC61850_SERVICE_TYPE_SET_GOCB_VALUES, DATA_ACCESS_ERROR_SUCCESS); #endif /* (CONFIG_IEC61850_SERVICE_TRACKING == 1) */ + } } } else { - printf("GoCB already enabled!\n"); + 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; diff --git a/src/iec61850/server/mms_mapping/mms_mapping.c b/src/iec61850/server/mms_mapping/mms_mapping.c index 32471c8f..131ac030 100644 --- a/src/iec61850/server/mms_mapping/mms_mapping.c +++ b/src/iec61850/server/mms_mapping/mms_mapping.c @@ -2276,11 +2276,17 @@ writeAccessGooseControlBlock(MmsMapping* self, MmsDomain* domain, char* variable if (MmsValue_getType(value) != MMS_BOOLEAN) return DATA_ACCESS_ERROR_TYPE_INCONSISTENT; - if (MmsValue_getBoolean(value)) { - MmsGooseControlBlock_enable(mmsGCB, self); + if (MmsGooseControlBlock_getNdsCom(mmsGCB)) + return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID; - if (self->goCbHandler) - self->goCbHandler(mmsGCB, IEC61850_GOCB_EVENT_ENABLE, self->goCbHandlerParameter); + 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); } diff --git a/src/mms/inc/mms_value.h b/src/mms/inc/mms_value.h index 57e2f39e..28b1fab4 100644 --- a/src/mms/inc/mms_value.h +++ b/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); + /**@}*/ /**@}*/ diff --git a/src/mms/iso_mms/server/mms_access_result.c b/src/mms/iso_mms/server/mms_access_result.c index d7d78975..b17461f0 100644 --- a/src/mms/iso_mms/server/mms_access_result.c +++ b/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; }