- 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

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

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