RYTEC
/
libiec61850
mirror of https://github.com/mz-automation/libiec61850.git
6
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

- fixed MAC calculation and parsing of security extension

Browse Source
v1.6_develop_329_GOOSE_signatures
Michael Zillgith 1 year ago
parent 58518750e7
commit baba27cd9e
8 changed files with 330 additions and 50 deletions
Show Stats Download Patch File Download Diff File

10
examples/goose_subscriber/goose_subscriber_example.c
Unescape Escape View File

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

10
src/goose/goose_publisher.c
Unescape Escape View File

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

2
src/goose/goose_publisher.h
Unescape Escape View File

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

38
src/goose/goose_receiver.c
Unescape Escape View File

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

12
src/goose/goose_receiver.h
Unescape Escape View File

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

241
src/goose/goose_sec.c
Unescape Escape View File

@ -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,7 +139,8 @@ L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int
}
/* start encoding ... */
if (encode)
{
bufPos = BerEncoder_encodeTL(0xa0, securityExtensionSize, buffer, bufPos);
bufPos = BerEncoder_encodeTL(0xa4, authValueSize, buffer, bufPos);
@ -181,21 +171,236 @@ L2Security_addSecurityExtension(L2Security self, uint8_t* buffer, int start, int
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);
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, macEnd, self->currentKey, self->currentKeySize, buffer + bufPos, 32);
RSessionCrypto_createHMAC(buffer + start, macEnd - start, self->currentKey, self->currentKeySize, buffer + bufPos, 32);
bufPos += 32;
}
}
return securityExtensionSize;
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 len = 0;
bufPos = BerDecoder_decodeLength(buffer, &len, bufPos, secExtLen);
if (bufPos == -1)
{
printf("L2_SECURITY: invalid len for tag %02x\n", tag);
return false;
}
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;
}
bufPos += len;
}
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;
}
}
L2Security
L2Security_create()
{

16
src/goose/l2_security.h
Unescape Escape View File

@ -106,11 +106,25 @@ 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 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);

5
src/r_session/r_session_crypto_mbedtls.c
Unescape Escape View File

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

Write Preview
Loading…
Cancel
Save