diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 93aa670f..67567e4b 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -68,6 +68,7 @@ if(${BUILD_SV_GOOSE_EXAMPLES}) add_subdirectory(goose_observer) add_subdirectory(goose_subscriber) add_subdirectory(goose_publisher) + add_subdirectory(goose_publisher2) add_subdirectory(sv_subscriber) add_subdirectory(iec61850_9_2_LE_example) add_subdirectory(iec61850_sv_client_example) diff --git a/examples/goose_publisher2/CMakeLists.txt b/examples/goose_publisher2/CMakeLists.txt new file mode 100644 index 00000000..d8eb8c10 --- /dev/null +++ b/examples/goose_publisher2/CMakeLists.txt @@ -0,0 +1,20 @@ + +set(goose_publisher_example_SRCS + goose_publisher_example2.c +) + +IF(MSVC) + +set_source_files_properties(${goose_publisher_example_SRCS} + PROPERTIES LANGUAGE CXX) +ENDIF(MSVC) + +add_executable(goose_publisher_example2 + ${goose_publisher_example_SRCS} +) + +target_link_libraries(goose_publisher_example2 + iec61850 +) + + diff --git a/examples/goose_publisher2/Makefile b/examples/goose_publisher2/Makefile new file mode 100644 index 00000000..1c002227 --- /dev/null +++ b/examples/goose_publisher2/Makefile @@ -0,0 +1,17 @@ +LIBIEC_HOME=../.. + +PROJECT_BINARY_NAME = goose_publisher_example2 +PROJECT_SOURCES = goose_publisher_example2.c + +include $(LIBIEC_HOME)/make/target_system.mk +include $(LIBIEC_HOME)/make/stack_includes.mk + +all: $(PROJECT_BINARY_NAME) + +include $(LIBIEC_HOME)/make/common_targets.mk + +$(PROJECT_BINARY_NAME): $(PROJECT_SOURCES) $(LIB_NAME) + $(CC) $(CFLAGS) $(LDFLAGS) -o $(PROJECT_BINARY_NAME) $(PROJECT_SOURCES) $(INCLUDES) $(LIB_NAME) $(LDLIBS) + +clean: + rm -f $(PROJECT_BINARY_NAME) diff --git a/examples/goose_publisher2/goose_publisher_example2.c b/examples/goose_publisher2/goose_publisher_example2.c new file mode 100644 index 00000000..e909590c --- /dev/null +++ b/examples/goose_publisher2/goose_publisher_example2.c @@ -0,0 +1,97 @@ +/* + * goose_publisher_example.c + */ + +#include +#include +#include +#include +#include + +#include "mms_value.h" +#include "goose_publisher.h" +#include "hal_thread.h" + +/* has to be executed as root! */ +int +main(int argc, char **argv) +{ + char *interface; + + if (argc > 1) + interface = argv[1]; + else + interface = "eth0"; + + printf("Using interface %s\n", interface); + + LinkedList dataSetValues = LinkedList_create(); + + LinkedList_add(dataSetValues, MmsValue_newIntegerFromInt32(1234)); + LinkedList_add(dataSetValues, MmsValue_newBinaryTime(false)); + LinkedList_add(dataSetValues, MmsValue_newIntegerFromInt32(5678)); + + CommParameters gooseCommParameters; + + gooseCommParameters.appId = 1001; + gooseCommParameters.dstAddress[0] = 0x01; + gooseCommParameters.dstAddress[1] = 0x0c; + gooseCommParameters.dstAddress[2] = 0xcd; + gooseCommParameters.dstAddress[3] = 0x01; + gooseCommParameters.dstAddress[4] = 0x00; + gooseCommParameters.dstAddress[5] = 0x02; + gooseCommParameters.vlanId = 0; + gooseCommParameters.vlanPriority = 4; + + /* + * Create a new GOOSE publisher instance. As the second parameter the interface + * name can be provided (e.g. "eth0" on a Linux system). If the second parameter + * is NULL the interface name as defined with CONFIG_ETHERNET_INTERFACE_ID in + * stack_config.h is used. + */ + GoosePublisher publisher = GoosePublisher_create(&gooseCommParameters, interface); + + if (publisher) + { + GoosePublisher_setGoCbRef(publisher, "simpleIOGenericIO/LLN0$GO$gcbAnalogValues2"); + GoosePublisher_setConfRev(publisher, 1); + GoosePublisher_setDataSetRef(publisher, "simpleIOGenericIO/LLN0$AnalogValues2"); + GoosePublisher_setTimeAllowedToLive(publisher, 500); + + char* key = "ABCDEF0123456789"; + + 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_addKey(l2Sec, 0x12345678, (uint8_t*)key, 16, MC_SEC_SEC_ALGO_NONE, MC_SEC_SIG_ALGO_AES_GMAC_128); + L2Security_setActiveKey(l2Sec, 1); + + GoosePublisher_setL2Security(publisher, l2Sec); + + int i = 0; + + for (i = 0; i < 4; i++) { + Thread_sleep(1000); + + if (i == 3) { + /* now change dataset to send an invalid GOOSE message */ + LinkedList_add(dataSetValues, MmsValue_newBoolean(true)); + GoosePublisher_publish(publisher, dataSetValues); + } + else { + if (GoosePublisher_publish(publisher, dataSetValues) == -1) { + printf("Error sending message!\n"); + } + } + } + + GoosePublisher_destroy(publisher); + } + else { + printf("Failed to create GOOSE publisher. Reason can be that the Ethernet interface doesn't exist or root permission are required.\n"); + } + + LinkedList_destroyDeep(dataSetValues, (LinkedListValueDeleteFunction) MmsValue_delete); + + return 0; +} diff --git a/examples/goose_subscriber/goose_subscriber_example.c b/examples/goose_subscriber/goose_subscriber_example.c index ff767206..27e0b861 100644 --- a/examples/goose_subscriber/goose_subscriber_example.c +++ b/examples/goose_subscriber/goose_subscriber_example.c @@ -66,20 +66,38 @@ main(int argc, char** argv) 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_addKey(l2Sec, 0x12345678, (uint8_t*)key, 16, MC_SEC_SEC_ALGO_NONE, MC_SEC_SIG_ALGO_AES_GMAC_128); + L2Security_addKey(l2Sec, 1, (uint8_t*)key, 16, MC_SEC_SEC_ALGO_NONE, MC_SEC_SIG_ALGO_AES_GMAC_128); L2Security_setActiveKey(l2Sec, 1); - GooseReceiver_setL2Security(receiver, l2Sec); - + GooseSubscriber_setL2Security(subscriber, l2Sec); GooseSubscriber_setListener(subscriber, gooseListener, NULL); GooseReceiver_addSubscriber(receiver, subscriber); + GooseSubscriber subscriber2 = GooseSubscriber_create("simpleIOGenericIO/LLN0$GO$gcbAnalogValues2", NULL); + + uint8_t dstMac2[6] = {0x01,0x0c,0xcd,0x01,0x00,0x02}; + GooseSubscriber_setDstMac(subscriber2, dstMac2); + GooseSubscriber_setAppId(subscriber2, 1001); + + char* key2 = "ABCDEF0123456789"; + + L2Security l2Sec2 = L2Security_create(); + + //L2Security_addKey(l2Sec, 0x12345678, (uint8_t*)key, 16, MC_SEC_SEC_ALGO_NONE, MC_SEC_SIG_ALGO_HMAC_SHA256_256); + L2Security_addKey(l2Sec2, 1, (uint8_t*)key2, 16, MC_SEC_SEC_ALGO_NONE, MC_SEC_SIG_ALGO_AES_GMAC_128); + L2Security_setActiveKey(l2Sec2, 1); + + GooseSubscriber_setL2Security(subscriber2, l2Sec2); + GooseSubscriber_setListener(subscriber2, gooseListener, NULL); + + GooseReceiver_addSubscriber(receiver, subscriber2); + + GooseReceiver_start(receiver); if (GooseReceiver_isRunning(receiver)) { diff --git a/src/goose/goose_receiver.c b/src/goose/goose_receiver.c index fbefd4e4..2e90b9c9 100644 --- a/src/goose/goose_receiver.c +++ b/src/goose/goose_receiver.c @@ -1,7 +1,7 @@ /* * goose_receiver.c * - * Copyright 2014-2024 Michael Zillgith + * Copyright 2014-2025 Michael Zillgith * * This file is part of libIEC61850. * @@ -43,12 +43,6 @@ #define DEBUG_GOOSE_SUBSCRIBER 0 #endif -#define CONFIG_GOOSE_L2_SECURITY 1 - -#if (CONFIG_GOOSE_L2_SECURITY == 1) -#include "l2_security.h" -#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */ - #ifdef DEBUG_GOOSE_SUBSCRIBER #undef DEBUG_GOOSE_SUBSCRIBER #define DEBUG_GOOSE_SUBSCRIBER 1 @@ -77,10 +71,6 @@ 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 @@ -102,10 +92,6 @@ 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; @@ -169,14 +155,6 @@ 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) { @@ -1140,61 +1118,6 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes) } } - if (secExtLength > 0) - { - /* calculate crc */ - uint16_t crc = L2Security_calculateCRC16(buffer + gooseStart, 8); - - if (secExtCrc == crc) - { - printf("GOOSE_SUBSCRIBER: CRC check - OK\n"); - } - else - { - printf("GOOSE_SUBSCRIBER: CRC check - FAILED (expected: %04x actual: %04x)\n", secExtCrc, crc); - } - - /* 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("GOOSE_SUBSCRIBER: Security check - %s\n", secCheckPassed ? "OK" : "FAILED"); - - if (secCheckPassed == false) - { - printf("GOOSE_SUBSCRIBER: security check failed -> ignore message\n"); - return; - } - } - else - { - //if (DEBUG_GOOSE_SUBSCRIBER) - printf("GOOSE_SUBSCRIBER: ERROR - No security layer specified -> cannot check security extension!\n"); - - secCheckPassed = false; - } - } - else - { - if (self->l2Security) - { - printf("GOOSE SUBSCRIBER: ERROR - no security extension\n"); - return; - } - } - /* check if there is an interested subscriber */ LinkedList element = LinkedList_getNext(self->subscriberList); @@ -1218,6 +1141,60 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes) (!subscriber->dstMacSet || (memcmp(subscriber->dstMac, dstMac,6) == 0))) { subscriberFound = true; + + if (secExtLength > 0) + { + /* calculate crc */ + uint16_t crc = L2Security_calculateCRC16(buffer + gooseStart, 8); + + if (secExtCrc == crc) + { + printf("GOOSE_SUBSCRIBER: CRC check - OK\n"); + } + else + { + printf("GOOSE_SUBSCRIBER: CRC check - FAILED (expected: %04x actual: %04x)\n", secExtCrc, crc); + } + + /* 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 (subscriber->l2Security) + { + secCheckPassed = L2Security_checkSecurityExtension(subscriber->l2Security, buffer, gooseStart + 2, length, secExtLength); + + printf("GOOSE_SUBSCRIBER: Security check - %s\n", secCheckPassed ? "OK" : "FAILED"); + + if (secCheckPassed == false) + { + printf("GOOSE_SUBSCRIBER: security check failed -> ignore message\n"); + return; + } + } + else + { + //if (DEBUG_GOOSE_SUBSCRIBER) + printf("GOOSE_SUBSCRIBER: ERROR - No security layer specified -> cannot check security extension!\n"); + + secCheckPassed = false; + } + } + else + { + if (subscriber->l2Security) + { + printf("GOOSE SUBSCRIBER: ERROR - no security extension\n"); + return; + } + } + break; } diff --git a/src/goose/goose_receiver_internal.h b/src/goose/goose_receiver_internal.h index a429c675..8c2262ba 100644 --- a/src/goose/goose_receiver_internal.h +++ b/src/goose/goose_receiver_internal.h @@ -1,7 +1,7 @@ /* * goose_receiver_internal.h * - * Copyright 2014 Michael Zillgith + * Copyright 2014-2025 Michael Zillgith * * This file is part of libIEC61850. * @@ -24,7 +24,6 @@ #ifndef GOOSE_RECEIVER_INTERNAL_H_ #define GOOSE_RECEIVER_INTERNAL_H_ - #define ETH_BUFFER_LENGTH 1518 #define ETH_P_GOOSE 0x88b8 @@ -33,8 +32,14 @@ #define DEBUG_GOOSE_SUBSCRIBER 0 #endif +#define CONFIG_GOOSE_L2_SECURITY 1 + +#if (CONFIG_GOOSE_L2_SECURITY == 1) +#include "l2_security.h" +#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */ -struct sGooseSubscriber { +struct sGooseSubscriber +{ char goCBRef[130]; char datSet[130]; char goId[130]; @@ -65,8 +70,10 @@ struct sGooseSubscriber { GooseListener listener; void* listenerParameter; -}; - +#if (CONFIG_GOOSE_L2_SECURITY == 1) + L2Security l2Security; +#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */ +}; #endif /* GOOSE_RECEIVER_INTERNAL_H_ */ diff --git a/src/goose/goose_subscriber.c b/src/goose/goose_subscriber.c index 019c2b7e..6e15658f 100644 --- a/src/goose/goose_subscriber.c +++ b/src/goose/goose_subscriber.c @@ -59,6 +59,10 @@ GooseSubscriber_create(char* goCbRef, MmsValue* dataSetValues) self->isObserver = false; self->vlanSet = false; self->parseError = GOOSE_PARSE_ERROR_NO_ERROR; + +#if (CONFIG_GOOSE_L2_SECURITY == 1) + self->l2Security = NULL; +#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */ } return self; @@ -95,6 +99,14 @@ GooseSubscriber_setAppId(GooseSubscriber self, uint16_t appId) self->appId = (int32_t) appId; } +#if (CONFIG_GOOSE_L2_SECURITY == 1) +void +GooseSubscriber_setL2Security(GooseSubscriber self, L2Security l2Security) +{ + self->l2Security = l2Security; +} +#endif /* (CONFIG_GOOSE_L2_SECURITY == 1) */ + void GooseSubscriber_destroy(GooseSubscriber self) { diff --git a/src/goose/goose_subscriber.h b/src/goose/goose_subscriber.h index ca986633..5dc631bc 100644 --- a/src/goose/goose_subscriber.h +++ b/src/goose/goose_subscriber.h @@ -25,6 +25,7 @@ #define GOOSE_SUBSCRIBER_H_ #include "libiec61850_common_api.h" +#include "l2_security.h" #ifdef __cplusplus extern "C" { @@ -129,6 +130,15 @@ GooseSubscriber_setDstMac(GooseSubscriber self, uint8_t dstMac[6]); LIB61850_API void GooseSubscriber_setAppId(GooseSubscriber self, uint16_t appId); +/** + * \brief Optionally set L2Security instance when L2 authentication or encryption should be used + * + * \param self GooseSubscriber instance to operate on. + * \param l2Security the L2Security instance to use with this subscriber + */ +LIB61850_API void +GooseSubscriber_setL2Security(GooseSubscriber self, L2Security l2Security); + /** * \brief Check if subscriber state is valid * @@ -312,7 +322,6 @@ GooseSubscriber_setObserver(GooseSubscriber self); } #endif - /**@}*/ #endif /* GOOSE_SUBSCRIBER_H_ */