From 805d73b86f756d36070d97bf4accd5c560e388b0 Mon Sep 17 00:00:00 2001 From: Michael Zillgith Date: Sat, 19 Sep 2020 16:39:28 +0200 Subject: [PATCH] - GOOSE: added GOOSE observer feature (GooseSubscriber listening to all GOOSE messages) and GOOSE observer example --- examples/CMakeLists.txt | 1 + examples/goose_observer/CMakeLists.txt | 30 +++++++ examples/goose_observer/Makefile | 17 ++++ examples/goose_observer/goose_observer.c | 99 ++++++++++++++++++++++++ hal/ethernet/linux/ethernet_linux.c | 25 ++---- src/goose/goose_receiver.c | 67 +++++++++++++++- src/goose/goose_receiver_internal.h | 9 ++- src/goose/goose_subscriber.c | 68 ++++++++++++++-- src/goose/goose_subscriber.h | 34 +++++++- 9 files changed, 321 insertions(+), 29 deletions(-) create mode 100644 examples/goose_observer/CMakeLists.txt create mode 100644 examples/goose_observer/Makefile create mode 100644 examples/goose_observer/goose_observer.c diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 0c2cd8c7..03326ad9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -46,6 +46,7 @@ endif(WITH_MBEDTLS) if(${BUILD_SV_GOOSE_EXAMPLES}) add_subdirectory(server_example_goose) + add_subdirectory(goose_observer) add_subdirectory(goose_subscriber) add_subdirectory(goose_publisher) add_subdirectory(sv_subscriber) diff --git a/examples/goose_observer/CMakeLists.txt b/examples/goose_observer/CMakeLists.txt new file mode 100644 index 00000000..dae58f29 --- /dev/null +++ b/examples/goose_observer/CMakeLists.txt @@ -0,0 +1,30 @@ + +set(goose_observer_SRCS + goose_observer.c +) + +IF(WIN32) + +set_source_files_properties(${goose_observer_SRCS} + PROPERTIES LANGUAGE CXX) +add_executable(goose_observer + ${goose_observer_SRCS} +) + +target_link_libraries(goose_observer + iec61850 +) + +ELSE(WIN32) + +add_executable(goose_observer + ${goose_observer_SRCS} +) + +target_link_libraries(goose_observer + iec61850 +) + +ENDIF(WIN32) + + diff --git a/examples/goose_observer/Makefile b/examples/goose_observer/Makefile new file mode 100644 index 00000000..30a8c9cf --- /dev/null +++ b/examples/goose_observer/Makefile @@ -0,0 +1,17 @@ +LIBIEC_HOME=../.. + +PROJECT_BINARY_NAME = goose_observer +PROJECT_SOURCES = goose_observer.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_observer/goose_observer.c b/examples/goose_observer/goose_observer.c new file mode 100644 index 00000000..4568ff58 --- /dev/null +++ b/examples/goose_observer/goose_observer.c @@ -0,0 +1,99 @@ +/* + * goose_observer.c + * + * This is an example for generic GOOSE observer + * + * Has to be started as root in Linux. + */ + +#include "goose_receiver.h" +#include "goose_subscriber.h" +#include "hal_thread.h" + +#include +#include +#include + +static int running = 1; + +void sigint_handler(int signalId) +{ + running = 0; +} + +void +gooseListener(GooseSubscriber subscriber, void* parameter) +{ + printf("GOOSE event:\n"); + printf(" vlanTag: %s\n", GooseSubscriber_isVlanSet(subscriber) ? "found" : "NOT found"); + if (GooseSubscriber_isVlanSet(subscriber)) + { + printf(" vlanId: %u\n", GooseSubscriber_getVlanId(subscriber)); + printf(" vlanPrio: %u\n", GooseSubscriber_getVlanPrio(subscriber)); + } + printf(" appId: %d\n", GooseSubscriber_getAppId(subscriber)); + uint8_t macBuf[6]; + GooseSubscriber_getSrcMac(subscriber,macBuf); + printf(" srcMac: %02X:%02X:%02X:%02X:%02X:%02X\n", macBuf[0],macBuf[1],macBuf[2],macBuf[3],macBuf[4],macBuf[5]); + GooseSubscriber_getDstMac(subscriber,macBuf); + printf(" dstMac: %02X:%02X:%02X:%02X:%02X:%02X\n", macBuf[0],macBuf[1],macBuf[2],macBuf[3],macBuf[4],macBuf[5]); + printf(" goId: %s\n", GooseSubscriber_getGoId(subscriber)); + printf(" goCbRef: %s\n", GooseSubscriber_getGoCbRef(subscriber)); + printf(" dataSet: %s\n", GooseSubscriber_getDataSet(subscriber)); + printf(" confRev: %u\n", GooseSubscriber_getConfRev(subscriber)); + printf(" ndsCom: %s\n", GooseSubscriber_needsCommission(subscriber) ? "true" : "false"); + printf(" simul: %s\n", GooseSubscriber_isTest(subscriber) ? "true" : "false"); + printf(" stNum: %u sqNum: %u\n", GooseSubscriber_getStNum(subscriber), + GooseSubscriber_getSqNum(subscriber)); + printf(" timeToLive: %u\n", GooseSubscriber_getTimeAllowedToLive(subscriber)); + + uint64_t timestamp = GooseSubscriber_getTimestamp(subscriber); + + printf(" timestamp: %u.%u\n", (uint32_t) (timestamp / 1000), (uint32_t) (timestamp % 1000)); + + MmsValue* values = GooseSubscriber_getDataSetValues(subscriber); + + char buffer[1024]; + + MmsValue_printToBuffer(values, buffer, 1024); + + printf("%s\n", buffer); +} + +int +main(int argc, char** argv) +{ + GooseReceiver receiver = GooseReceiver_create(); + + if (argc > 1) { + printf("Set interface id: %s\n", argv[1]); + GooseReceiver_setInterfaceId(receiver, argv[1]); + } + else { + printf("Using interface eth0\n"); + GooseReceiver_setInterfaceId(receiver, "eth0"); + } + + GooseSubscriber subscriber = GooseSubscriber_create("", NULL); + GooseSubscriber_setObserver(subscriber); + GooseSubscriber_setListener(subscriber, gooseListener, NULL); + + GooseReceiver_addSubscriber(receiver, subscriber); + + GooseReceiver_start(receiver); + + if (GooseReceiver_isRunning(receiver)) { + signal(SIGINT, sigint_handler); + + while (running) { + Thread_sleep(100); + } + } + else { + printf("Failed to start GOOSE subscriber. Reason can be that the Ethernet interface doesn't exist or root permission are required.\n"); + } + + GooseReceiver_stop(receiver); + + GooseReceiver_destroy(receiver); +} diff --git a/hal/ethernet/linux/ethernet_linux.c b/hal/ethernet/linux/ethernet_linux.c index 5b2a406d..79781efe 100644 --- a/hal/ethernet/linux/ethernet_linux.c +++ b/hal/ethernet/linux/ethernet_linux.c @@ -226,26 +226,15 @@ Ethernet_setProtocolFilter(EthernetSocket ethSocket, uint16_t etherType) { if (etherType == 0x88b8) { - struct sock_fprog fprog; + /* enable linux kernel filtering for GOOSE */ - /* sudo tcpdump -i lo ether proto 0x88B8 and ether dst 01:0c:cd:01:00:01 -dd - struct sock_filter filter[] = { - { 0x28, 0, 0, 0x0000000c }, - { 0x15, 0, 5, 0x000088b8 }, - { 0x20, 0, 0, 0x00000002 }, - { 0x15, 0, 3, 0xcd010001 }, - { 0x28, 0, 0, 0x00000000 }, - { 0x15, 0, 1, 0x0000010c }, - { 0x6, 0, 0, 0x00040000 }, - { 0x6, 0, 0, 0x00000000 } - }; - */ - /* sudo tcpdump -i lo ether proto 0x88B8 -dd # no DST MAC filter */ + struct sock_fprog fprog; struct sock_filter filter[] = { - { 0x28, 0, 0, 0x0000000c }, - { 0x15, 0, 1, 0x000088b8 }, - { 0x6, 0, 0, 0x00040000 }, - { 0x6, 0, 0, 0x00000000 } + /* sudo tcpdump -i enx88d7f6377223 '(ether proto 0x88b8)' -dd */ + { 0x28, 0, 0, 0x0000000c }, + { 0x15, 0, 1, 0x000088b8 }, + { 0x6, 0, 0, 0x00040000 }, + { 0x6, 0, 0, 0x00000000 } }; fprog.len = sizeof(filter) / sizeof(*filter); diff --git a/src/goose/goose_receiver.c b/src/goose/goose_receiver.c index e6ed1c73..e3d069f3 100644 --- a/src/goose/goose_receiver.c +++ b/src/goose/goose_receiver.c @@ -549,7 +549,21 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength) while (element != NULL) { GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element); - if (subscriber->goCBRefLen == elementLength) { + if (subscriber->isObserver) + { + if (elementLength > 129) { + if (DEBUG_GOOSE_SUBSCRIBER) + printf("GOOSE_SUBSCRIBER: gocbRef too long!\n"); + } + else { + memcpy(subscriber->goCBRef, buffer + bufPos, elementLength); + subscriber->goCBRef[elementLength] = 0; + } + + matchingSubscriber = subscriber; + break; + } + else if (subscriber->goCBRefLen == elementLength) { if (memcmp(subscriber->goCBRef, buffer + bufPos, elementLength) == 0) { if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: gocbRef is matching!\n"); @@ -579,11 +593,41 @@ parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength) case 0x82: if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found dataSet\n"); + { + if (matchingSubscriber) { + if (matchingSubscriber->isObserver) + { + if (elementLength > 64) { + if (DEBUG_GOOSE_SUBSCRIBER) + printf("GOOSE_SUBSCRIBER: datSet too long!\n"); + } + else { + memcpy(matchingSubscriber->datSet, buffer + bufPos, elementLength); + matchingSubscriber->datSet[elementLength] = 0; + } + } + } + } break; case 0x83: if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found goId\n"); + { + if (matchingSubscriber) { + if (matchingSubscriber->isObserver) + { + if (elementLength > 64) { + if (DEBUG_GOOSE_SUBSCRIBER) + printf("GOOSE_SUBSCRIBER: goId too long!\n"); + } + else { + memcpy(matchingSubscriber->goId, buffer + bufPos, elementLength); + matchingSubscriber->goId[elementLength] = 0; + } + } + } + } break; case 0x84: @@ -708,8 +752,14 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes) bufPos = 12; int headerLength = 14; + uint8_t priority = 0; + uint16_t vlanId = 0; + bool vlanSet = false; /* check for VLAN tag */ if ((buffer[bufPos] == 0x81) && (buffer[bufPos + 1] == 0x00)) { + priority = buffer[bufPos + 2] & 0xF8 >> 5; + vlanId = ((buffer[bufPos + 2] & 0x07) << 8) + buffer[bufPos + 3]; + vlanSet = true; bufPos += 4; /* skip VLAN tag */ headerLength += 4; } @@ -719,6 +769,9 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes) return; if (buffer[bufPos++] != 0xb8) return; + + uint8_t srcMac[6]; + memcpy(srcMac,&buffer[6],6); uint8_t dstMac[6]; memcpy(dstMac,buffer,6); @@ -758,6 +811,18 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes) while (element != NULL) { GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element); + + if (subscriber->isObserver) + { + subscriber->appId = appId; + memcpy(subscriber->srcMac, srcMac,6); + memcpy(subscriber->dstMac, dstMac, 6); + subscriberFound = true; + subscriber->vlanSet = vlanSet; + subscriber->vlanId = vlanId; + subscriber->vlanPrio = priority; + break; + } if (((subscriber->appId == -1) || (subscriber->appId == appId)) && (!subscriber->dstMacSet || (memcmp(subscriber->dstMac, dstMac,6) == 0))) { diff --git a/src/goose/goose_receiver_internal.h b/src/goose/goose_receiver_internal.h index 33c93720..a063a305 100644 --- a/src/goose/goose_receiver_internal.h +++ b/src/goose/goose_receiver_internal.h @@ -35,7 +35,9 @@ struct sGooseSubscriber { - char* goCBRef; + char goCBRef[130]; + char datSet[65]; + char goId[65]; int goCBRefLen; uint32_t timeAllowedToLive; uint32_t stNum; @@ -48,12 +50,17 @@ struct sGooseSubscriber { uint64_t invalidityTime; bool stateValid; + uint8_t srcMac[6]; /* source mac address */ uint8_t dstMac[6]; /* destination mac address */ int32_t appId; /* APPID or -1 if APPID should be ignored */ MmsValue* dataSetValues; bool dataSetValuesSelfAllocated; bool dstMacSet; + bool isObserver; + bool vlanSet; + uint16_t vlanId; + uint8_t vlanPrio; GooseListener listener; void* listenerParameter; diff --git a/src/goose/goose_subscriber.c b/src/goose/goose_subscriber.c index 9b50a500..f6a5a8ab 100644 --- a/src/goose/goose_subscriber.c +++ b/src/goose/goose_subscriber.c @@ -40,7 +40,9 @@ GooseSubscriber_create(char* goCbRef, MmsValue* dataSetValues) { GooseSubscriber self = (GooseSubscriber) GLOBAL_CALLOC(1, sizeof(struct sGooseSubscriber)); - self->goCBRef = StringUtils_copyString(goCbRef); + strncpy(self->goCBRef, goCbRef, 129); + self->goCBRef[129] = 0; + self->goCBRefLen = strlen(goCbRef); self->timestamp = MmsValue_newUtcTime(0); self->dataSetValues = dataSetValues; @@ -51,6 +53,8 @@ GooseSubscriber_create(char* goCbRef, MmsValue* dataSetValues) memset(self->dstMac, 0xFF, 6); self->dstMacSet = false; self->appId = -1; + self->isObserver = false; + self->vlanSet = false; return self; } @@ -83,8 +87,6 @@ GooseSubscriber_setAppId(GooseSubscriber self, uint16_t appId) void GooseSubscriber_destroy(GooseSubscriber self) { - GLOBAL_FREEMEM(self->goCBRef); - MmsValue_delete(self->timestamp); if (self->dataSetValuesSelfAllocated) @@ -100,6 +102,42 @@ GooseSubscriber_setListener(GooseSubscriber self, GooseListener listener, void* self->listenerParameter = parameter; } +int32_t +GooseSubscriber_getAppId(GooseSubscriber self) +{ + return self->appId; +} + +char * +GooseSubscriber_getGoId(GooseSubscriber self) +{ + return self->goId; +} + +char * +GooseSubscriber_getGoCbRef(GooseSubscriber self) +{ + return self->goCBRef; +} + +char * +GooseSubscriber_getDataSet(GooseSubscriber self) +{ + return self->datSet; +} + +void +GooseSubscriber_getSrcMac(GooseSubscriber self, uint8_t *buffer) +{ + memcpy(buffer, self->srcMac,6); +} + +void +GooseSubscriber_getDstMac(GooseSubscriber self, uint8_t *buffer) +{ + memcpy(buffer, self->dstMac,6); +} + uint32_t GooseSubscriber_getStNum(GooseSubscriber self) { @@ -148,10 +186,26 @@ GooseSubscriber_getDataSetValues(GooseSubscriber self) return self->dataSetValues; } +bool +GooseSubscriber_isVlanSet(GooseSubscriber self) +{ + return self->vlanSet; +} +uint16_t +GooseSubscriber_getVlanId(GooseSubscriber self) +{ + return self->vlanId; +} +uint8_t +GooseSubscriber_getVlanPrio(GooseSubscriber self) +{ + return self->vlanPrio; +} - - - - +void +GooseSubscriber_setObserver(GooseSubscriber self) +{ + self->isObserver = true; +} diff --git a/src/goose/goose_subscriber.h b/src/goose/goose_subscriber.h index 9b0fb86e..647c8c92 100644 --- a/src/goose/goose_subscriber.h +++ b/src/goose/goose_subscriber.h @@ -70,9 +70,14 @@ typedef void (*GooseListener)(GooseSubscriber subscriber, void* parameter); LIB61850_API GooseSubscriber GooseSubscriber_create(char* goCbRef, MmsValue* dataSetValues); -/* char* +LIB61850_API char* +GooseSubscriber_getGoId(GooseSubscriber self); + +LIB61850_API char* GooseSubscriber_getGoCbRef(GooseSubscriber self); -*/ + +LIB61850_API char* +GooseSubscriber_getDataSet(GooseSubscriber self); /** * \brief set the destination mac address used by the subscriber to filter relevant messages. @@ -119,6 +124,14 @@ GooseSubscriber_destroy(GooseSubscriber self); LIB61850_API void GooseSubscriber_setListener(GooseSubscriber self, GooseListener listener, void* parameter); +LIB61850_API int32_t +GooseSubscriber_getAppId(GooseSubscriber self); + +LIB61850_API void +GooseSubscriber_getSrcMac(GooseSubscriber self, uint8_t *buffer); + +LIB61850_API void +GooseSubscriber_getDstMac(GooseSubscriber self, uint8_t *buffer); /** * \brief return the state number (stNum) of the last received GOOSE message. * @@ -214,6 +227,23 @@ GooseSubscriber_getTimestamp(GooseSubscriber self); LIB61850_API MmsValue* GooseSubscriber_getDataSetValues(GooseSubscriber self); +LIB61850_API bool +GooseSubscriber_isVlanSet(GooseSubscriber self); + +LIB61850_API uint16_t +GooseSubscriber_getVlanId(GooseSubscriber self); + +LIB61850_API uint8_t +GooseSubscriber_getVlanPrio(GooseSubscriber self); + +/** + * \brief Configure the Subscriber to listen to any received GOOSE message + * + * NOTE: When the observer flag is set the subscriber also has access to the + * goCbRef, goId, and datSet values of the received GOOSE message + */ +LIB61850_API void +GooseSubscriber_setObserver(GooseSubscriber self); #ifdef __cplusplus } #endif