- GOOSE: added GOOSE observer feature (GooseSubscriber listening to all GOOSE messages) and GOOSE observer example

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)

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)
+
+

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)

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 <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+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);
+}

hal/ethernet/linux/ethernet_linux.c

@@ -226,22 +226,11 @@ 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_fprog fprog;
-         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_filter filter[] = {
+            /* sudo tcpdump -i enx88d7f6377223 '(ether proto 0x88b8)' -dd */
             { 0x28, 0, 0, 0x0000000c },
             { 0x15, 0, 1, 0x000088b8 },
             { 0x6, 0, 0, 0x00040000 },

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;
     }
@@ -720,6 +770,9 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
     if (buffer[bufPos++] != 0xb8)
         return;
     
+    uint8_t srcMac[6];
+    memcpy(srcMac,&buffer[6],6);
+
     uint8_t dstMac[6];
     memcpy(dstMac,buffer,6);
 
@@ -759,6 +812,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))) {
             subscriberFound = true;

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;

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;
+}

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