diff --git a/examples/goose_subscriber/goose_subscriber_example.c b/examples/goose_subscriber/goose_subscriber_example.c index 2b16ca75..c67e0eb8 100644 --- 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 index cbd10383..661e17cd 100644 --- 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 index e595da00..33c93720 100644 --- 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 index 62375e0d..9b50a500 100644 --- 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 index 9dfb4c2a..9b0fb86e 100644 --- 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);