diff --git a/examples/sv_publisher/sv_publisher_example.c b/examples/sv_publisher/sv_publisher_example.c index 4b41401c..b1d69b68 100644 --- a/examples/sv_publisher/sv_publisher_example.c +++ b/examples/sv_publisher/sv_publisher_example.c @@ -48,6 +48,8 @@ main(int argc, char** argv) int ts2 = SVPublisher_ASDU_addTimestamp(asdu2); SVPublisher_setupComplete(svPublisher); + SVPublisher_ASDU_setSmpSynch(asdu1, 2); + SVPublisher_ASDU_setSmpSynch(asdu2, 1); float fVal1 = 1234.5678f; float fVal2 = 0.12345f; diff --git a/examples/sv_subscriber/sv_subscriber_example.c b/examples/sv_subscriber/sv_subscriber_example.c index d27108b9..8da223bf 100644 --- a/examples/sv_subscriber/sv_subscriber_example.c +++ b/examples/sv_subscriber/sv_subscriber_example.c @@ -32,6 +32,7 @@ svUpdateListener (SVSubscriber subscriber, void* parameter, SVSubscriber_ASDU as printf(" smpCnt: %i\n", SVSubscriber_ASDU_getSmpCnt(asdu)); printf(" confRev: %u\n", SVSubscriber_ASDU_getConfRev(asdu)); + printf(" smpSynch: %u\n", SVSubscriber_ASDU_getSmpSynch(asdu)); /* * Access to the data requires a priori knowledge of the data set. diff --git a/hal/serial/linux/serial_port_linux.c b/hal/serial/linux/serial_port_linux.c index 15e57940..9da1831e 100644 --- a/hal/serial/linux/serial_port_linux.c +++ b/hal/serial/linux/serial_port_linux.c @@ -30,6 +30,7 @@ #include #include #include +#include #include "hal_serial.h" #include "hal_time.h" diff --git a/src/iec61850/client/client_report_control.c b/src/iec61850/client/client_report_control.c index 390863e4..085e4a75 100644 --- a/src/iec61850/client/client_report_control.c +++ b/src/iec61850/client/client_report_control.c @@ -698,11 +698,22 @@ IedConnection_getRCBValues(IedConnection self, IedClientError* error, const char if (returnRcb == NULL) returnRcb = ClientReportControlBlock_create(rcbReference); - clientReportControlBlock_updateValues(returnRcb, rcb); + if (clientReportControlBlock_updateValues(returnRcb, rcb)) { + *error = IED_ERROR_OK; + } + else { + if (DEBUG_IED_CLIENT) + printf("DEBUG_IED_CLIENT: getRCBValues returned wrong type!\n"); - MmsValue_delete(rcb); + *error = IED_ERROR_TYPE_INCONSISTENT; - *error = IED_ERROR_OK; + if (updateRcb == NULL) { + ClientReportControlBlock_destroy(returnRcb); + returnRcb = NULL; + } + } + + MmsValue_delete(rcb); return returnRcb; } diff --git a/src/iec61850/client/ied_connection.c b/src/iec61850/client/ied_connection.c index 3ca81e11..28f28660 100644 --- a/src/iec61850/client/ied_connection.c +++ b/src/iec61850/client/ied_connection.c @@ -1951,10 +1951,16 @@ IedConnection_getFile(IedConnection self, IedClientError* error, const char* fil return clientFileReadHandler.byteReceived; } - static void mmsConnectionFileCloseHandler (uint32_t invokeId, void* parameter, MmsError mmsError, bool success) { + (void)success; + + if (mmsError != MMS_ERROR_NONE) { + if (DEBUG_IED_CLIENT) + printf("IED_CLIENT: failed to close file error: %i (mms-error: %i)\n", iedConnection_mapMmsErrorToIedError(mmsError), mmsError); + } + IedConnection self = (IedConnection) parameter; IedConnectionOutstandingCall call = iedConnection_lookupOutstandingCall(self, invokeId); @@ -1985,11 +1991,19 @@ mmsConnectionFileReadHandler (uint32_t invokeId, void* parameter, MmsError mmsEr handler(call->specificParameter2.getFileInfo.originalInvokeId, call->callbackParameter, err, invokeId, NULL, 0, false); - /* close file */ - call->invokeId = MmsConnection_fileCloseAsync(self->connection, &mmsError, frsmId, mmsConnectionFileCloseHandler, self); + if (mmsError != MMS_ERROR_SERVICE_TIMEOUT) { + /* close file */ + call->invokeId = MmsConnection_fileCloseAsync(self->connection, &mmsError, frsmId, mmsConnectionFileCloseHandler, self); + + if (mmsError != MMS_ERROR_NONE) + iedConnection_releaseOutstandingCall(self, call); + } + else { + if (DEBUG_IED_CLIENT) + printf("IED_CLIENT: getFile timeout -> stop download\n"); - if (mmsError != MMS_ERROR_NONE) iedConnection_releaseOutstandingCall(self, call); + } } else { bool cont = handler(call->specificParameter2.getFileInfo.originalInvokeId, call->callbackParameter, IED_ERROR_OK, invokeId, buffer, byteReceived, moreFollows); diff --git a/src/mms/inc_private/iso_client_connection.h b/src/mms/inc_private/iso_client_connection.h index 1925cd2b..9a09b4f9 100644 --- a/src/mms/inc_private/iso_client_connection.h +++ b/src/mms/inc_private/iso_client_connection.h @@ -44,7 +44,7 @@ typedef enum ISO_IND_TICK } IsoIndication; -typedef void* +typedef bool (*IsoIndicationCallback)(IsoIndication indication, void* param, ByteBuffer* payload); /** diff --git a/src/mms/inc_private/mms_server_internal.h b/src/mms/inc_private/mms_server_internal.h index 23430d4d..ce144dfd 100644 --- a/src/mms/inc_private/mms_server_internal.h +++ b/src/mms/inc_private/mms_server_internal.h @@ -186,6 +186,7 @@ struct sMmsServerConnection { int maxServOutstandingCalling; int maxServOutstandingCalled; int dataStructureNestingLevel; + uint8_t negotiatedParameterCBC[2]; uint32_t maxPduSize; /* local detail */ IsoConnection isoConnection; MmsServer server; @@ -254,6 +255,9 @@ mmsServer_createConfirmedResponse(uint32_t invokeId); LIB61850_INTERNAL void mmsMsg_createServiceErrorPdu(uint32_t invokeId, ByteBuffer* response, MmsError errorType); +LIB61850_INTERNAL void +mmsMsg_createInitiateErrorPdu(ByteBuffer* response, uint8_t initiateErrorCode); + LIB61850_INTERNAL void mmsServer_createServiceErrorPduWithServiceSpecificInfo(uint32_t invokeId, ByteBuffer* response, MmsError errorType, uint8_t* serviceSpecificInfo, int serviceSpecficInfoLength); diff --git a/src/mms/iso_client/iso_client_connection.c b/src/mms/iso_client/iso_client_connection.c index f5462265..5aa7f803 100644 --- a/src/mms/iso_client/iso_client_connection.c +++ b/src/mms/iso_client/iso_client_connection.c @@ -499,7 +499,9 @@ IsoClientConnection_handleConnection(IsoClientConnection self) setState(self, STATE_CONNECTED); nextState = INT_STATE_WAIT_FOR_DATA_MSG; - self->callback(ISO_IND_ASSOCIATION_SUCCESS, self->callbackParameter, self->receivePayloadBuffer); + if (self->callback(ISO_IND_ASSOCIATION_SUCCESS, self->callbackParameter, self->receivePayloadBuffer) == false) { + nextState = INT_STATE_CLOSE_ON_ERROR; + } CotpConnection_resetPayload(self->cotpConnection); } diff --git a/src/mms/iso_mms/client/mms_client_connection.c b/src/mms/iso_mms/client/mms_client_connection.c index e5d0d5e0..d88d3e15 100644 --- a/src/mms/iso_mms/client/mms_client_connection.c +++ b/src/mms/iso_mms/client/mms_client_connection.c @@ -981,7 +981,7 @@ handleAsyncResponse(MmsConnection self, ByteBuffer* response, uint32_t bufPos, M removeFromOutstandingCalls(self, outstandingCall->invokeId); } -static void +static bool mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) { MmsConnection self = (MmsConnection) parameter; @@ -1022,7 +1022,7 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) } } - return; + return true; } if (indication == ISO_IND_CLOSED) { @@ -1035,7 +1035,7 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) if (self->connectionLostHandler != NULL) self->connectionLostHandler(self, self->connectionLostHandlerParameter); - return; + return true; } if (indication == ISO_IND_ASSOCIATION_FAILED) { @@ -1043,12 +1043,12 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) printf("MMS_CLIENT: mmsIsoCallback: association failed!\n"); setConnectionState(self, MMS_CONNECTION_STATE_CLOSING); - return; + return false; } if (payload != NULL) { if (ByteBuffer_getSize(payload) < 1) { - return; + return false; } } @@ -1075,7 +1075,8 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) } else { setConnectionState(self, MMS_CONNECTION_STATE_CLOSING); - IsoClientConnection_close(self->isoClient); + + goto exit_with_error; } } else { @@ -1083,8 +1084,19 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: Failed to parse initiate response!\n"); + + return false; } } + else if (tag == 0xaa) { /* initiate error PDU */ + + if (DEBUG_MMS_CLIENT) + printf("MMS_CLIENT: received initiate error PDU\n"); + + setConnectionState(self, MMS_CONNECTION_STATE_CLOSING); + + return false; + } else if (tag == 0xa3) { /* unconfirmed PDU */ handleUnconfirmedMmsPdu(self, payload); } @@ -1096,7 +1108,7 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) } else if (tag == 0x8c) { /* conclude response PDU */ if (DEBUG_MMS_CLIENT) - printf("MMS_CLIENT: received conclude.reponse+\n"); + printf("MMS_CLIENT: received conclude.response+\n"); if (self->concludeHandler) { self->concludeHandler(self->concludeHandlerParameter, MMS_ERROR_NONE, true); @@ -1151,14 +1163,14 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: server sent unexpected confirmed error PDU!\n"); - return; + return false; } } else { if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: server sent confirmed error PDU without invoke ID!\n"); - return; + return false; } } @@ -1195,11 +1207,11 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) } } else { - return; + return false; } } else { - return; + return false; } } @@ -1247,7 +1259,7 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: unexpected message from server!\n"); - return; + return false; } } else @@ -1385,14 +1397,14 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: LEAVE mmsIsoCallback - OK\n"); - return; + return true; exit_with_error: if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: received malformed message from server!\n"); - return; + return false; } #if (CONFIG_MMS_THREADLESS_STACK == 0) diff --git a/src/mms/iso_mms/client/mms_client_initiate.c b/src/mms/iso_mms/client/mms_client_initiate.c index f79f163e..a7e16bc0 100644 --- a/src/mms/iso_mms/client/mms_client_initiate.c +++ b/src/mms/iso_mms/client/mms_client_initiate.c @@ -1,7 +1,7 @@ /* * mms_client_initiate.c * - * Copyright 2013-2017 Michael Zillgith + * Copyright 2013-2020 Michael Zillgith * * This file is part of libIEC61850. * @@ -94,11 +94,11 @@ mmsClient_createInitiateRequest(MmsConnection self, ByteBuffer* message) buffer[bufPos++] = 0x01; buffer[bufPos++] = 0x01; - /* proposedParameterCBC */ + /* proposedParameterCBC: fixed */ buffer[bufPos++] = 0x81; buffer[bufPos++] = 0x03; buffer[bufPos++] = 0x05; /* padding */ - buffer[bufPos++] = 0xf1; + buffer[bufPos++] = 0xf1; /* str1, str2, vnam, vlis, valt */ buffer[bufPos++] = 0x00; /* servicesSupportedCalling */ diff --git a/src/mms/iso_mms/common/mms_type_spec.c b/src/mms/iso_mms/common/mms_type_spec.c index 76c4cfea..5dd93c3f 100644 --- a/src/mms/iso_mms/common/mms_type_spec.c +++ b/src/mms/iso_mms/common/mms_type_spec.c @@ -138,10 +138,8 @@ MmsVariableSpecification_isValueOfType(MmsVariableSpecification* self, MmsValue* if (self->typeSpec.bitString == value->value.bitString.size) return true; - if (self->typeSpec.bitString < 0) { - if (value->value.bitString.size <= (-self->typeSpec.bitString)) - return true; - } + if (self->typeSpec.bitString < 0) + return true; } else if (self->type == MMS_FLOAT) { if ((self->typeSpec.floatingpoint.exponentWidth == value->value.floatingPoint.exponentWidth) && diff --git a/src/mms/iso_mms/common/mms_value.c b/src/mms/iso_mms/common/mms_value.c index bf38ae7b..53337ed2 100644 --- a/src/mms/iso_mms/common/mms_value.c +++ b/src/mms/iso_mms/common/mms_value.c @@ -258,11 +258,12 @@ MmsValue_update(MmsValue* self, const MmsValue* update) case MMS_BIT_STRING: if (self->value.bitString.size == update->value.bitString.size) memcpy(self->value.bitString.buf, update->value.bitString.buf, bitStringByteSize(self)); - else if (update->value.bitString.size < self->value.bitString.size) { + else if (update->value.bitString.size != self->value.bitString.size) { int i; for (i = 0; i < update->value.bitString.size; i++) { - MmsValue_setBitStringBit(self, i, MmsValue_getBitStringBit(update, i)); + if (i < self->value.bitString.size) + MmsValue_setBitStringBit(self, i, MmsValue_getBitStringBit(update, i)); } } else diff --git a/src/mms/iso_mms/server/mms_association_service.c b/src/mms/iso_mms/server/mms_association_service.c index afa5d07a..34aecc73 100644 --- a/src/mms/iso_mms/server/mms_association_service.c +++ b/src/mms/iso_mms/server/mms_association_service.c @@ -62,14 +62,13 @@ #define MMS_SERVICE_CONCLUDE 0x10 #define MMS_SERVICE_CANCEL 0x08 -/* negotiated parameter CBB */ +/* our supported parameter CBB: str1, str2, vnam, vlis, valt */ static uint8_t parameterCBB[] = { 0xf1, 0x00 }; - /********************************************************************************************** * MMS Initiate Service *********************************************************************************************/ @@ -86,7 +85,10 @@ encodeInitResponseDetail(MmsServerConnection self, uint8_t* buffer, int bufPos, bufPos = BerEncoder_encodeUInt32WithTL(0x80, 1, buffer, bufPos); /* negotiated protocol version */ - bufPos = BerEncoder_encodeBitString(0x81, 11, parameterCBB, buffer, bufPos); + self->negotiatedParameterCBC[0] = self->negotiatedParameterCBC[0] & parameterCBB[0]; + self->negotiatedParameterCBC[1] = self->negotiatedParameterCBC[1] & parameterCBB[1]; + + bufPos = BerEncoder_encodeBitString(0x81, 11, self->negotiatedParameterCBC, buffer, bufPos); #if (CONFIG_MMS_SERVER_CONFIG_SERVICES_AT_RUNTIME == 1) @@ -157,7 +159,6 @@ encodeInitResponseDetail(MmsServerConnection self, uint8_t* buffer, int bufPos, #endif } - #else uint8_t servicesSupported[] = { @@ -253,6 +254,67 @@ createInitiateResponse(MmsServerConnection self, ByteBuffer* writeBuffer) return bufPos; } +static bool +parseInitRequestDetail(MmsServerConnection self, uint8_t* buffer, int bufPos, int maxBufPos) +{ + while (bufPos < maxBufPos) { + uint8_t tag = buffer[bufPos++]; + int length; + + bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, maxBufPos); + + if (bufPos < 0) { + /* TODO write initiate error PDU! */ + return false; + } + + if (bufPos + length > maxBufPos) { + if (DEBUG_MMS_SERVER) + printf("MMS_SERVER: length field too long\n"); + + return false; + } + + switch(tag) { + case 0x80: /* proposed-version-number */ + { + uint32_t protocolVersion = BerDecoder_decodeUint32(buffer, length, bufPos); + + if (protocolVersion != 1) { + if (DEBUG_MMS_SERVER) + printf("MMS_SERVER: invalid protocol version %u\n", protocolVersion); + + return false; + } + } + + break; + + case 0x81: /* proposed-parameter-CBC */ + + if (length == 3) { + self->negotiatedParameterCBC[0] = buffer[bufPos + 1]; + self->negotiatedParameterCBC[1] = buffer[bufPos + 2]; + + if (DEBUG_MMS_SERVER) + printf("MMS_SERVER: requested parameter CBC: %02x %02x\n", + self->negotiatedParameterCBC[0], + self->negotiatedParameterCBC[1]); + } + else { + if (DEBUG_MMS_SERVER) + printf("MMS_SERVER: unexpected parameter CBC length\n"); + } + + break; + } + + bufPos += length; + } + + return true; +} + static bool parseInitiateRequestPdu(MmsServerConnection self, uint8_t* buffer, int bufPos, int maxBufPos) { @@ -265,6 +327,9 @@ parseInitiateRequestPdu(MmsServerConnection self, uint8_t* buffer, int bufPos, i self->maxServOutstandingCalling = DEFAULT_MAX_SERV_OUTSTANDING_CALLING; + self->negotiatedParameterCBC[0] = 0; + self->negotiatedParameterCBC[1] = 0; + while (bufPos < maxBufPos) { uint8_t tag = buffer[bufPos++]; int length; @@ -276,6 +341,13 @@ parseInitiateRequestPdu(MmsServerConnection self, uint8_t* buffer, int bufPos, i return false; } + if (bufPos + length > maxBufPos) { + if (DEBUG_MMS_SERVER) + printf("MMS_SERVER: length field too long\n"); + + return false; + } + switch (tag) { case 0x80: /* local-detail-calling */ self->maxPduSize = BerDecoder_decodeUint32(buffer, length, bufPos); @@ -305,7 +377,10 @@ parseInitiateRequestPdu(MmsServerConnection self, uint8_t* buffer, int bufPos, i break; case 0xa4: /* mms-init-request-detail */ - /* we ignore this */ + + if (parseInitRequestDetail(self, buffer, bufPos, bufPos + length) == false) + return false; + break; case 0x00: /* indefinite length end tag -> ignore */ @@ -328,10 +403,12 @@ mmsServer_handleInitiateRequest ( ByteBuffer* response) { - if (parseInitiateRequestPdu(self, buffer, bufPos, maxBufPos)) + if (parseInitiateRequestPdu(self, buffer, bufPos, maxBufPos)) { createInitiateResponse(self, response); + } else { - /* TODO send initiate error PDU */ + /* send initiate error PDU */ + mmsMsg_createInitiateErrorPdu(response, 0); } } diff --git a/src/mms/iso_mms/server/mms_server_common.c b/src/mms/iso_mms/server/mms_server_common.c index b28bcce9..30d5118d 100644 --- a/src/mms/iso_mms/server/mms_server_common.c +++ b/src/mms/iso_mms/server/mms_server_common.c @@ -216,6 +216,27 @@ mmsMsg_createServiceErrorPdu(uint32_t invokeId, ByteBuffer* response, MmsError e mmsServer_createServiceErrorPduWithServiceSpecificInfo(invokeId, response, errorType, NULL, 0); } +void +mmsMsg_createInitiateErrorPdu(ByteBuffer* response, uint8_t initiateErrorCode) +{ + /* determine encoded size */ + + uint32_t serviceErrorContentSize = 5; /* errorClass */ + + /* encode */ + uint8_t* buffer = response->buffer; + int bufPos = response->size; + + bufPos = BerEncoder_encodeTL(0xaa, serviceErrorContentSize, buffer, bufPos); /* serviceError */ + bufPos = BerEncoder_encodeTL(0xa0, 3, buffer, bufPos); /* serviceError */ + + buffer[bufPos++] = 8; /* initiate */ + buffer[bufPos++] = 1; + buffer[bufPos++] = initiateErrorCode; + + response->size = bufPos; +} + bool mmsServer_isIndexAccess(AlternateAccess_t* alternateAccess) { diff --git a/src/mms/iso_mms/server/mms_server_connection.c b/src/mms/iso_mms/server/mms_server_connection.c index 4834bd38..1f931797 100644 --- a/src/mms/iso_mms/server/mms_server_connection.c +++ b/src/mms/iso_mms/server/mms_server_connection.c @@ -695,7 +695,7 @@ MmsServerConnection_parseMessage(MmsServerConnection self, ByteBuffer* message, return; - parsing_error: +parsing_error: if (DEBUG_MMS_SERVER) printf("MMS_SERVER: error parsing message\n"); diff --git a/src/sampled_values/sv_subscriber.c b/src/sampled_values/sv_subscriber.c index 1d49b39c..fb1cae6c 100644 --- a/src/sampled_values/sv_subscriber.c +++ b/src/sampled_values/sv_subscriber.c @@ -876,6 +876,12 @@ SVSubscriber_ASDU_getDataSize(SVSubscriber_ASDU self) return self->dataBufferLength; } +uint8_t +SVSubscriber_ASDU_getSmpSynch(SVSubscriber_ASDU self) +{ + return self->smpSynch[0]; +} + uint16_t SVClientASDU_getSmpCnt(SVSubscriber_ASDU self) { diff --git a/src/sampled_values/sv_subscriber.h b/src/sampled_values/sv_subscriber.h index 44ee9066..48d83bdb 100644 --- a/src/sampled_values/sv_subscriber.h +++ b/src/sampled_values/sv_subscriber.h @@ -529,6 +529,16 @@ SVSubscriber_ASDU_getQuality(SVSubscriber_ASDU self, int index); LIB61850_API int SVSubscriber_ASDU_getDataSize(SVSubscriber_ASDU self); +/** + * \brief return the SmpSynch value included in the SV ASDU + * + * The SmpSynch gives information about the clock synchronization. + * + * \param self ASDU object instance + */ +uint8_t +SVSubscriber_ASDU_getSmpSynch(SVSubscriber_ASDU self); + #ifndef DEPRECATED #if defined(__GNUC__) || defined(__clang__) #define DEPRECATED __attribute__((deprecated))