- fixed MAC calculation and parsing of security extension

examples/goose_subscriber/goose_subscriber_example.c

@@ -65,6 +65,16 @@ main(int argc, char** argv)
     GooseSubscriber_setDstMac(subscriber, dstMac);
     GooseSubscriber_setAppId(subscriber, 1000);
 
+    //char* key = "0123456789ABCDEF";
+    char* key = "0123456789ABCDEG";
+
+    L2Security l2Sec = L2Security_create();
+
+    L2Security_addKey(l2Sec, 0x12345678, (uint8_t*)key, 16, MC_SEC_SEC_ALGO_NONE, MC_SEC_SIG_ALGO_HMAC_SHA256_256);
+    L2Security_setActiveKey(l2Sec, 1);
+
+    GooseReceiver_setL2Security(receiver, l2Sec);
+
     GooseSubscriber_setListener(subscriber, gooseListener, NULL);
 
     GooseReceiver_addSubscriber(receiver, subscriber);

src/goose/goose_publisher.c

@@ -502,11 +502,9 @@ GoosePublisher_publish(GoosePublisher self, LinkedList dataSet)
 
         if (self->l2Security)
         {
-            /* add security extension */
+            /* calculate length of security extension */
             secExtLength = L2Security_addSecurityExtension(self->l2Security, self->buffer,
-                self->gooseStart, self->payloadStart + self->payloadLength - self->gooseStart, GOOSE_MAX_MESSAGE_SIZE);
+                self->gooseStart, self->payloadStart + self->payloadLength - self->gooseStart, GOOSE_MAX_MESSAGE_SIZE, false);
-        
-            printf("secExtLength: %i\n", secExtLength);
 
             self->buffer[self->gooseStart + 6] = (uint8_t)((secExtLength >> 8) & 0x0f);
             self->buffer[self->gooseStart + 7] = (uint8_t)(secExtLength & 0xff);
@@ -517,7 +515,9 @@ GoosePublisher_publish(GoosePublisher self, LinkedList dataSet)
             self->buffer[self->gooseStart + 8] = (uint8_t)(crc / 0x100);
             self->buffer[self->gooseStart + 9] = (uint8_t)(crc % 0x100);
 
-            printf("reserved1: %02x %02x\n", self->buffer[self->gooseStart + 6], self->buffer[self->gooseStart + 7]);
+            /* add security extension */
+            L2Security_addSecurityExtension(self->l2Security, self->buffer,
+                self->gooseStart, self->payloadStart + self->payloadLength - self->gooseStart, GOOSE_MAX_MESSAGE_SIZE, true);
         }
 
         gooseLength += secExtLength;

src/goose/goose_publisher.h

@@ -1,7 +1,7 @@
 /*
  *  goose_publisher.h
  *
- *  Copyright 2013-2022 Michael Zillgith
+ *  Copyright 2013-2024 Michael Zillgith
  *
  *  This file is part of libIEC61850.
  *

src/goose/goose_receiver.c

@@ -77,6 +77,10 @@ struct sGooseReceiver
 #if (CONFIG_MMS_THREADLESS_STACK == 0)
     Thread thread;
 #endif
+
+#if (CONFIG_GOOSE_L2_SECURITY == 1)
+    L2Security l2Security;
+#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */
 };
 
 GooseReceiver
@@ -94,6 +98,10 @@ GooseReceiver_createEx(uint8_t* buffer)
 #if (CONFIG_MMS_THREADLESS_STACK == 0)
         self->thread = NULL;
 #endif
+
+#if (CONFIG_GOOSE_L2_SECURITY == 1)
+        self->l2Security = NULL;
+#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */
     }
 
     return self;
@@ -155,6 +163,14 @@ GooseReceiver_getInterfaceId(GooseReceiver self)
         return CONFIG_ETHERNET_INTERFACE_ID;
 }
 
+#if (CONFIG_GOOSE_L2_SECURITY == 1)
+void
+GooseReceiver_setL2Security(GooseReceiver self, L2Security l2Security)
+{
+    self->l2Security = l2Security;
+}
+#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */
+
 static void
 createNewStringFromBufferElement(MmsValue* value, uint8_t* bufferSrc, int elementLength)
 {
@@ -941,6 +957,7 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
         vlanSet = true;
         bufPos += 4; /* skip VLAN tag */
         headerLength += 4;
+        printf("has VLAN tag\n");
     }
 
     int gooseStart = bufPos;
@@ -1023,12 +1040,31 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
             printf("CRC check - FAILED (expected: %04x actual: %04x)\n", secExtCrc, crc);
         }
 
-        /* verify correct lenght of message including security extension */
+        /* verify correct length of message including security extension */
         if (numbytes < length + headerLength + secExtLength) {
             //if (DEBUG_GOOSE_SUBSCRIBER)
                 printf("GOOSE_SUBSCRIBER: Invalid PDU size (security extension is missing)\n");
             return;
         }
+
+        /* check security extension */
+        bool secCheckPassed = false;
+
+        if (self->l2Security)
+        {
+            secCheckPassed = L2Security_checkSecurityExtension(self->l2Security, buffer, gooseStart + 2, length, secExtLength);
+            //secCheckPassed = L2Security_checkSecurityExtension(self->l2Security, buffer, 16, 182, secExtLength);
+
+
+            printf("Security check - %s\n", secCheckPassed ? "OK" : "FAILED");
+        }
+        else
+        {
+            //if (DEBUG_GOOSE_SUBSCRIBER)
+            printf("GOOSE_SUBSCRIBER: ERROR - No security layer specified -> cannot check security extension!\n");
+
+            secCheckPassed = false;
+        }
     }
 
     /* check if there is an interested subscriber */

src/goose/goose_receiver.h

@@ -1,7 +1,7 @@
 /*
  *  goose_receiver.h
  *
- *  Copyright 2014-2022 Michael Zillgith
+ *  Copyright 2014-2024 Michael Zillgith
  *
  *  This file is part of libIEC61850.
  *
@@ -33,6 +33,7 @@ extern "C" {
 #include "hal_ethernet.h"
 #include "goose_subscriber.h"
 #include "r_session.h"
+#include "l2_security.h"
 
 /**
  * \addtogroup goose_api_group
@@ -94,6 +95,15 @@ GooseReceiver_setInterfaceId(GooseReceiver self, const char* interfaceId);
 LIB61850_API const char*
 GooseReceiver_getInterfaceId(GooseReceiver self);
 
+/**
+ * \brief Enable and configure L2 security (signatures)
+ * 
+ * \param self GooseReceiver instance
+ * \param l2Security L2 security instance to use with this GOOSE receiver
+*/
+void
+GooseReceiver_setL2Security(GooseReceiver self, L2Security l2Security);
+
 /**
  * \brief Add a subscriber to this receiver instance
  *

src/goose/goose_sec.c

@@ -3,6 +3,7 @@
 
 #include "l2_security.h"
 #include "ber_encoder.h"
+#include "ber_decode.h"
 #include "r_session_crypto.h"
 
 struct sL2Security {
@@ -66,19 +67,6 @@ calculateCRC(uint8_t* data, int size)
     return (uint16_t)(~crc);
 }
 
-#if 0
-int
-main(int argc, char** argv)
-{
-    uint8_t data[] = { 0x02, 0x07, 0x01, 0x03, 0x01, 0x02, 0x00, 0x34, 0x07, 0x07, 0x1C, 0x59, 0x34, 0x6F, 0xE1, 0x83, 0x00, 0x00, 0x41, 0x06, 0x06, 0x7B, 0x3C, 0xFF, 0xCF, 0x3C, 0xC0 };
-
-    uint16_t crc = calculateCRC(data, sizeof(data));
-
-    printf("CRC = %04x\n", crc);
-    printf("CRC = %04x\n", (uint16_t)(~crc));
-}
-#endif
-
 uint16_t
 L2Security_calculateCRC16(uint8_t* data, int size)
 {
@@ -95,8 +83,9 @@ L2Security_calculateCRC16(uint8_t* data, int size)
  * \return length of the security extension
  */
 uint16_t
-L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int length, int maxBufSize)
+L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int length, int maxBufSize, bool encode)
 {
+    printf("L2Security_addSecurityExtension: start=%i, length=%i, maxBufSize=%i\n", start, length, maxBufSize);
     if (self->currentSigAlgo != MC_SEC_SIG_ALGO_NONE)
     {
         bool hasIV = false;
@@ -150,49 +139,265 @@ L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int
         }
 
         /* start encoding ... */
+        if (encode)
+        {
+            bufPos = BerEncoder_encodeTL(0xa0, securityExtensionSize, buffer, bufPos);
 
-        bufPos = BerEncoder_encodeTL(0xa0, securityExtensionSize, buffer, bufPos);
+            bufPos = BerEncoder_encodeTL(0xa4, authValueSize, buffer, bufPos);
 
-        bufPos = BerEncoder_encodeTL(0xa4, authValueSize, buffer, bufPos);
+            /* encode AuthenticationValue content */
 
-        /* encode AuthenticationValue content */
+            /* Version */
+            bufPos =  BerEncoder_encodeInt32WithTL(0x80, 1, buffer, bufPos);
 
-        /* Version */
+            /* TimeofCurrentKey */
-        bufPos =  BerEncoder_encodeInt32WithTL(0x80, 1, buffer, bufPos);
+            bufPos = BerEncoder_encodeInt32WithTL(0x81, self->timeOfCurrentKey, buffer, bufPos);
 
-        /* TimeofCurrentKey */
+            /* TimeofNextKey */
-        bufPos = BerEncoder_encodeInt32WithTL(0x81, self->timeOfCurrentKey, buffer, bufPos);
+            bufPos = BerEncoder_encodeInt32WithTL(0x82, self->timeToNextKey, buffer, bufPos);
 
-        /* TimeofNextKey */
+            /* IV */
-        bufPos = BerEncoder_encodeInt32WithTL(0x82, self->timeToNextKey, buffer, bufPos);
+            if (hasIV) {
+                //TODO encode IV
+            }
 
-        /* IV */
+            /* KeyID */
-        if (hasIV) {
+            bufPos = BerEncoder_encodeInt32WithTL(0x84, self->currentKeyId, buffer, bufPos);
-            //TODO encode IV
+
+
+            int macEnd = bufPos;
+
+            /* encode mAC */
+            bufPos = BerEncoder_encodeTL(0x85, mACSize - 2, buffer, bufPos);
+
+            if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_128) {
+                RSessionCrypto_createHMAC(buffer + start, macEnd, self->currentKey, self->currentKeySize, buffer + bufPos, 16);
+                bufPos += 16;
+            }
+            else if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_256) {
+                RSessionCrypto_createHMAC(buffer + start, macEnd - start, self->currentKey, self->currentKeySize, buffer + bufPos, 32);
+                bufPos += 32;
+            }
         }
 
-        /* KeyID */
+        return securityExtensionSize + 2;
-        bufPos = BerEncoder_encodeInt32WithTL(0x84, self->currentKeyId, buffer, bufPos);
+    }
+    else {
+        return 0;
+    }
+}
+
+static bool
+checkSecurityExtension(L2Security self, uint8_t* buffer, int secExtLen, uint8_t* macStart)
+{
+    bool hasIV = false;
+    int ivSize = 0;
+    uint8_t* ivBuffer = NULL;
+    int mACSize = 0;
+    uint8_t* mACBuffer = NULL;
+    int bufPos = 0;
+
+    /* determine length of the mAC */
+    if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_128) {
+        mACSize = 2 + 16;
+    }
+    else if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_256) {
+        mACSize = 2 + 32;
+    }
+    else {
+        /* signature algorithm not supported */
+        printf("L2_SECURITY: signature algorithm not supported\n");
+        return false;
+    }
+
+    while (bufPos < secExtLen)
+    {
+        int macEnd = (buffer - macStart) + bufPos;
 
+        uint8_t tag = buffer[bufPos++];
 
-        int macEnd = bufPos;
+        int len = 0;
 
-        /* encode mAC */
+        bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, secExtLen);
-        bufPos = BerEncoder_encodeTL(0x85, mACSize - 2, buffer, bufPos);
 
-        if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_128) {
+        if (bufPos == -1)
-            RSessionCrypto_createHMAC(buffer, macEnd, self->currentKey, self->currentKeySize, buffer + bufPos, 16);
+        {
-            bufPos += 16;
+            printf("L2_SECURITY: invalid len for tag %02x\n", tag);
+            return false;
         }
-        else if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_256) {
+
-            RSessionCrypto_createHMAC(buffer, macEnd, self->currentKey, self->currentKeySize, buffer + bufPos, 32);
+        if (tag == 0xa4)
-            bufPos += 32;
+        {
+            /* AuthenticationValue */
+            uint8_t* authValueBuf = buffer + bufPos;
+            int authValuePos = 0;
+            int authValueLen = len;
+
+            while (authValuePos < authValueLen)
+            {
+                uint8_t authTag = authValueBuf[authValuePos++];
+
+                int authLen = 0;
+
+                authValuePos = BerDecoder_decodeLength(authValueBuf, &authLen, authValuePos, authValueLen);
+
+                if (authValuePos == -1)
+                {
+                    printf("L2_SECURITY: invalid len for tag %02x in AuthenticationValue\n", authTag);
+                    return false;
+                }
+
+                if (authTag == 0x80)
+                {
+                    /* Version */
+                    int32_t version = BerDecoder_decodeInt32(authValueBuf, authLen, authValuePos);
+
+                    if (version != 1)
+                    {
+                        printf("L2_SECURITY: invalid version (%i) in AuthenticationValue\n", version);
+                        return false;
+                    }
+                }
+                else if (authTag == 0x81)
+                {
+                    /* TimeofCurrentKey */
+                    self->timeOfCurrentKey = BerDecoder_decodeUint32(authValueBuf, authLen, authValuePos);
+                }
+                else if (authTag == 0x82)
+                {
+                    /* TimeToNextKey */
+                    self->timeToNextKey = BerDecoder_decodeInt32(authValueBuf, authLen, authValuePos);
+                }
+                else if (authTag == 0x83)
+                {
+                    /* IV */
+                    hasIV = true;
+                    ivBuffer = authValueBuf + authValuePos;
+                    ivSize = authLen;
+                }
+                else if (authTag == 0x84)
+                {
+                    /* KeyID */
+                    uint32_t keyId = BerDecoder_decodeUint32(authValueBuf, authLen, authValuePos);
+
+                    if (keyId != self->currentKeyId)
+                    {
+                        printf("L2_SECURITY: invalid key ID in AuthenticationValue\n");
+                        return false;
+                    }
+                }
+                else
+                {
+                    printf("L2_SECURITY: invalid tag in AuthenticationValue\n");
+                    return false;
+                }
+
+                authValuePos += authLen;
+            }
+        }
+        else if (tag == 0x85)
+        {
+            /* mAC */
+
+            mACBuffer = buffer + bufPos;
+            mACSize = len;
+
+            printf("L2_SECURITY: found MAC with size: %i\n", mACSize);
+
+            if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_128)
+            {
+                printf("Algo: HMAC_SHA256_128\n");
+
+                uint8_t calculatedMac[16];
+
+                RSessionCrypto_createHMAC(macStart, macEnd, self->currentKey, self->currentKeySize, calculatedMac, sizeof(calculatedMac));
+
+                if (memcmp(calculatedMac, mACBuffer, 16) != 0)
+                {
+                    printf("L2_SECURITY: MAC mismatch\n");
+                    return false;
+                }
+            }
+            else if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_256)
+            {
+                 printf("Algo: HMAC_SHA256_256\n");
+
+                uint8_t calculatedMac[32];
+
+                RSessionCrypto_createHMAC(macStart, macEnd, self->currentKey, self->currentKeySize, calculatedMac, sizeof(calculatedMac));
+
+                if (memcmp(calculatedMac, mACBuffer, 32) != 0)
+                {
+                    printf("L2_SECURITY: MAC mismatch\n");
+                    return false;
+                }
+            }
+            else
+            {
+                printf("L2_SECURITY: signature algorithm not supported\n");
+                return false;
+            }
+        }
+        else {
+            printf("L2_SECURITY: invalid tag %02x in security extension\n", tag);
+
+            return false;
         }
 
-        return securityExtensionSize;
+        bufPos += len;
     }
-    else {
+
-        return 0;
+
+    return true;
+}
+
+bool
+L2Security_checkSecurityExtension(L2Security self, uint8_t* buffer, int start, int length, int secExtSize)
+{
+    if (self->currentSigAlgo == MC_SEC_SIG_ALGO_NONE)
+    {
+        if (secExtSize > 0)
+        {
+            printf("L2_SECURITY: security extension found but no security association\n");
+            return false;
+        }
+        else
+        {
+            return true;
+        }
+    }
+    else 
+    {
+        uint8_t* secExtBuf = buffer + start + length;
+
+        int bufPos = 0;
+
+        while (bufPos < secExtSize)
+        {
+            uint8_t tag = secExtBuf[bufPos++];
+            int len = 0;
+
+            bufPos = BerDecoder_decodeLength(secExtBuf, &len, bufPos, secExtSize);
+
+            if (bufPos == -1)
+            {
+                printf("L2_SECURITY: [2] invalid len for tag %02x\n", tag);
+                return false;
+            }
+
+            if (tag == 0xa0)
+            {
+                /* SecurityExtension */
+                printf("L2_SECURITY: found security extension\n");
+                return checkSecurityExtension(self, secExtBuf + bufPos, len, buffer + start - 2);
+            }
+            else {
+                printf("L2_SECURITY: invalid tag %02x in security extension\n", tag);
+                return false;
+            }
+        }
+
+        return false;
     }
 }
 

src/goose/l2_security.h

@@ -105,12 +105,26 @@ L2Security_calculateCRC16(uint8_t* data, int size);
  * 
  * \param buffer buffer with the encoded GOOSE message (security extension will be added)
  * \param start start of the message payload used to calculate the MAC
- * \param length length of the message payload 
+ * \param length length of the message payload
+ * \param encode true if the security extension should be encoded, false only length calculation without modifying the buffer
  * 
  * \return length of the security extension
  */
 uint16_t
-L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int length, int maxBufSize);
+L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int length, int maxBufSize, bool encode);
+
+/**
+ * \brief Check the security extension
+ * 
+ * \param buffer buffer with the encoded GOOSE message
+ * \param start start of the message payload used to calculate the MAC
+ * \param length length of the message payload
+ * \param secExtSize size of the security extension
+ * 
+ * \return true if the security extension is valid
+ */
+bool
+L2Security_checkSecurityExtension(L2Security self, uint8_t* buffer, int start, int length, int secExtSize);
 
 LIB61850_API void
 L2Security_destroy(L2Security self);

src/r_session/r_session_crypto_mbedtls.c

@@ -47,6 +47,11 @@ RSessionCrypto_createHMAC(uint8_t* buffer, int bufSize, uint8_t* key, int keySiz
 
     mbedtls_md_setup(&md_ctx, md_info, 1);
 
+    for (int i = 0; i < bufSize; i++) {
+        printf("%02x", buffer[i]);
+    }
+    printf("\n");
+
     if (mbedtls_md_hmac_starts(&md_ctx, key, keySize)) {
         printf("Error in initializing HMAC\n");
         return false;