Merge branch 'v1.5' of https://github.com/mz-automation/libiec61850 into v1.5

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);
 
 /**

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);

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);
                        }

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)

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 */

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);
     }
 
 }

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)
 {

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");