diff --git a/src/mms/inc_private/iso_client_connection.h b/src/mms/inc_private/iso_client_connection.h index 64715733..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 1628487f..df06d35f 100644 --- a/src/mms/inc_private/mms_server_internal.h +++ b/src/mms/inc_private/mms_server_internal.h @@ -191,6 +191,7 @@ struct sMmsServerConnection { int maxServOutstandingCalling; int maxServOutstandingCalled; int dataStructureNestingLevel; + uint8_t negotiatedParameterCBC[2]; uint32_t maxPduSize; /* local detail */ IsoConnection isoConnection; MmsServer server; @@ -259,6 +260,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 35349de3..141476b4 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 e4c19f8e..5644912d 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 1c8a6d5c..62ca427d 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/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 55f39b9f..afb81cef 100644 --- a/src/mms/iso_mms/server/mms_server_connection.c +++ b/src/mms/iso_mms/server/mms_server_connection.c @@ -701,7 +701,7 @@ MmsServerConnection_parseMessage(MmsServerConnection self, ByteBuffer* message, return; - parsing_error: +parsing_error: if (DEBUG_MMS_SERVER) printf("MMS_SERVER: error parsing message\n");