From b27e36337263da18bf11f0c308faf1eff1488f48 Mon Sep 17 00:00:00 2001 From: Michael Zillgith Date: Wed, 28 Oct 2020 12:16:28 +0100 Subject: [PATCH] - IED server: add support for correct CBB handling (required for test case sAss4) and initiate error PDU --- src/mms/inc_private/mms_server_internal.h | 4 + src/mms/iso_mms/client/mms_client_initiate.c | 6 +- .../iso_mms/server/mms_association_service.c | 91 +++++++++++++++++-- src/mms/iso_mms/server/mms_server_common.c | 21 +++++ .../iso_mms/server/mms_server_connection.c | 2 +- 5 files changed, 113 insertions(+), 11 deletions(-) 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_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/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");