- add GOOSE subscriber/Ethernet layer support to listen to specific multicast messages

hal/ethernet/bsd/ethernet_bsd.c

@@ -357,6 +357,18 @@ Ethernet_createSocket(const char* interfaceId, uint8_t* destAddress)
     return self;
 }
 
+void
+Ethernet_setMode(EthernetSocket ethSocket, EthernetSocketMode mode)
+{
+    /* not implemented */
+}
+
+void
+Ethernet_addMulticastAddress(EthernetSocket ethSocket, uint8_t* multicastAddress)
+{
+    /* not implemented */
+}
+
 void
 Ethernet_setProtocolFilter(EthernetSocket self, uint16_t etherType)
 {

hal/ethernet/linux/ethernet_linux.c

@@ -1,7 +1,7 @@
 /*
  *  ethernet_linux.c
  *
- *  Copyright 2013 Michael Zillgith
+ *  Copyright 2013-2022 Michael Zillgith
  *
  *  This file is part of libIEC61850.
  *
@@ -136,22 +136,6 @@ getInterfaceIndex(int sock, const char* deviceName)
 
     int interfaceIndex = ifr.ifr_ifindex;
 
-    if (ioctl (sock, SIOCGIFFLAGS, &ifr) == -1)
-    {
-        if (DEBUG_SOCKET)
-            printf("ETHERNET_LINUX: Problem getting device flags");
-        return -1;
-    }
-
-    /* replace IFF_ALLMULTI by IFF_PROMISC to also receive unicast messages for other nodes */
-    ifr.ifr_flags |= IFF_ALLMULTI;
-    if (ioctl (sock, SIOCSIFFLAGS, &ifr) == -1)
-    {
-        if (DEBUG_SOCKET)
-            printf("ETHERNET_LINUX: Setting device to promiscuous mode failed");
-        return -1;
-    }
-
     return interfaceIndex;
 }
 
@@ -179,7 +163,6 @@ Ethernet_getInterfaceMACAddress(const char* interfaceId, uint8_t* addr)
     }
 }
 
-
 EthernetSocket
 Ethernet_createSocket(const char* interfaceId, uint8_t* destAddress)
 {
@@ -208,7 +191,7 @@ Ethernet_createSocket(const char* interfaceId, uint8_t* destAddress)
         ethernetSocket->socketAddress.sll_ifindex = ifcIdx;
 
         ethernetSocket->socketAddress.sll_hatype =  ARPHRD_ETHER;
-        ethernetSocket->socketAddress.sll_pkttype = PACKET_OTHERHOST;
+        ethernetSocket->socketAddress.sll_pkttype = PACKET_HOST | PACKET_MULTICAST;
 
         ethernetSocket->socketAddress.sll_halen = ETH_ALEN;
 
@@ -223,6 +206,82 @@ Ethernet_createSocket(const char* interfaceId, uint8_t* destAddress)
     return ethernetSocket;
 }
 
+void
+Ethernet_setMode(EthernetSocket ethSocket, EthernetSocketMode mode)
+{
+    if (ethSocket) {
+
+        if (mode == ETHERNET_SOCKET_MODE_PROMISC) {
+
+            struct ifreq ifr;
+
+            if (ioctl (ethSocket->rawSocket, SIOCGIFFLAGS, &ifr) == -1)
+            {
+                if (DEBUG_SOCKET)
+                    printf("ETHERNET_LINUX: Problem getting device flags");
+                return;
+            }
+
+
+            ifr.ifr_flags |= IFF_PROMISC;
+            if (ioctl (ethSocket->rawSocket, SIOCSIFFLAGS, &ifr) == -1)
+            {
+                if (DEBUG_SOCKET)
+                    printf("ETHERNET_LINUX: Setting device to promiscuous mode failed");
+                return;
+            }
+        }
+        else if (mode == ETHERNET_SOCKET_MODE_ALL_MULTICAST) {
+            struct ifreq ifr;
+
+            if (ioctl (ethSocket->rawSocket, SIOCGIFFLAGS, &ifr) == -1)
+            {
+                if (DEBUG_SOCKET)
+                    printf("ETHERNET_LINUX: Problem getting device flags");
+                return;
+            }
+
+
+            ifr.ifr_flags |= IFF_ALLMULTI;
+            if (ioctl (ethSocket->rawSocket, SIOCSIFFLAGS, &ifr) == -1)
+            {
+                if (DEBUG_SOCKET)
+                    printf("ETHERNET_LINUX: Setting device to promiscuous mode failed");
+                return;
+            }
+        }
+        else if (mode == ETHERNET_SOCKET_MODE_HOST_ONLY) {
+            ethSocket->socketAddress.sll_pkttype = PACKET_HOST;
+        }
+        else if (mode == ETHERNET_SOCKET_MODE_MULTICAST) {
+            ethSocket->socketAddress.sll_pkttype = PACKET_HOST | PACKET_MULTICAST;
+        }
+    }
+}
+
+void
+Ethernet_addMulticastAddress(EthernetSocket ethSocket, uint8_t* multicastAddress)
+{
+    struct packet_mreq mreq;
+    mreq.mr_ifindex = ethSocket->socketAddress.sll_ifindex;
+    mreq.mr_alen = ETH_ALEN;
+    mreq.mr_type = PACKET_MR_MULTICAST;
+    mreq.mr_address[0] = multicastAddress[0];
+    mreq.mr_address[1] = multicastAddress[1];
+    mreq.mr_address[2] = multicastAddress[2];
+    mreq.mr_address[3] = multicastAddress[3];
+    mreq.mr_address[4] = multicastAddress[4];
+    mreq.mr_address[5] = multicastAddress[5];
+
+    int res = setsockopt(ethSocket->rawSocket, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
+
+
+    if (res != 0) {
+        if (DEBUG_SOCKET)
+            printf("ETHERNET_LINUX: Setting multicast address failed");
+    }
+}
+
 void
 Ethernet_setProtocolFilter(EthernetSocket ethSocket, uint16_t etherType)
 {

hal/ethernet/win32/ethernet_win32.c

@@ -378,6 +378,18 @@ Ethernet_sendPacket(EthernetSocket ethSocket, uint8_t* buffer, int packetSize)
         printf("Error sending the packet: %s\n", pcap_geterr(ethSocket->rawSocket));
 }
 
+void
+Ethernet_setMode(EthernetSocket ethSocket, EthernetSocketMode mode)
+{
+    /* not implemented */
+}
+
+void
+Ethernet_addMulticastAddress(EthernetSocket ethSocket, uint8_t* multicastAddress)
+{
+    /* not implemented */
+}
+
 void
 Ethernet_setProtocolFilter(EthernetSocket ethSocket, uint16_t etherType)
 {
@@ -478,6 +490,16 @@ Ethernet_sendPacket(EthernetSocket ethSocket, uint8_t* buffer, int packetSize)
 {
 }
 
+void
+Ethernet_setMode(EthernetSocket ethSocket, EthernetSocketMode mode)
+{
+}
+
+void
+Ethernet_addMulticastAddress(EthernetSocket ethSocket, uint8_t* multicastAddress)
+{
+}
+
 void
 Ethernet_setProtocolFilter(EthernetSocket ethSocket, uint16_t etherType)
 {

hal/inc/hal_ethernet.h

@@ -36,6 +36,13 @@ typedef struct sEthernetSocket* EthernetSocket;
 /** Opaque reference for a set of Ethernet socket handles */
 typedef struct sEthernetHandleSet* EthernetHandleSet;
 
+typedef enum {
+    ETHERNET_SOCKET_MODE_PROMISC, /**<< receive all Ethernet messages */
+    ETHERNET_SOCKET_MODE_ALL_MULTICAST, /**<< receive all multicast messages */
+    ETHERNET_SOCKET_MODE_MULTICAST, /**<< receive only specific multicast messages */
+    ETHERNET_SOCKET_MODE_HOST_ONLY /**<< receive only messages for the host */
+} EthernetSocketMode;
+
 /**
  * \brief Create a new connection handle set (EthernetHandleSet)
  *
@@ -117,6 +124,29 @@ Ethernet_destroySocket(EthernetSocket ethSocket);
 PAL_API void
 Ethernet_sendPacket(EthernetSocket ethSocket, uint8_t* buffer, int packetSize);
 
+/*
+ * \brief set the receive mode of the Ethernet socket
+ *
+ * NOTE: When not implemented the the implementation has to be able to receive
+ * all messages required by GOOSE and/or SV (usually multicast addresses).
+ *
+ * \param ethSocket the ethernet socket handle
+ * \param mode the mode of the socket
+ */
+PAL_API void
+Ethernet_setMode(EthernetSocket ethSocket, EthernetSocketMode mode);
+
+/**
+ * \brief Add a multicast address to be received by the Ethernet socket
+ *
+ * Used when mode is ETHERNET_SOCKET_MODE_MULTICAST
+ *
+ * \param ethSocket the ethernet socket handle
+ * \param multicastAddress the multicast Ethernet address (this has to be a byte buffer of at least 6 byte)
+ */
+PAL_API void
+Ethernet_addMulticastAddress(EthernetSocket ethSocket, uint8_t* multicastAddress);
+
 /*
  * \brief set a protocol filter for the specified etherType
  *

src/goose/goose_receiver.c

@@ -1060,6 +1060,26 @@ GooseReceiver_startThreadless(GooseReceiver self)
 
     if (self->ethSocket != NULL) {
         Ethernet_setProtocolFilter(self->ethSocket, ETH_P_GOOSE);
+
+        /* set multicast addresses for subscribers */
+        Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_MULTICAST);
+
+        LinkedList element = LinkedList_getNext(self->subscriberList);
+
+        while (element != NULL) {
+            GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
+
+            if (subscriber->dstMacSet == false) {
+                /* no destination MAC address defined -> we have to switch to all multicast mode */
+                Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_ALL_MULTICAST);
+            }
+            else {
+                Ethernet_addMulticastAddress(self->ethSocket, subscriber->dstMac);
+            }
+
+            element = LinkedList_getNext(element);
+        }
+
         self->running = true;
     }
     else