From baba27cd9ed97657773681407752c1844c382866 Mon Sep 17 00:00:00 2001 From: Michael Zillgith Date: Thu, 11 Jul 2024 21:15:52 +0100 Subject: [PATCH] - fixed MAC calculation and parsing of security extension --- .../goose_subscriber_example.c | 10 + src/goose/goose_publisher.c | 10 +- src/goose/goose_publisher.h | 2 +- src/goose/goose_receiver.c | 38 ++- src/goose/goose_receiver.h | 12 +- src/goose/goose_sec.c | 285 +++++++++++++++--- src/goose/l2_security.h | 18 +- src/r_session/r_session_crypto_mbedtls.c | 5 + 8 files changed, 330 insertions(+), 50 deletions(-) diff --git a/examples/goose_subscriber/goose_subscriber_example.c b/examples/goose_subscriber/goose_subscriber_example.c index bea2280e..3edd3db0 100644 --- a/examples/goose_subscriber/goose_subscriber_example.c +++ b/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); diff --git a/src/goose/goose_publisher.c b/src/goose/goose_publisher.c index df125133..77226c5c 100644 --- a/src/goose/goose_publisher.c +++ b/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); - - printf("secExtLength: %i\n", secExtLength); + self->gooseStart, self->payloadStart + self->payloadLength - self->gooseStart, GOOSE_MAX_MESSAGE_SIZE, false); 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; diff --git a/src/goose/goose_publisher.h b/src/goose/goose_publisher.h index 695c7673..00e88b47 100644 --- a/src/goose/goose_publisher.h +++ b/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. * diff --git a/src/goose/goose_receiver.c b/src/goose/goose_receiver.c index 4206cd42..cee7d870 100644 --- a/src/goose/goose_receiver.c +++ b/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 */ diff --git a/src/goose/goose_receiver.h b/src/goose/goose_receiver.h index 4637c6a8..aebbd779 100644 --- a/src/goose/goose_receiver.h +++ b/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 * diff --git a/src/goose/goose_sec.c b/src/goose/goose_sec.c index 4471190b..72569389 100644 --- a/src/goose/goose_sec.c +++ b/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 */ - bufPos = BerEncoder_encodeInt32WithTL(0x80, 1, buffer, bufPos); + /* TimeofCurrentKey */ + bufPos = BerEncoder_encodeInt32WithTL(0x81, self->timeOfCurrentKey, buffer, bufPos); - /* TimeofCurrentKey */ - bufPos = BerEncoder_encodeInt32WithTL(0x81, self->timeOfCurrentKey, buffer, bufPos); + /* TimeofNextKey */ + bufPos = BerEncoder_encodeInt32WithTL(0x82, self->timeToNextKey, buffer, bufPos); - /* TimeofNextKey */ - bufPos = BerEncoder_encodeInt32WithTL(0x82, self->timeToNextKey, buffer, bufPos); + /* IV */ + if (hasIV) { + //TODO encode IV + } - /* IV */ - if (hasIV) { - //TODO encode IV + /* KeyID */ + bufPos = BerEncoder_encodeInt32WithTL(0x84, self->currentKeyId, buffer, bufPos); + + + 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 */ - bufPos = BerEncoder_encodeInt32WithTL(0x84, self->currentKeyId, buffer, bufPos); + return securityExtensionSize + 2; + } + 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 = BerEncoder_encodeTL(0x85, mACSize - 2, buffer, bufPos); + bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, secExtLen); - if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_128) { - RSessionCrypto_createHMAC(buffer, macEnd, self->currentKey, self->currentKeySize, buffer + bufPos, 16); - bufPos += 16; + if (bufPos == -1) + { + 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); - bufPos += 32; + + if (tag == 0xa4) + { + /* 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; } } diff --git a/src/goose/l2_security.h b/src/goose/l2_security.h index 377c26c1..4e0f8525 100644 --- a/src/goose/l2_security.h +++ b/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); diff --git a/src/r_session/r_session_crypto_mbedtls.c b/src/r_session/r_session_crypto_mbedtls.c index 0fe577cb..ae31b34e 100644 --- a/src/r_session/r_session_crypto_mbedtls.c +++ b/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;