From a7dc9148c2178db9b1d36fb1bb569d6dfaa253b3 Mon Sep 17 00:00:00 2001 From: Michael Zillgith Date: Mon, 7 Nov 2022 15:35:14 +0000 Subject: [PATCH] - implemented GOOSE signatures in GOOSE publisher (LIB61850-329) --- .../goose_publisher/goose_publisher_example.c | 9 + .../r_goose_publisher_example.c | 1 + src/CMakeLists.txt | 1 + src/goose/goose_publisher.c | 130 +++++++--- src/goose/goose_publisher.h | 10 + src/goose/goose_sec.c | 243 ++++++++++++++++++ src/goose/l2_security.h | 122 +++++++++ src/mms/asn1/ber_encoder.c | 28 ++ src/mms/inc_private/ber_encoder.h | 6 + src/r_session/r_session.c | 2 +- 10 files changed, 514 insertions(+), 38 deletions(-) create mode 100644 src/goose/goose_sec.c create mode 100644 src/goose/l2_security.h diff --git a/examples/goose_publisher/goose_publisher_example.c b/examples/goose_publisher/goose_publisher_example.c index 491ae95a..a31514c2 100644 --- a/examples/goose_publisher/goose_publisher_example.c +++ b/examples/goose_publisher/goose_publisher_example.c @@ -57,6 +57,15 @@ main(int argc, char **argv) GoosePublisher_setDataSetRef(publisher, "simpleIOGenericIO/LLN0$AnalogValues"); GoosePublisher_setTimeAllowedToLive(publisher, 500); + char* key = "0123456789ABCDEF"; + + 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); + + GoosePublisher_setL2Security(publisher, l2Sec); + int i = 0; for (i = 0; i < 4; i++) { diff --git a/examples/r_goose_publisher_example/r_goose_publisher_example.c b/examples/r_goose_publisher_example/r_goose_publisher_example.c index 0434c343..c0018631 100644 --- a/examples/r_goose_publisher_example/r_goose_publisher_example.c +++ b/examples/r_goose_publisher_example/r_goose_publisher_example.c @@ -12,6 +12,7 @@ #include "mms_value.h" #include "goose_publisher.h" #include "hal_thread.h" +#include "l2_security.h" static bool running = true; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 65b3ca1a..3172ea01 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -182,6 +182,7 @@ set (lib_goose_SRCS ./goose/goose_subscriber.c ./goose/goose_receiver.c ./goose/goose_publisher.c +./goose/goose_sec.c ) set (lib_sv_SRCS diff --git a/src/goose/goose_publisher.c b/src/goose/goose_publisher.c index 7409bd21..c53b70b4 100644 --- a/src/goose/goose_publisher.c +++ b/src/goose/goose_publisher.c @@ -35,6 +35,12 @@ #define DEBUG_GOOSE_PUBLISHER 0 #endif +#define CONFIG_GOOSE_L2_SECURITY 1 + +#if (CONFIG_GOOSE_L2_SECURITY == 1) +#include "l2_security.h" +#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */ + #define GOOSE_MAX_MESSAGE_SIZE 1518 static bool @@ -52,7 +58,11 @@ struct sGoosePublisher { /* only for Ethernet based GOOSE */ EthernetSocket ethernetSocket; - int lengthField; +#if (CONFIG_GOOSE_L2_SECURITY == 1) + L2Security l2Security; +#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */ + + int gooseStart; int payloadStart; int payloadLength; @@ -89,7 +99,6 @@ GoosePublisher_createRemote(RSession session, uint16_t appId) self->payloadStart = 0; self->remoteSession = session; - self->lengthField = 0; self->simulation = false; self->appId = appId; @@ -153,6 +162,14 @@ GoosePublisher_destroy(GoosePublisher self) } } +#if (CONFIG_GOOSE_L2_SECURITY == 1) +void +GoosePublisher_setL2Security(GoosePublisher self, L2Security l2Security) +{ + self->l2Security = l2Security; +} +#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */ + void GoosePublisher_setGoID(GoosePublisher self, char* goID) { @@ -269,54 +286,55 @@ prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char* if (self->ethernetSocket) { self->buffer = (uint8_t*) GLOBAL_MALLOC(GOOSE_MAX_MESSAGE_SIZE); - memcpy(self->buffer, dstAddr, 6); - memcpy(self->buffer + 6, srcAddr, 6); + if (self->buffer) { + memcpy(self->buffer, dstAddr, 6); + memcpy(self->buffer + 6, srcAddr, 6); - int bufPos = 12; + int bufPos = 12; - if (useVlanTags) { - /* Priority tag - IEEE 802.1Q */ - self->buffer[bufPos++] = 0x81; - self->buffer[bufPos++] = 0x00; + if (useVlanTags) { + /* Priority tag - IEEE 802.1Q */ + self->buffer[bufPos++] = 0x81; + self->buffer[bufPos++] = 0x00; - uint8_t tci1 = priority << 5; - tci1 += vlanId / 256; + uint8_t tci1 = priority << 5; + tci1 += vlanId / 256; - uint8_t tci2 = vlanId % 256; + uint8_t tci2 = vlanId % 256; - self->buffer[bufPos++] = tci1; /* Priority + VLAN-ID */ - self->buffer[bufPos++] = tci2; /* VLAN-ID */ - } + self->buffer[bufPos++] = tci1; /* Priority + VLAN-ID */ + self->buffer[bufPos++] = tci2; /* VLAN-ID */ + } - /* EtherType GOOSE */ - self->buffer[bufPos++] = 0x88; - self->buffer[bufPos++] = 0xB8; + /* EtherType GOOSE */ + self->gooseStart = bufPos; - /* APPID */ - self->buffer[bufPos++] = appId / 256; - self->buffer[bufPos++] = appId % 256; + self->buffer[bufPos++] = 0x88; + self->buffer[bufPos++] = 0xB8; - self->lengthField = bufPos; + /* APPID */ + self->buffer[bufPos++] = appId / 256; + self->buffer[bufPos++] = appId % 256; - /* Length */ - self->buffer[bufPos++] = 0x00; - self->buffer[bufPos++] = 0x08; + /* Length */ + self->buffer[bufPos++] = 0x00; + self->buffer[bufPos++] = 0x08; - /* Reserved1 */ - self->buffer[bufPos++] = 0x00; - self->buffer[bufPos++] = 0x00; + /* Reserved1 */ + self->buffer[bufPos++] = 0x00; + self->buffer[bufPos++] = 0x00; - /* Reserved2 */ - self->buffer[bufPos++] = 0x00; - self->buffer[bufPos++] = 0x00; + /* Reserved2 */ + self->buffer[bufPos++] = 0x00; + self->buffer[bufPos++] = 0x00; - self->payloadStart = bufPos; + self->payloadStart = bufPos; - return true; - } - else { - return false; + return true; + } } + + return false; } static int32_t @@ -463,15 +481,53 @@ GoosePublisher_publish(GoosePublisher self, LinkedList dataSet) self->sqNum = 1; if (self->ethernetSocket) { - int lengthIndex = self->lengthField; + int lengthIndex = self->gooseStart + 4; size_t gooseLength = self->payloadLength + 8; + /* set simulation flag in reserved_1 field */ + if (self->simulation) { + self->buffer[self->gooseStart + 6] = 0x80; + } + else { + self->buffer[self->gooseStart + 6] = 0x00; + } + +#if (CONFIG_GOOSE_L2_SECURITY == 1) + + int secExtLength = 0; + + if (self->l2Security) { + /* calculate crc */ + uint16_t crc = L2Security_calculateCRC16(self->buffer + self->gooseStart, 8); + + self->buffer[self->gooseStart + 8] = (uint8_t)(crc & 0x00ff); + self->buffer[self->gooseStart + 9] = (uint8_t)((crc >> 8) & 0x00ff); + + /* add security extension */ + secExtLength = L2Security_addSecurityExtension(self->l2Security, self->buffer, + self->gooseStart, self->payloadStart + self->payloadLength - self->gooseStart, GOOSE_MAX_MESSAGE_SIZE); + + self->buffer[self->gooseStart + 6] = (uint8_t)((secExtLength >> 8) & 0x00ff); + self->buffer[self->gooseStart + 7] = (uint8_t)(secExtLength & 0x00ff); + } + + gooseLength += secExtLength; + + self->buffer[lengthIndex] = gooseLength / 256; + self->buffer[lengthIndex + 1] = gooseLength & 0xff; + + Ethernet_sendPacket(self->ethernetSocket, self->buffer, self->payloadStart + self->payloadLength + secExtLength); + +#else + self->buffer[lengthIndex] = gooseLength / 256; self->buffer[lengthIndex + 1] = gooseLength & 0xff; Ethernet_sendPacket(self->ethernetSocket, self->buffer, self->payloadStart + self->payloadLength); +#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */ + if (DEBUG_GOOSE_PUBLISHER) printf("GOOSE_PUBLISHER: send GOOSE message\n"); } diff --git a/src/goose/goose_publisher.h b/src/goose/goose_publisher.h index 170af7fd..695c7673 100644 --- a/src/goose/goose_publisher.h +++ b/src/goose/goose_publisher.h @@ -26,6 +26,7 @@ #include "iec61850_common.h" #include "r_session.h" +#include "l2_security.h" #include "linked_list.h" #include "mms_value.h" @@ -114,6 +115,15 @@ GoosePublisher_publish(GoosePublisher self, LinkedList dataSet); LIB61850_API int GoosePublisher_publishAndDump(GoosePublisher self, LinkedList dataSet, char* msgBuf, int32_t* msgLen, int32_t bufSize); +/** + * \brief Enable and configure L2 security (signatures) + * + * \param self GoosePublisher instance + * \param l2Security L2 security instance to use with this GOOSE publisher +*/ +LIB61850_API void +GoosePublisher_setL2Security(GoosePublisher self, L2Security l2Security); + /** * \brief Sets the GoID used by the GoosePublisher instance * diff --git a/src/goose/goose_sec.c b/src/goose/goose_sec.c new file mode 100644 index 00000000..b5706aa7 --- /dev/null +++ b/src/goose/goose_sec.c @@ -0,0 +1,243 @@ +#include +#include + +#include "l2_security.h" +#include "ber_encoder.h" +#include "r_session_crypto.h" + +struct sL2Security { + RSignatureAlgorithm currentSigAlgo; + + uint32_t timeOfCurrentKey; + uint32_t currentKeyId; + uint8_t* currentKey; + int currentKeySize; + + int timeToNextKey; +}; + +static uint16_t poly[] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +}; + +/* see https://www.rfc-editor.org/rfc/rfc1662#page-19 */ +static uint16_t +calculateCRC(uint8_t* data, int size) +{ + uint16_t crc = 0xffff; + + int i; + + for (i = 0; i < size; i++) { + crc = (uint16_t)(((uint16_t)(crc >> 8)) ^ poly[(crc ^ data[i]) & 0xff]); + } + + 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) +{ + return calculateCRC(data, size); +} + +/** + * \brief Create the security extension + * + * \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 + * + * \return length of the security extension + */ +uint16_t +L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int length, int maxBufSize) +{ + if (self->currentSigAlgo != MC_SEC_SIG_ALGO_NONE) { + bool hasIV = false; + int ivSize = 0; + int mACSize = 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 */ + return 0; + } + + uint32_t securityExtensionSize = 0; + + /* TODO check for applicable/valid key */ + + /* determine length of the AuthenticationValue */ + + uint32_t authValueSize = 3; /* TLV(Version) */ + + /* TimeofCurrentKey */ + authValueSize += (2 + BerEncoder_UInt32determineEncodedSize(self->timeOfCurrentKey)); + + /* TimeToNextKey */ + authValueSize += (2 + BerEncoder_Int32determineEncodedSize(self->timeToNextKey)); + + /* IV */ + if (hasIV) { + authValueSize += (2 + ivSize); + } + + /* Key ID */ + authValueSize += (2 + BerEncoder_UInt32determineEncodedSize(self->currentKeyId)); + + securityExtensionSize += (1 + BerEncoder_determineLengthSize(authValueSize) + authValueSize); + securityExtensionSize += mACSize; + + //TODO check that total size fits into the buffer! + + int bufPos = start + length; + + if (bufPos + securityExtensionSize > maxBufSize) { + printf("L2_SECURITY: security extension doesn't fit into message\n"); + return 0; + } + + /* start encoding ... */ + + bufPos = BerEncoder_encodeTL(0xa0, securityExtensionSize, buffer, bufPos); + + bufPos = BerEncoder_encodeTL(0xa4, authValueSize, buffer, bufPos); + + //TODO encode AuthenticationValue content + + /* Version */ + bufPos = BerEncoder_encodeInt32WithTL(0x80, 1, buffer, bufPos); + + /* TimeofCurrentKey */ + bufPos = BerEncoder_encodeInt32WithTL(0x81, self->timeOfCurrentKey, buffer, bufPos); + + /* TimeofNextKey */ + bufPos = BerEncoder_encodeInt32WithTL(0x82, self->timeToNextKey, buffer, bufPos); + + /* 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, macEnd, self->currentKey, self->currentKeySize, buffer + bufPos, 16); + bufPos += 16; + } + else if (self->currentSigAlgo == MC_SEC_SIG_ALGO_HMAC_SHA256_256) { + RSessionCrypto_createHMAC(buffer, macEnd, self->currentKey, self->currentKeySize, buffer + bufPos, 32); + bufPos += 32; + } + + return securityExtensionSize; + } + else { + return 0; + } +} + +L2Security +L2Security_create() +{ + L2Security self = (L2Security)GLOBAL_CALLOC(1, sizeof(struct sL2Security)); + + if (self) { + + } + + return self; +} + +MCKeyError +L2Security_addKey(L2Security self, uint32_t keyId, uint8_t* key, int keyLength, MCSecurityAlgorithm secAlgo, MCSignatureAlgorithm sigAlgo) +{ + self->currentKeyId = keyId; + self->currentSigAlgo = sigAlgo; + self->currentKey = key; + self->currentKeySize = keyLength; +} + +MCKeyError +L2Security_removeKey(L2Security self, uint32_t keyId) +{ + +} + +void +L2Security_removeAllKeys(L2Security self) +{ + +} + +RSessionError +L2Security_setActiveKey(L2Security self, uint32_t keyId) +{ + self->currentKeyId = keyId; +} + +void +L2Security_destroy(L2Security self) +{ + if (self) { + GLOBAL_FREEMEM(self); + } +} diff --git a/src/goose/l2_security.h b/src/goose/l2_security.h new file mode 100644 index 00000000..377c26c1 --- /dev/null +++ b/src/goose/l2_security.h @@ -0,0 +1,122 @@ +/* + * l2_security.h + * + * Copyright 2022 Michael Zillgith + * + * This file is part of libIEC61850. + * + * libIEC61850 is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * libIEC61850 is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with libIEC61850. If not, see . + * + * See COPYING file for the complete license text. + */ + +#ifndef LIBIEC61850_L2_SECURITY_H_ +#define LIBIEC61850_L2_SECURITY_H_ + +#include "libiec61850_common_api.h" +#include "r_session.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + MULTICAST_KEY_ERROR_OK = 0, + MULTICAST_KEY_ERROR_INVALID_KEY = 1, + MULTICAST_KEY_ERROR_KEY_QUEUE_FULL = 2, + MULTICAST_KEY_ERROR_SEC_ALGO_NOT_SUPPORTED = 3, + MULTICAST_KEY_ERROR_SIG_ALGO_NOT_SUPPORTED = 4, + MULTICAST_KEY_ERROR_OUT_OF_MEMORY = 5, + MULTICAST_KEY_ERROR_INVALID_MESSAGE = 7, + MULTICAST_KEY_ERROR_SET_FAILED = 8 +} MCKeyError; + +typedef enum { + MC_SEC_SEC_ALGO_NONE = 0, + MC_SEC_SEC_ALGO_AES_128_GCM = 1, + MC_SEC_SEC_ALGO_AES_256_GCM = 2 +} MCSecurityAlgorithm; + +typedef enum { + MC_SEC_SIG_ALGO_NONE = 0, + MC_SEC_SIG_ALGO_HMAC_SHA256_80 = 1, + MC_SEC_SIG_ALGO_HMAC_SHA256_128 = 2, + MC_SEC_SIG_ALGO_HMAC_SHA256_256 = 3, + MC_SEC_SIG_ALGO_AES_GMAC_64 = 4, + MC_SEC_SIG_ALGO_AES_GMAC_128 = 5, + MC_SEC_SIG_ALGO_HMAC_SHA3_80 = 6, + MC_SEC_SIG_ALGO_HMAC_SHA3_128 = 7, + MC_SEC_SIG_ALGO_HMAC_SHA3_256 = 8 +} MCSignatureAlgorithm; + +typedef enum +{ + MC_SEC_KEY_EVENT__NEED_KEY = 1 +} MCKeyEvent; + +typedef void (*MC_KeyEventHandler) (void* parameter, RSession rSession, MCKeyEvent event, uint32_t keyID); + + + +typedef struct sL2Security* L2Security; + +LIB61850_API L2Security +L2Security_create(); + +LIB61850_API MCKeyError +L2Security_addKey(L2Security self, uint32_t keyId, uint8_t* key, int keyLength, MCSecurityAlgorithm secAlgo, MCSignatureAlgorithm sigAlgo); + +/** + * \brief Remove key from the list of accepted keys + * + * \param self the MulticastKeystore instance + * \param keyId the key ID is unique for the security association + */ +LIB61850_API MCKeyError +L2Security_removeKey(L2Security self, uint32_t keyId); + +/** + * \brief Remove all keys from the list of accepted keys + * + * \param self the L2Security instance + */ +void +L2Security_removeAllKeys(L2Security self); + +LIB61850_API RSessionError +L2Security_setActiveKey(L2Security self, uint32_t keyId); + +uint16_t +L2Security_calculateCRC16(uint8_t* data, int size); + +/** + * \brief Create the security extension + * + * \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 + * + * \return length of the security extension + */ +uint16_t +L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int length, int maxBufSize); + +LIB61850_API void +L2Security_destroy(L2Security self); + +#ifdef __cplusplus +} +#endif + +#endif /* LIBIEC61850_L2_SECURITY_H_ */ \ No newline at end of file diff --git a/src/mms/asn1/ber_encoder.c b/src/mms/asn1/ber_encoder.c index 63566f38..7b418d91 100644 --- a/src/mms/asn1/ber_encoder.c +++ b/src/mms/asn1/ber_encoder.c @@ -310,6 +310,12 @@ BerEncoder_encodeUInt32WithTL(uint8_t tag, uint32_t value, uint8_t* buffer, int return bufPos; } +int +BerEncoder_encodeInt32WithTL(uint8_t tag, int32_t value, uint8_t* buffer, int bufPos) +{ + return BerEncoder_encodeUInt32WithTL(tag, (uint32_t) value, buffer, bufPos); +} + int BerEncoder_encodeFloat(uint8_t* floatValue, uint8_t formatWidth, uint8_t exponentWidth, uint8_t* buffer, int bufPos) @@ -356,6 +362,28 @@ BerEncoder_UInt32determineEncodedSize(uint32_t value) return size; } +int +BerEncoder_Int32determineEncodedSize(int32_t value) +{ + uint8_t* valueArray = (uint8_t*) &value; + uint8_t valueBuffer[5]; + + valueBuffer[0] = 0; + + int i; + for (i = 0; i < 4; i++) { + valueBuffer[i + 1] = valueArray[i]; + } + +#if (ORDER_LITTLE_ENDIAN == 1) + BerEncoder_revertByteOrder(valueBuffer + 1, 4); +#endif + + int size = BerEncoder_compressInteger(valueBuffer, 5); + + return size; +} + int BerEncoder_determineLengthSize(uint32_t length) { diff --git a/src/mms/inc_private/ber_encoder.h b/src/mms/inc_private/ber_encoder.h index 48cd2bc6..c1dcf65c 100644 --- a/src/mms/inc_private/ber_encoder.h +++ b/src/mms/inc_private/ber_encoder.h @@ -61,6 +61,9 @@ BerEncoder_encodeInt32(int32_t value, uint8_t* buffer, int bufPos); LIB61850_INTERNAL int BerEncoder_encodeUInt32WithTL(uint8_t tag, uint32_t value, uint8_t* buffer, int bufPos); +LIB61850_INTERNAL int +BerEncoder_encodeInt32WithTL(uint8_t tag, int32_t value, uint8_t* buffer, int bufPos); + LIB61850_INTERNAL int BerEncoder_encodeBitString(uint8_t tag, int bitStringSize, uint8_t* bitString, uint8_t* buffer, int bufPos); @@ -75,6 +78,9 @@ BerEncoder_encodeFloat(uint8_t* floatValue, uint8_t formatWidth, uint8_t exponen LIB61850_INTERNAL int BerEncoder_UInt32determineEncodedSize(uint32_t value); +LIB61850_INTERNAL int +BerEncoder_Int32determineEncodedSize(int32_t value); + LIB61850_INTERNAL int BerEncoder_determineLengthSize(uint32_t length); diff --git a/src/r_session/r_session.c b/src/r_session/r_session.c index 704aba19..cd98c3ec 100644 --- a/src/r_session/r_session.c +++ b/src/r_session/r_session.c @@ -372,7 +372,7 @@ decodeInt16FixedSize(int16_t* outValue, uint8_t* buffer, int bufPos) } static bool -lookupKey(RSession self, uint32_t keyId, uint8_t** key, int* keySize, RSecurityAlgorithm* secAlgo, RSignatureAlgorithm* sigAlgo) +lookupKey(RSession self, uint32_t keyId, uint8_t** key, int* keySize, RSecurityAlgorithm* secAlgo, RSignatureAlgorithm* sigAlgo) { if (keyId == 0) { DEBUG_PRINTF("Invalid key ID");