- GOOSE subscriber: added optional destination address check for GOOS…

…E messages
mz-automation · Jun 26, 2020 · 7e1dd82 · 7e1dd82
1 parent e24357d
commit 7e1dd82
Show file tree

Hide file tree

Showing 5 changed files with 33 additions and 3 deletions.
diff --git a/examples/goose_subscriber/goose_subscriber_example.c b/examples/goose_subscriber/goose_subscriber_example.c
@@ -58,6 +58,8 @@ main(int argc, char** argv)
 
     GooseSubscriber subscriber = GooseSubscriber_create("simpleIOGenericIO/LLN0$GO$gcbAnalogValues", NULL);
 
+    uint8_t dstMac[6] = {0x01,0x0c,0xcd,0x01,0x00,0x01};
+    GooseSubscriber_setDstMac(subscriber, dstMac);
     GooseSubscriber_setAppId(subscriber, 1000);
 
     GooseSubscriber_setListener(subscriber, gooseListener, NULL);

diff --git a/src/goose/goose_receiver.c b/src/goose/goose_receiver.c
@@ -720,6 +720,9 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
     if (buffer[bufPos++] != 0xb8)
         return;
 
+    uint8_t dstMac[6];
+    memcpy(dstMac,buffer,6);
+
     uint16_t appId;
 
     appId = buffer[bufPos++] * 0x100;
@@ -743,6 +746,8 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
 
     if (DEBUG_GOOSE_SUBSCRIBER) {
         printf("GOOSE_SUBSCRIBER: GOOSE message:\nGOOSE_SUBSCRIBER: ----------------\n");
+        printf("GOOSE_SUBSCRIBER:   DST-MAC: %02x:%02x:%02x:%02x:%02X:%02X\n",
+               dstMac[0], dstMac[1], dstMac[2], dstMac[3], dstMac[4], dstMac[5]);
         printf("GOOSE_SUBSCRIBER:   APPID: %u\n", appId);
         printf("GOOSE_SUBSCRIBER:   LENGTH: %u\n", length);
         printf("GOOSE_SUBSCRIBER:   APDU length: %i\n", apduLength);
@@ -754,7 +759,8 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
     while (element != NULL) {
         GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
 
-        if (subscriber->appId == appId) {
+        if (((subscriber->appId == -1) || (subscriber->appId == appId)) &&
+                (!subscriber->dstMacSet || (memcmp(subscriber->dstMac, dstMac,6) == 0))) {
             subscriberFound = true;
             break;
         }
@@ -766,7 +772,7 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
         parseGoosePayload(self, buffer + bufPos, apduLength);
     else {
         if (DEBUG_GOOSE_SUBSCRIBER)
-            printf("GOOSE_SUBSCRIBER: GOOSE message ignored due to unknown APPID value\n");
+            printf("GOOSE_SUBSCRIBER: GOOSE message ignored due to unknown DST-MAC or APPID value\n");
     }
 }
 

diff --git a/src/goose/goose_receiver_internal.h b/src/goose/goose_receiver_internal.h
@@ -48,10 +48,12 @@ struct sGooseSubscriber {
     uint64_t invalidityTime;
     bool stateValid;
 
+    uint8_t dstMac[6]; /* destination mac address */
     int32_t appId; /* APPID or -1 if APPID should be ignored */
 
     MmsValue* dataSetValues;
     bool dataSetValuesSelfAllocated;
+    bool dstMacSet;
 
     GooseListener listener;
     void* listenerParameter;

diff --git a/src/goose/goose_subscriber.c b/src/goose/goose_subscriber.c
@@ -48,6 +48,8 @@ GooseSubscriber_create(char* goCbRef, MmsValue* dataSetValues)
     if (dataSetValues != NULL)
         self->dataSetValuesSelfAllocated = false;
 
+    memset(self->dstMac, 0xFF, 6);
+    self->dstMacSet = false;
     self->appId = -1;
 
     return self;
@@ -65,6 +67,13 @@ GooseSubscriber_isValid(GooseSubscriber self)
     return true;
 }
 
+void
+GooseSubscriber_setDstMac(GooseSubscriber self, uint8_t dstMac[6])
+{
+  memcpy(self->dstMac, dstMac,6);
+  self->dstMacSet = true;
+}
+
 void
 GooseSubscriber_setAppId(GooseSubscriber self, uint16_t appId)
 {

diff --git a/src/goose/goose_subscriber.h b/src/goose/goose_subscriber.h
@@ -74,13 +74,24 @@ GooseSubscriber_create(char* goCbRef, MmsValue* dataSetValues);
 GooseSubscriber_getGoCbRef(GooseSubscriber self);
 */
 
+/**
+ * \brief set the destination mac address used by the subscriber to filter relevant messages.
+ *
+ * If dstMac is set the subscriber will ignore all messages with other dstMac values.
+ *
+ * \param self GooseSubscriber instance to operate on.
+ * \param dstMac the destination mac address
+ */
+LIB61850_API void
+GooseSubscriber_setDstMac(GooseSubscriber self, uint8_t dstMac[6]);
+
 /**
  * \brief set the APPID used by the subscriber to filter relevant messages.
  *
  * If APPID is set the subscriber will ignore all messages with other APPID values.
  *
  * \param self GooseSubscriber instance to operate on.
- * \param the APPID value the subscriber should use to filter messages
+ * \param appId the APPID value the subscriber should use to filter messages
  */
 LIB61850_API void
 GooseSubscriber_setAppId(GooseSubscriber self, uint16_t appId);