From 7cf67bb01b31f6dbbeaa8a681c9045465c13e275 Mon Sep 17 00:00:00 2001 From: Michael Zillgith Date: Sun, 10 Nov 2024 10:53:57 +0000 Subject: [PATCH] - added function IedServer_setLocalAddresses to configure called local T-,S-,P-selectors (LIB61850-471) - added code to optionally reject incoming connections with wrong values of called T-,S-,P-selectors (LIB61850-471) --- src/iec61850/inc/iec61850_server.h | 15 ++ src/iec61850/server/impl/ied_server.c | 5 + src/mms/inc/mms_server.h | 7 +- src/mms/inc_private/cotp.h | 5 + src/mms/inc_private/iso_presentation.h | 12 +- src/mms/inc_private/iso_server.h | 4 + src/mms/inc_private/iso_server_private.h | 9 + src/mms/inc_private/iso_session.h | 6 +- src/mms/inc_private/mms_server_internal.h | 12 +- src/mms/iso_cotp/cotp.c | 46 +++- src/mms/iso_mms/server/mms_server.c | 28 +++ src/mms/iso_presentation/iso_presentation.c | 238 +++++++++++++------- src/mms/iso_server/iso_connection.c | 91 +++++--- src/mms/iso_server/iso_server.c | 119 +++++++--- src/mms/iso_session/iso_session.c | 39 +++- 15 files changed, 477 insertions(+), 159 deletions(-) diff --git a/src/iec61850/inc/iec61850_server.h b/src/iec61850/inc/iec61850_server.h index 63fe2268..64db0f61 100644 --- a/src/iec61850/inc/iec61850_server.h +++ b/src/iec61850/inc/iec61850_server.h @@ -514,6 +514,21 @@ IedServer_addAccessPoint(IedServer self, const char* ipAddr, int tcpPort, TLSCon LIB61850_API void IedServer_setLocalIpAddress(IedServer self, const char* localIpAddress); +/** + * \brief Set the local addresses + * + * \note When set, the values will be checked againt the called addresses of the incoming connections. + * If the set addresses do not match the incoming connection will be rejected. + * Default behavior is to accept all incoming connections independent of the called selectors. + * + * \param self the IedServer instance + * \param pSelector the presentation-selector to set or NULL when no check is required + * \param sSelector the session-selector to set or NULL when no check is required + * \param tSelector the transport-selector to set or NULL when no check is required + */ +LIB61850_API void +IedServer_setLocalAddresses(IedServer self, const PSelector* pSelector, const SSelector* sSelector, const TSelector* tSelector); + /** * \brief Set the identify for the MMS identify service * diff --git a/src/iec61850/server/impl/ied_server.c b/src/iec61850/server/impl/ied_server.c index 9125b638..093b2bf8 100644 --- a/src/iec61850/server/impl/ied_server.c +++ b/src/iec61850/server/impl/ied_server.c @@ -923,6 +923,11 @@ IedServer_setLocalIpAddress(IedServer self, const char* localIpAddress) MmsServer_setLocalIpAddress(self->mmsServer, self->localIpAddress); } +void +IedServer_setLocalAddresses(IedServer self, const PSelector* pSelector, const SSelector* sSelector, const TSelector* tSelector) +{ + MmsServer_setLocalAddresses(self->mmsServer, pSelector, sSelector, tSelector); +} void IedServer_startThreadless(IedServer self, int tcpPort) diff --git a/src/mms/inc/mms_server.h b/src/mms/inc/mms_server.h index 15248789..18895376 100644 --- a/src/mms/inc/mms_server.h +++ b/src/mms/inc/mms_server.h @@ -54,6 +54,9 @@ typedef enum { LIB61850_INTERNAL void MmsServer_setLocalIpAddress(MmsServer self, const char* localIpAddress); +LIB61850_INTERNAL void +MmsServer_setLocalAddresses(MmsServer self, const PSelector* pSelector, const SSelector* sSelector, const TSelector* tSelector); + LIB61850_INTERNAL bool MmsServer_isRunning(MmsServer self); @@ -285,10 +288,6 @@ MmsServer_setMaxDataSetEntries(MmsServer self, int maxDataSetEntries); LIB61850_INTERNAL void MmsServer_enableJournalService(MmsServer self, bool enable); - - - - /*************************************************** * Functions for MMS identify service ***************************************************/ diff --git a/src/mms/inc_private/cotp.h b/src/mms/inc_private/cotp.h index 3cbe2681..574e7d39 100644 --- a/src/mms/inc_private/cotp.h +++ b/src/mms/inc_private/cotp.h @@ -59,6 +59,8 @@ typedef struct { uint8_t* socketExtensionBuffer; /* buffer to store data when TCP socket is not accepting all data */ int socketExtensionBufferSize; /* maximum number of bytes to store in the extension buffer */ int socketExtensionBufferFill; /* number of bytes in the extension buffer (bytes to write) */ + + const TSelector* checkTSel; /* when set the called TSelector will be check to be equal to this value */ } CotpConnection; typedef enum { @@ -87,6 +89,9 @@ CotpConnection_init(CotpConnection* self, Socket socket, ByteBuffer* payloadBuffer, ByteBuffer* readBuffer, ByteBuffer* writeBuffer, uint8_t* socketExtensionBuffer, int socketExtensionBufferSize); +LIB61850_INTERNAL void +CotpConnection_setCheckDstSelector(CotpConnection* self, const TSelector* tSel); + LIB61850_INTERNAL CotpIndication CotpConnection_parseIncomingMessage(CotpConnection* self); diff --git a/src/mms/inc_private/iso_presentation.h b/src/mms/inc_private/iso_presentation.h index 367e2995..b3dab62b 100644 --- a/src/mms/inc_private/iso_presentation.h +++ b/src/mms/inc_private/iso_presentation.h @@ -31,6 +31,7 @@ typedef struct { PSelector callingPresentationSelector; PSelector calledPresentationSelector; + const PSelector* checkCalledPSelector; uint8_t nextContextId; uint8_t acseContextId; uint8_t mmsContextId; @@ -38,13 +39,16 @@ typedef struct { } IsoPresentation; LIB61850_INTERNAL void -IsoPresentation_init(IsoPresentation* session); +IsoPresentation_init(IsoPresentation* self); -LIB61850_INTERNAL int -IsoPresentation_parseUserData(IsoPresentation* session, ByteBuffer* message); +LIB61850_INTERNAL void +IsoPresentation_setCalledPSelector(IsoPresentation* self, const PSelector* calledPSelector); LIB61850_INTERNAL int -IsoPresentation_parseConnect(IsoPresentation* session, ByteBuffer* message); +IsoPresentation_parseUserData(IsoPresentation* self, ByteBuffer* message); + +LIB61850_INTERNAL bool +IsoPresentation_parseConnect(IsoPresentation* self, ByteBuffer* message); LIB61850_INTERNAL void IsoPresentation_createConnectPdu(IsoPresentation* self, IsoConnectionParameters parameters, diff --git a/src/mms/inc_private/iso_server.h b/src/mms/inc_private/iso_server.h index 8ce80bdc..fcb1cb0d 100644 --- a/src/mms/inc_private/iso_server.h +++ b/src/mms/inc_private/iso_server.h @@ -113,6 +113,10 @@ IsoServer_setMaxConnections(IsoServer self, int maxConnections); LIB61850_INTERNAL void IsoServer_setLocalIpAddress(IsoServer self, const char* ipAddress); +LIB61850_INTERNAL void +IsoServer_setLocalAddresses(IsoServer self, const PSelector* pSel, const SSelector* sSel, + const TSelector* tSel); + LIB61850_INTERNAL IsoServerState IsoServer_getState(IsoServer self); diff --git a/src/mms/inc_private/iso_server_private.h b/src/mms/inc_private/iso_server_private.h index 6249255e..e65a3a2c 100644 --- a/src/mms/inc_private/iso_server_private.h +++ b/src/mms/inc_private/iso_server_private.h @@ -70,6 +70,15 @@ private_IsoServer_decreaseConnectionCounter(IsoServer self); LIB61850_INTERNAL int private_IsoServer_getConnectionCounter(IsoServer self); +LIB61850_INTERNAL const PSelector* +private_IsoServer_getLocalPSelector(IsoServer self); + +LIB61850_INTERNAL const SSelector* +private_IsoServer_getLocalSSelector(IsoServer self); + +LIB61850_INTERNAL const TSelector* +private_IsoServer_getLocalTSelector(IsoServer self); + LIB61850_INTERNAL bool IsoConnection_isRunning(IsoConnection self); diff --git a/src/mms/inc_private/iso_session.h b/src/mms/inc_private/iso_session.h index 5f48c2be..e304bacf 100644 --- a/src/mms/inc_private/iso_session.h +++ b/src/mms/inc_private/iso_session.h @@ -1,7 +1,7 @@ /* * ise_session.h * - * Copyright 2013-2018 Michael Zillgith + * Copyright 2013-2014 Michael Zillgith * * This file is part of libIEC61850. * @@ -31,6 +31,7 @@ typedef struct { SSelector callingSessionSelector; SSelector calledSessionSelector; + const SSelector* checkCalledSSelector; uint16_t sessionRequirement; uint8_t protocolOptions; ByteBuffer userData; @@ -51,6 +52,9 @@ typedef enum { LIB61850_INTERNAL void IsoSession_init(IsoSession* session); +LIB61850_INTERNAL void +IsoSession_setCalledSSelector(IsoSession* session, const SSelector* calledSSelector); + LIB61850_INTERNAL ByteBuffer* IsoSession_getUserData(IsoSession* session); diff --git a/src/mms/inc_private/mms_server_internal.h b/src/mms/inc_private/mms_server_internal.h index 3fae4f26..6181ead8 100644 --- a/src/mms/inc_private/mms_server_internal.h +++ b/src/mms/inc_private/mms_server_internal.h @@ -105,8 +105,8 @@ struct sMmsObtainFileTask { #endif /* (MMS_OBTAIN_FILE_SERVICE == 1) */ -struct sMmsServer { - +struct sMmsServer +{ LinkedList /**/ isoServerList; MmsDevice* device; @@ -198,6 +198,12 @@ struct sMmsServer { int maxDomainSpecificDataSets; #endif /* (CONFIG_SET_FILESTORE_BASEPATH_AT_RUNTIME == 1) */ + PSelector pSel; + bool checkCalledPSel; + SSelector sSel; + bool checkCalledSSel; + TSelector tSel; + bool checkCalledTSel; }; struct sMmsServerConnection { @@ -245,8 +251,6 @@ MmsServer_callConnectionHandler(MmsServer self, MmsServerConnection connection); LIB61850_INTERNAL int mmsServer_write_out(const void *buffer, size_t size, void *app_key); - - LIB61850_INTERNAL void mmsServer_handleDeleteNamedVariableListRequest(MmsServerConnection connection, uint8_t* buffer, int bufPos, int maxBufPos, diff --git a/src/mms/iso_cotp/cotp.c b/src/mms/iso_cotp/cotp.c index 70044864..9b0f5d5e 100644 --- a/src/mms/iso_cotp/cotp.c +++ b/src/mms/iso_cotp/cotp.c @@ -579,6 +579,14 @@ CotpConnection_init(CotpConnection* self, Socket socket, self->socketExtensionBuffer = socketExtensionBuffer; self->socketExtensionBufferSize = socketExtensionBufferSize; self->socketExtensionBufferFill = 0; + + self->checkTSel = NULL; +} + +void +CotpConnection_setCheckDstSelector(CotpConnection* self, const TSelector* tSel) +{ + self->checkTSel = tSel; } int /* in byte */ @@ -632,6 +640,24 @@ CotpConnection_getLocalRef(CotpConnection* self) +--+------+---------+---------+---+---+------+-------+---------+ */ +static bool +checkDstTSelector(CotpConnection* self) +{ + if (self->checkTSel == NULL) + return true; + + if (self->options.tSelDst.size != self->checkTSel->size) + return false; + + int i; + for (i = 0; i < self->options.tSelDst.size; i++) + { + if (self->options.tSelDst.value[i] != self->checkTSel->value[i]) + return false; + } + + return true; +} static bool parseConnectRequestTpdu(CotpConnection* self, uint8_t* buffer, uint8_t len) @@ -642,7 +668,25 @@ parseConnectRequestTpdu(CotpConnection* self, uint8_t* buffer, uint8_t len) self->remoteRef = getUint16(buffer + 2); self->protocolClass = getUint8(buffer + 4); - return parseOptions(self, buffer + 5, len - 6); + if (parseOptions(self, buffer + 5, len - 6)) + { + if (checkDstTSelector(self) == false) + { + if (DEBUG_COTP) + printf("COTP: CR TPDU: wrong called T-selector value\n"); + + return false; + } + } + else + { + if (DEBUG_COTP) + printf("COTP: CR TPDU: error parsing options\n"); + + return false; + } + + return true; } static bool diff --git a/src/mms/iso_mms/server/mms_server.c b/src/mms/iso_mms/server/mms_server.c index 1e860f3a..f72eb98b 100644 --- a/src/mms/iso_mms/server/mms_server.c +++ b/src/mms/iso_mms/server/mms_server.c @@ -180,6 +180,28 @@ MmsServer_setLocalIpAddress(MmsServer self, const char* localIpAddress) } } +void +MmsServer_setLocalAddresses(MmsServer self, const PSelector* pSelector, const SSelector* sSelector, const TSelector* tSelector) +{ + if (pSelector) + { + self->pSel = *pSelector; + self->checkCalledPSel = true; + } + + if (sSelector) + { + self->sSel = *sSelector; + self->checkCalledSSel = true; + } + + if (tSelector) + { + self->tSel = *tSelector; + self->checkCalledTSel = true; + } +} + bool MmsServer_isRunning(MmsServer self) { @@ -723,6 +745,8 @@ MmsServer_startListening(MmsServer self, int tcpPort) if (tcpPort != -1) IsoServer_setTcpPort(isoServer, tcpPort); + IsoServer_setLocalAddresses(isoServer, &self->pSel, &self->sSel, &self->tSel); + IsoServer_startListening(isoServer); elem = LinkedList_getNext(elem); @@ -770,6 +794,10 @@ MmsServer_startListeningThreadless(MmsServer self, int tcpPort) if (tcpPort != -1) IsoServer_setTcpPort(isoServer, tcpPort); + IsoServer_setLocalAddresses(isoServer, self->checkCalledPSel ? &self->pSel : NULL, + self->checkCalledSSel ? &self->sSel : NULL, + self->checkCalledTSel ? &self->tSel : NULL); + IsoServer_startListeningThreadless(isoServer); elem = LinkedList_getNext(elem); diff --git a/src/mms/iso_presentation/iso_presentation.c b/src/mms/iso_presentation/iso_presentation.c index c4f2f64e..743be0a8 100644 --- a/src/mms/iso_presentation/iso_presentation.c +++ b/src/mms/iso_presentation/iso_presentation.c @@ -1,7 +1,7 @@ /* * iso_presentation.c * - * Copyright 2013-2022 Michael Zillgith + * Copyright 2013-2024 Michael Zillgith * * This file is part of libIEC61850. * @@ -35,7 +35,6 @@ #define DEBUG_PRES 0 #endif - static uint8_t calledPresentationSelector[] = { 0x00, 0x00, 0x00, 0x01 }; static uint8_t asn_id_as_acse[] = { 0x52, 0x01, 0x00, 0x01 }; @@ -58,8 +57,7 @@ encodeAcceptBer(uint8_t* buffer, int bufPos) } static int -encodeUserData(uint8_t* buffer, int bufPos, - BufferChain payload, bool encode, uint8_t contextId) +encodeUserData(uint8_t* buffer, int bufPos, BufferChain payload, bool encode, uint8_t contextId) { int payloadLength = payload->length; @@ -73,7 +71,8 @@ encodeUserData(uint8_t* buffer, int bufPos, fullyEncodedDataLength += BerEncoder_determineLengthSize(encodedDataSetLength) + 1; - if (encode) { + if (encode) + { /* fully-encoded-data */ bufPos = BerEncoder_encodeTL(0x61, fullyEncodedDataLength, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x30, encodedDataSetLength, buffer, bufPos); @@ -87,7 +86,8 @@ encodeUserData(uint8_t* buffer, int bufPos, return bufPos; } - else { + else + { int encodedUserDataLength = fullyEncodedDataLength + 1; encodedUserDataLength += BerEncoder_determineLengthSize(fullyEncodedDataLength); @@ -195,7 +195,8 @@ parseFullyEncodedData(IsoPresentation* self, uint8_t* buffer, int len, int bufPo int endPos = bufPos + len; - if (buffer[bufPos++] != 0x30) { + if (buffer[bufPos++] != 0x30) + { if (DEBUG_PRES) printf("PRES: user-data parse error\n"); return -1; @@ -203,7 +204,8 @@ parseFullyEncodedData(IsoPresentation* self, uint8_t* buffer, int len, int bufPo bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, endPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: wrong parameter length\n"); return -1; @@ -211,19 +213,22 @@ parseFullyEncodedData(IsoPresentation* self, uint8_t* buffer, int len, int bufPo endPos = bufPos + len; - while (bufPos < endPos) { + while (bufPos < endPos) + { uint8_t tag = buffer[bufPos++]; int length; bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, endPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: wrong parameter length\n"); return -1; } - switch (tag) { + switch (tag) + { case 0x02: /* presentation-context-identifier */ if (DEBUG_PRES) printf("PRES: presentation-context-identifier\n"); @@ -262,7 +267,8 @@ parseFullyEncodedData(IsoPresentation* self, uint8_t* buffer, int len, int bufPo } } - if (!userDataPresent) { + if (!userDataPresent) + { if (DEBUG_PRES) printf("PRES: user-data not present\n"); return -1; @@ -280,19 +286,22 @@ parsePCDLEntry(IsoPresentation* self, uint8_t* buffer, int totalLength, int bufP bool isAcse = false; bool isMms = false; - while (bufPos < endPos) { + while (bufPos < endPos) + { uint8_t tag = buffer[bufPos++]; int len; bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, endPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: Invalid PDU\n"); return -1; } - switch (tag) { + switch (tag) + { case 0x02: /* presentation-context-identifier */ contextId = BerDecoder_decodeUint32(buffer, len, bufPos); bufPos += len; @@ -302,11 +311,13 @@ parsePCDLEntry(IsoPresentation* self, uint8_t* buffer, int totalLength, int bufP if (DEBUG_PRES) printf("PRES: abstract-syntax-name with len %i\n", len); - if (len == 5) { + if (len == 5) + { if (memcmp(buffer + bufPos, asn_id_mms, 5) == 0) isMms = true; } - else if (len == 4) { + else if (len == 4) + { if (memcmp(buffer + bufPos, asn_id_as_acse, 4) == 0) isAcse = true; } @@ -333,26 +344,30 @@ parsePCDLEntry(IsoPresentation* self, uint8_t* buffer, int totalLength, int bufP } } - if (contextId < 0) { + if (contextId < 0) + { if (DEBUG_PRES) printf("PRES: ContextId not defined!\n"); return -1; } - if ((isAcse == false) && (isMms == false)) { + if ((isAcse == false) && (isMms == false)) + { if (DEBUG_PRES) printf("PRES: not an ACSE or MMS context definition\n"); return -1; } - if (isMms) { - self->mmsContextId = (uint8_t) contextId; + if (isMms) + { + self->mmsContextId = (uint8_t)contextId; if (DEBUG_PRES) printf("PRES: MMS context id is %i\n", contextId); } - else { - self->acseContextId = (uint8_t) contextId; + else + { + self->acseContextId = (uint8_t)contextId; if (DEBUG_PRES) printf("PRES: ACSE context id is %i\n", contextId); } @@ -365,7 +380,8 @@ parsePresentationContextDefinitionList(IsoPresentation* self, uint8_t* buffer, i { int endPos = bufPos + totalLength; - while (bufPos < endPos) { + while (bufPos < endPos) + { uint8_t tag = buffer[bufPos++]; int len; @@ -373,7 +389,8 @@ parsePresentationContextDefinitionList(IsoPresentation* self, uint8_t* buffer, i if (bufPos < 0) return -1; - switch (tag) { + switch (tag) + { case 0x30: if (DEBUG_PRES) printf("PRES: parse pcd entry\n"); @@ -381,8 +398,10 @@ parsePresentationContextDefinitionList(IsoPresentation* self, uint8_t* buffer, i if (bufPos < 0) return -1; break; + case 0x00: /* indefinite length end tag -> ignore */ break; + default: if (DEBUG_PRES) printf("PRES: unknown tag in presentation-context-definition-list\n"); @@ -404,11 +423,13 @@ parseNormalModeParameters(IsoPresentation* self, uint8_t* buffer, int totalLengt bool hasUserData = false; - while (bufPos < endPos) { + while (bufPos < endPos) + { uint8_t tag = buffer[bufPos++]; int len; - if (bufPos == endPos) { + if (bufPos == endPos) + { if (DEBUG_PRES) printf("PRES: invalid message\n"); return -1; @@ -416,20 +437,24 @@ parseNormalModeParameters(IsoPresentation* self, uint8_t* buffer, int totalLengt bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, endPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: wrong parameter length\n"); return -1; } - switch (tag) { + switch (tag) + { case 0x81: /* calling-presentation-selector */ - if (len > 16) { + if (len > 16) + { if (DEBUG_PRES) printf("PRES: calling-presentation-sel too large\n"); } - else { + else + { self->callingPresentationSelector.size = len; int i; for (i = 0; i < len; i++) @@ -441,11 +466,13 @@ parseNormalModeParameters(IsoPresentation* self, uint8_t* buffer, int totalLengt case 0x82: /* called-presentation-selector */ - if (len > 16) { + if (len > 16) + { if (DEBUG_PRES) printf("PRES: called-presentation-sel too large\n"); } - else { + else + { self->calledPresentationSelector.size = len; int i; for (i = 0; i < len; i++) @@ -457,7 +484,8 @@ parseNormalModeParameters(IsoPresentation* self, uint8_t* buffer, int totalLengt case 0x83: /* responding-presentation-selector */ - if (len > 16) { + if (len > 16) + { if (DEBUG_PRES) printf("PRES: responding-presentation-sel too large\n"); } @@ -505,7 +533,8 @@ parseNormalModeParameters(IsoPresentation* self, uint8_t* buffer, int totalLengt } } - if (hasUserData == false) { + if (hasUserData == false) + { if (DEBUG_PRES) printf("PRES: user-data is missing\n"); @@ -525,7 +554,8 @@ IsoPresentation_parseAcceptMessage(IsoPresentation* self, ByteBuffer* byteBuffer uint8_t cpTag = buffer[bufPos++]; - if (cpTag != 0x31) { + if (cpTag != 0x31) + { if (DEBUG_PRES) printf("PRES: not a CPA message\n"); return 0; @@ -535,31 +565,36 @@ IsoPresentation_parseAcceptMessage(IsoPresentation* self, ByteBuffer* byteBuffer bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: Invalid message\n"); return 0; } - while (bufPos < maxBufPos) { + while (bufPos < maxBufPos) + { uint8_t tag = buffer[bufPos++]; bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: wrong parameter length\n"); return 0; } - switch (tag) { - case 0xa0: /* mode-selector */ + switch (tag) + { + case 0xa0: /* mode-selector */ bufPos += len; /* ignore content since only normal mode is allowed */ break; case 0xa2: /* normal-mode-parameters */ bufPos = parseNormalModeParameters(self, buffer, len, bufPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: error parsing normal-mode-parameters\n"); return 0; @@ -568,6 +603,7 @@ IsoPresentation_parseAcceptMessage(IsoPresentation* self, ByteBuffer* byteBuffer break; case 0x00: /* indefinite length end tag -> ignore */ break; + default: if (DEBUG_PRES) printf("PRES: CPA unknown tag %i\n", tag); @@ -582,7 +618,13 @@ IsoPresentation_parseAcceptMessage(IsoPresentation* self, ByteBuffer* byteBuffer void IsoPresentation_init(IsoPresentation* self) { - (void)self; + self->checkCalledPSelector = NULL; +} + +void +IsoPresentation_setCalledPSelector(IsoPresentation* self, const PSelector* calledPSelector) +{ + self->checkCalledPSelector = calledPSelector; } void @@ -670,7 +712,7 @@ IsoPresentation_parseUserData(IsoPresentation* self, ByteBuffer* readBuffer) return 0; } - if (buffer[bufPos++] != 0x30) + if (buffer[bufPos++] != 0x30) /* PDV list */ return 0; bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); @@ -703,10 +745,10 @@ IsoPresentation_parseUserData(IsoPresentation* self, ByteBuffer* readBuffer) case 0x06: /* transfer-syntax-name */ { /* check if basic-encoding (2.1.1 - 51 01) */ - if ((buffer[bufPos] != 0x51) || (buffer[bufPos + 1] != 0x01)) { - if (DEBUG_PRES) { + if ((buffer[bufPos] != 0x51) || (buffer[bufPos + 1] != 0x01)) + { + if (DEBUG_PRES) printf("PRES: unknown transfer-syntax-name\n"); - } return 0; } @@ -715,7 +757,6 @@ IsoPresentation_parseUserData(IsoPresentation* self, ByteBuffer* readBuffer) } break; - case 0xa0: /* presentation data */ { if (hasAbstractSyntaxName == false) { @@ -741,7 +782,26 @@ IsoPresentation_parseUserData(IsoPresentation* self, ByteBuffer* readBuffer) return 0; } -int +static bool +checkCalledPSelector(IsoPresentation* self) +{ + if (self->checkCalledPSelector == NULL) + return true; + + if (self->calledPresentationSelector.size != self->checkCalledPSelector->size) + return false; + + int i; + for (i = 0; i < self->calledPresentationSelector.size; i++) + { + if (self->calledPresentationSelector.value[i] != self->checkCalledPSelector->value[i]) + return false; + } + + return true; +} + +bool IsoPresentation_parseConnect(IsoPresentation* self, ByteBuffer* byteBuffer) { uint8_t* buffer = byteBuffer->buffer; @@ -752,70 +812,87 @@ IsoPresentation_parseConnect(IsoPresentation* self, ByteBuffer* byteBuffer) uint8_t cpTag = buffer[bufPos++]; - if (cpTag != 0x31) { + if (cpTag != 0x31) + { if (DEBUG_PRES) printf("PRES: not a CP type\n"); - return 0; + return false; } int len; bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: invalid message!\n"); - return 0; + return false; } if (DEBUG_PRES) printf("PRES: CPType with len %i\n", len); - while (bufPos < maxBufPos) { + while (bufPos < maxBufPos) + { uint8_t tag = buffer[bufPos++]; bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: invalid message!\n"); - return 0; + return false; } - switch (tag) { + switch (tag) + { case 0xa0: /* mode-selection */ + { + if (buffer[bufPos++] != 0x80) { - if (buffer[bufPos++] != 0x80) { - if (DEBUG_PRES) - printf("PRES: mode-value of wrong type!\n"); - return 0; - } + if (DEBUG_PRES) + printf("PRES: mode-value of wrong type!\n"); + return false; + } - bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); + bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, maxBufPos); - if (bufPos < 0) { - if (DEBUG_PRES) - printf("PRES: invalid message!\n"); - return 0; - } + if (bufPos < 0) + { + if (DEBUG_PRES) + printf("PRES: invalid message!\n"); + return false; + } - uint32_t modeSelector = BerDecoder_decodeUint32(buffer, len, bufPos); + uint32_t modeSelector = BerDecoder_decodeUint32(buffer, len, bufPos); - if (DEBUG_PRES) - printf("PRES: modesel %ui\n", modeSelector); + if (DEBUG_PRES) + printf("PRES: modesel %ui\n", modeSelector); - bufPos += len; - } - break; + bufPos += len; + } + break; case 0xa2: /* normal-mode-parameters */ bufPos = parseNormalModeParameters(self, buffer, len, bufPos); - if (bufPos < 0) { + if (bufPos < 0) + { if (DEBUG_PRES) printf("PRES: error parsing normal-mode-parameters\n"); - return 0; + return false; } - else { + else + { + if (checkCalledPSelector(self) == false) + { + if (DEBUG_PRES) + printf("PRES: called presentation selector not matching\n"); + + return false; + } + hasNormalModeParameters = true; } @@ -830,14 +907,15 @@ IsoPresentation_parseConnect(IsoPresentation* self, ByteBuffer* byteBuffer) } } - if (hasNormalModeParameters == false) { + if (hasNormalModeParameters == false) + { if (DEBUG_PRES) printf("PRES: error - normal mode parameters are missing\n"); - return 0; + return false; } - return 1; + return true; } void diff --git a/src/mms/iso_server/iso_connection.c b/src/mms/iso_server/iso_connection.c index 795b0636..bdd7d343 100644 --- a/src/mms/iso_server/iso_connection.c +++ b/src/mms/iso_server/iso_connection.c @@ -1,7 +1,7 @@ /* * iso_connection.c * - * Copyright 2013-2023 Michael Zillgith + * Copyright 2013-2024 Michael Zillgith * * This file is part of libIEC61850. * @@ -171,8 +171,8 @@ void IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) { #if (CONFIG_MMS_SINGLE_THREADED != 1) - if (isSingleThread == false) { - + if (isSingleThread == false) + { IsoConnection_callTickHandler(self); if (Handleset_waitReady(self->handleSet, 10) < 1) @@ -220,12 +220,14 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) ByteBuffer* sessionUserData = IsoSession_getUserData(self->session); - switch (sIndication) { + switch (sIndication) + { case SESSION_CONNECT: if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: session connect indication\n"); - if (IsoPresentation_parseConnect(self->presentation, sessionUserData)) { + if (IsoPresentation_parseConnect(self->presentation, sessionUserData)) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: presentation ok\n"); @@ -233,8 +235,8 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) AcseIndication aIndication = AcseConnection_parseMessage(self->acseConnection, acseBuffer); - if (aIndication == ACSE_ASSOCIATE) { - + if (aIndication == ACSE_ASSOCIATE) + { #if (CONFIG_MMS_THREADLESS_STACK != 1) IsoConnection_lock(self); #endif @@ -250,7 +252,8 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) ByteBuffer_wrap(&mmsResponseBuffer, self->sendBuffer, 0, SEND_BUF_SIZE); - if (self->msgRcvdHandler != NULL) { + if (self->msgRcvdHandler != NULL) + { self->msgRcvdHandler(self->handlerParameter, &mmsRequest, &mmsResponseBuffer); } @@ -261,7 +264,8 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) BufferChain_init(mmsBufferPart, mmsResponseBuffer.size, mmsResponseBuffer.size, NULL, self->sendBuffer); - if (mmsResponseBuffer.size > 0) { + if (mmsResponseBuffer.size > 0) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: application payload size: %i\n", mmsResponseBuffer.size); @@ -293,7 +297,8 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) CotpConnection_sendDataMessage(self->cotpConnection, sessionBufferPart); } - else { + else + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: association error. No response from application!\n"); } @@ -302,12 +307,12 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) IsoConnection_unlock(self); #endif } - else { + else + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: acse association failed\n"); self->state = ISO_CON_STATE_STOPPED; } - } else { self->state = ISO_CON_STATE_STOPPED; @@ -318,14 +323,16 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: session data indication\n"); - if (!IsoPresentation_parseUserData(self->presentation, sessionUserData)) { + if (!IsoPresentation_parseUserData(self->presentation, sessionUserData)) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: presentation layer error\n"); self->state = ISO_CON_STATE_STOPPED; break; } - if (self->presentation->nextContextId == self->presentation->mmsContextId) { + if (self->presentation->nextContextId == self->presentation->mmsContextId) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: mms message\n"); @@ -339,15 +346,15 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) ByteBuffer_wrap(&mmsResponseBuffer, self->sendBuffer, 0, SEND_BUF_SIZE); - if (self->msgRcvdHandler != NULL) { - + if (self->msgRcvdHandler != NULL) + { self->msgRcvdHandler(self->handlerParameter, mmsRequest, &mmsResponseBuffer); } /* send a response if required */ - if (mmsResponseBuffer.size > 0) { - + if (mmsResponseBuffer.size > 0) + { struct sBufferChain mmsBufferPartStruct; BufferChain mmsBufferPart = &mmsBufferPartStruct; @@ -376,7 +383,8 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) IsoConnection_unlock(self); #endif } - else { + else + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: unknown presentation layer context!"); } @@ -387,7 +395,8 @@ IsoConnection_handleTcpConnection(IsoConnection self, bool isSingleThread) if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: session finish indication\n"); - if (IsoPresentation_parseUserData(self->presentation, sessionUserData)) { + if (IsoPresentation_parseUserData(self->presentation, sessionUserData)) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: iso_connection: presentation ok\n"); @@ -491,11 +500,13 @@ IsoConnection_create(Socket socket, IsoServer isoServer, bool isSingleThread) { IsoConnection self = (IsoConnection) GLOBAL_CALLOC(1, sizeof(struct sIsoConnection)); - if (self) { + if (self) + { self->socket = socket; #if (CONFIG_MMS_SUPPORT_TLS == 1) - if (IsoServer_getTLSConfiguration(isoServer) != NULL) { + if (IsoServer_getTLSConfiguration(isoServer) != NULL) + { self->tlsSocket = TLSSocket_create(socket, IsoServer_getTLSConfiguration(isoServer), true); if (self->tlsSocket == NULL) { @@ -537,6 +548,11 @@ IsoConnection_create(Socket socket, IsoServer isoServer, bool isSingleThread) CotpConnection_init(self->cotpConnection, self->socket, &(self->rcvBuffer), &(self->cotpReadBuffer), &(self->cotpWriteBuffer), socketExtensionBuffer, socketExtensionBufferSize); + const TSelector* tSel = private_IsoServer_getLocalTSelector(isoServer); + + if (tSel) + CotpConnection_setCheckDstSelector(self->cotpConnection, tSel); + #if (CONFIG_MMS_SUPPORT_TLS == 1) if (self->tlsSocket) self->cotpConnection->tlsSocket = self->tlsSocket; @@ -545,9 +561,19 @@ IsoConnection_create(Socket socket, IsoServer isoServer, bool isSingleThread) self->session = (IsoSession*) GLOBAL_CALLOC(1, sizeof(IsoSession)); IsoSession_init(self->session); + const SSelector* sSel = private_IsoServer_getLocalSSelector(isoServer); + + if (sSel) + IsoSession_setCalledSSelector(self->session, sSel); + self->presentation = (IsoPresentation*) GLOBAL_CALLOC(1, sizeof(IsoPresentation)); IsoPresentation_init(self->presentation); + PSelector* pSel = (PSelector*) private_IsoServer_getLocalPSelector(isoServer); + + if (pSel) + IsoPresentation_setCalledPSelector(self->presentation, pSel); + self->acseConnection = (AcseConnection*) GLOBAL_CALLOC(1, sizeof(AcseConnection)); #if (CONFIG_MMS_SUPPORT_TLS == 1) @@ -563,7 +589,8 @@ IsoConnection_create(Socket socket, IsoServer isoServer, bool isSingleThread) #if (CONFIG_MMS_SINGLE_THREADED == 0) #if (CONFIG_MMS_THREADLESS_STACK == 0) - if (isSingleThread == false) { + if (isSingleThread == false) + { self->handleSet = Handleset_new(); Handleset_addSocket(self->handleSet, self->socket); self->thread = Thread_create((ThreadExecutionFunction) handleTcpConnection, self, false); @@ -607,7 +634,8 @@ IsoConnection_destroy(IsoConnection self) Handleset_destroy(self->handleSet); #endif - if (self->cotpConnection) { + if (self->cotpConnection) + { if (self->cotpConnection->handleSet) Handleset_destroy(self->cotpConnection->handleSet); @@ -650,7 +678,8 @@ IsoConnection_sendMessage(IsoConnection self, ByteBuffer* message) { bool success = false; - if (self->state == ISO_CON_STATE_STOPPED) { + if (self->state == ISO_CON_STATE_STOPPED) + { if (DEBUG_ISO_SERVER) printf("DEBUG_ISO_SERVER: sendMessage: connection already stopped!\n"); goto exit_error; @@ -682,7 +711,8 @@ IsoConnection_sendMessage(IsoConnection self, ByteBuffer* message) indication = CotpConnection_sendDataMessage(self->cotpConnection, sessionBuffer); - if (DEBUG_ISO_SERVER) { + if (DEBUG_ISO_SERVER) + { if (indication != COTP_OK) printf("ISO_SERVER: IsoConnection_sendMessage failed!\n"); else @@ -699,16 +729,19 @@ exit_error: void IsoConnection_close(IsoConnection self) { - if (self->state != ISO_CON_STATE_TERMINATED) { + if (self->state != ISO_CON_STATE_TERMINATED) + { self->state = ISO_CON_STATE_STOPPED; #if (CONFIG_MMS_THREADLESS_STACK != 1) && (CONFIG_MMS_SINGLE_THREADED != 1) /* wait for connection thread to terminate */ - if (self->thread) { + if (self->thread) + { Thread_destroy(self->thread); self->thread = NULL; } - else { + else + { finalizeIsoConnection(self); self->state = ISO_CON_STATE_TERMINATED; } diff --git a/src/mms/iso_server/iso_server.c b/src/mms/iso_server/iso_server.c index a2daa6ad..e932406a 100644 --- a/src/mms/iso_server/iso_server.c +++ b/src/mms/iso_server/iso_server.c @@ -90,6 +90,10 @@ struct sIsoServer { #endif int connectionCounter; + + const PSelector* localPSel; + const SSelector* localSSel; + const TSelector* localTSel; }; static void @@ -421,7 +425,8 @@ setupIsoServer(IsoServer self) self->serverSocket = (Socket) TcpServerSocket_create(self->localIpAddress, self->tcpPort); - if (self->serverSocket == NULL) { + if (self->serverSocket == NULL) + { setState(self, ISO_SVR_STATE_ERROR); success = false; @@ -462,7 +467,8 @@ exit_function: static void handleIsoConnections(IsoServer self, bool isSingleThread) { - if (isSingleThread) { + if (isSingleThread) + { /* * NOTE: when running in multi thread mode the tick handler is called * by the connection thread. @@ -475,11 +481,13 @@ handleIsoConnections(IsoServer self, bool isSingleThread) Socket connectionSocket; - if ((connectionSocket = ServerSocket_accept((ServerSocket) self->serverSocket)) != NULL) { - + if ((connectionSocket = ServerSocket_accept((ServerSocket) self->serverSocket)) != NULL) + { #if (CONFIG_MMS_SERVER_CONFIG_SERVICES_AT_RUNTIME == 1) - if (self->maxConnections > -1) { - if (private_IsoServer_getConnectionCounter(self) >= self->maxConnections) { + if (self->maxConnections > -1) + { + if (private_IsoServer_getConnectionCounter(self) >= self->maxConnections) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: maximum number of connections reached -> reject connection attempt.\n"); @@ -491,7 +499,8 @@ handleIsoConnections(IsoServer self, bool isSingleThread) #endif /* (CONFIG_MMS_SERVER_CONFIG_SERVICES_AT_RUNTIME == 1) */ #if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS != -1) - if (private_IsoServer_getConnectionCounter(self) >= CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS) { + if (private_IsoServer_getConnectionCounter(self) >= CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: maximum number of connections reached -> reject connection attempt.\n"); @@ -506,7 +515,8 @@ handleIsoConnections(IsoServer self, bool isSingleThread) IsoConnection isoConnection = IsoConnection_create(connectionSocket, self, isSingleThread); - if (isoConnection) { + if (isoConnection) + { addClientConnection(self, isoConnection); if (isSingleThread) @@ -518,7 +528,8 @@ handleIsoConnections(IsoServer self, bool isSingleThread) if (isSingleThread == false) IsoConnection_start(isoConnection); } - else { + else + { Socket_destroy(connectionSocket); } } @@ -534,7 +545,8 @@ isoServerThread(void* isoServerParam) { IsoServer self = (IsoServer) isoServerParam; - if (!setupIsoServer(self)) { + if (!setupIsoServer(self)) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: starting server failed!\n"); @@ -569,7 +581,8 @@ IsoServer_create(TLSConfiguration tlsConfiguration) { IsoServer self = (IsoServer) GLOBAL_CALLOC(1, sizeof(struct sIsoServer)); - if (self) { + if (self) + { self->state = ISO_SVR_STATE_IDLE; if (tlsConfiguration == NULL) @@ -597,6 +610,10 @@ IsoServer_create(TLSConfiguration tlsConfiguration) #endif /* (CONFIG_MMS_THREADLESS_STACK != 1) */ self->connectionCounter = 0; + + self->localPSel = NULL; + self->localSSel = NULL; + self->localTSel = NULL; } return self; @@ -627,6 +644,15 @@ IsoServer_setLocalIpAddress(IsoServer self, const char* ipAddress) self->localIpAddress = NULL; } +void +IsoServer_setLocalAddresses(IsoServer self, const PSelector* pSel, const SSelector* sSel, + const TSelector* tSel) +{ + self->localPSel = pSel; + self->localSSel = sSel; + self->localTSel = tSel; +} + IsoServerState IsoServer_getState(IsoServer self) { @@ -662,7 +688,8 @@ IsoServer_getTLSConfiguration(IsoServer self) void IsoServer_startListening(IsoServer self) { - if (self->state == ISO_SVR_STATE_RUNNING) { + if (self->state == ISO_SVR_STATE_RUNNING) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: server already in RUNNING state!\n"); @@ -695,13 +722,15 @@ exit_function: void IsoServer_startListeningThreadless(IsoServer self) { - if (!setupIsoServer(self)) { + if (!setupIsoServer(self)) + { if (DEBUG_ISO_SERVER) printf("ISO_SERVER: starting server failed!\n"); self->serverSocket = NULL; } - else { + else + { setState(self, ISO_SVR_STATE_RUNNING); if (DEBUG_ISO_SERVER) @@ -712,21 +741,22 @@ IsoServer_startListeningThreadless(IsoServer self) int IsoServer_waitReady(IsoServer self, unsigned int timeoutMs) { - int result = -1; - - if (getState(self) == ISO_SVR_STATE_RUNNING) { - - if (self->handleset) { - result = Handleset_waitReady(self->handleset, timeoutMs); - } - else { - if (DEBUG_ISO_SERVER) - printf("ISO_SERVER: internal error - no handleset!\n"); - } + int result = -1; - } + if (getState(self) == ISO_SVR_STATE_RUNNING) + { + if (self->handleset) + { + result = Handleset_waitReady(self->handleset, timeoutMs); + } + else + { + if (DEBUG_ISO_SERVER) + printf("ISO_SERVER: internal error - no handleset!\n"); + } + } - return result; + return result; } void @@ -747,7 +777,8 @@ stopListening(IsoServer self) { setState(self, ISO_SVR_STATE_STOPPED); - if (self->serverSocket != NULL) { + if (self->serverSocket) + { ServerSocket_destroy((ServerSocket) self->serverSocket); self->serverSocket = NULL; } @@ -760,7 +791,8 @@ IsoServer_stopListeningThreadless(IsoServer self) closeAllOpenClientConnections(self); - if (self->handleset) { + if (self->handleset) + { Handleset_destroy(self->handleset); self->handleset = NULL; } @@ -778,8 +810,9 @@ IsoServer_stopListening(IsoServer self) if (self->serverThread != NULL) Thread_destroy(self->serverThread); - if (self->serverSocket != NULL) { - ServerSocket_destroy((ServerSocket) self->serverSocket); + if (self->serverSocket != NULL) + { + ServerSocket_destroy((ServerSocket)self->serverSocket); self->serverSocket = NULL; } @@ -789,7 +822,8 @@ IsoServer_stopListening(IsoServer self) while (private_IsoServer_getConnectionCounter(self) > 0) Thread_sleep(10); - if (self->handleset) { + if (self->handleset) + { Handleset_destroy(self->handleset); self->handleset = NULL; } @@ -819,8 +853,8 @@ IsoServer_setConnectionHandler(IsoServer self, ConnectionIndicationHandler handl void IsoServer_destroy(IsoServer self) { - if (self) { - + if (self) + { #if (CONFIG_MMS_THREADLESS_STACK != 1) if (self->state == ISO_SVR_STATE_RUNNING) IsoServer_stopListening(self); @@ -895,3 +929,20 @@ private_IsoServer_getConnectionCounter(IsoServer self) return connectionCounter; } +const PSelector* +private_IsoServer_getLocalPSelector(IsoServer self) +{ + return self->localPSel; +} + +const SSelector* +private_IsoServer_getLocalSSelector(IsoServer self) +{ + return self->localSSel; +} + +const TSelector* +private_IsoServer_getLocalTSelector(IsoServer self) +{ + return self->localTSel; +} diff --git a/src/mms/iso_session/iso_session.c b/src/mms/iso_session/iso_session.c index 9b46bd3a..cdc62613 100644 --- a/src/mms/iso_session/iso_session.c +++ b/src/mms/iso_session/iso_session.c @@ -507,6 +507,14 @@ IsoSession_init(IsoSession* session) session->calledSessionSelector.size = 2; session->calledSessionSelector.value[0] = 0; session->calledSessionSelector.value[1] = 1; + + session->checkCalledSSelector = NULL; +} + +void +IsoSession_setCalledSSelector(IsoSession* session, const SSelector* calledSSelector) +{ + session->checkCalledSSelector = calledSSelector; } ByteBuffer* @@ -515,6 +523,25 @@ IsoSession_getUserData(IsoSession* session) return &session->userData; } +static bool +checkCalledSSelector(IsoSession* self) +{ + if (self->checkCalledSSelector == NULL) + return true; + + if (self->calledSessionSelector.size != self->checkCalledSSelector->size) + return false; + + int i; + for (i = 0; i < self->calledSessionSelector.size; i++) + { + if (self->calledSessionSelector.value[i] != self->checkCalledSSelector->value[i]) + return false; + } + + return true; +} + IsoSessionIndication IsoSession_parseMessage(IsoSession* self, ByteBuffer* message) { @@ -537,8 +564,16 @@ IsoSession_parseMessage(IsoSession* self, ByteBuffer* message) return SESSION_ERROR; if (parseSessionHeaderParameters(self, message, length) == SESSION_OK) { - //TODO check called S-selector when configured - return SESSION_CONNECT; + if (checkCalledSSelector(self)) + { + return SESSION_CONNECT; + } + else + { + if (DEBUG_SESSION) + printf("SESSION: wrong called S-selector value\n"); + return SESSION_ERROR; + } } else {