- GOOSE receiver: improvements in GOOSE receiver

hal/ethernet/linux/ethernet_linux.c

@@ -24,6 +24,7 @@
 #include <sys/socket.h>
 #include <sys/ioctl.h>
 #include <poll.h>
+#include <linux/filter.h>
 #include <linux/if_packet.h>
 #include <linux/if_ether.h>
 #include <linux/if_arp.h>
@@ -193,7 +194,7 @@ Ethernet_createSocket(const char* interfaceId, uint8_t* destAddress)
         }
 
         ethernetSocket->socketAddress.sll_family = PF_PACKET;
-        ethernetSocket->socketAddress.sll_protocol = htons(ETH_P_IP);
+        ethernetSocket->socketAddress.sll_protocol = htons(ETH_P_ALL);
 
         int ifcIdx =  getInterfaceIndex(ethernetSocket->rawSocket, interfaceId);
 
@@ -223,7 +224,41 @@ Ethernet_createSocket(const char* interfaceId, uint8_t* destAddress)
 void
 Ethernet_setProtocolFilter(EthernetSocket ethSocket, uint16_t etherType)
 {
+    if (etherType == 0x88b8)
+    {
+        struct sock_fprog fprog;
+
+        /*  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_filter filter[] = {
+                { 0x28, 0, 0, 0x0000000c },
+                { 0x15, 0, 1, 0x000088b8 },
+                { 0x6, 0, 0, 0x00040000 },
+                { 0x6, 0, 0, 0x00000000 }
+        };
+
+        fprog.len = sizeof(filter) / sizeof(*filter);
+        fprog.filter = filter;
+
+        if (setsockopt(ethSocket->rawSocket, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog)) == -1)
+            if (DEBUG_SOCKET)
+                printf("ETHERNET_LINUX: Applying filter failed");
+    }
+    else
+    {
         ethSocket->socketAddress.sll_protocol = htons(etherType);
+    }
 }
 
 

src/goose/goose_receiver.c

@@ -778,23 +778,34 @@ parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
 
 #if (CONFIG_MMS_THREADLESS_STACK == 0)
 static void
-gooseReceiverLoop(void* threadParameter)
+gooseReceiverLoop(void *threadParameter)
 {
     GooseReceiver self = (GooseReceiver) threadParameter;
+    EthernetHandleSet handleSet = EthernetHandleSet_new();
+    EthernetHandleSet_addSocket(handleSet, self->ethSocket);
 
     if (self->running) {
 
         while (self->running) {
-
-            if (GooseReceiver_tick(self) == false)
-                Thread_sleep(1);
-
+            switch (EthernetHandleSet_waitReady(handleSet, 100))
+            {
+            case -1:
+                if (DEBUG_GOOSE_SUBSCRIBER)
+                    printf("GOOSE_SUBSCRIBER: EhtnernetHandleSet_waitReady() failure\n");
+                break;
+            case 0:
+                break;
+            default:
+                GooseReceiver_tick(self);
+            }
             if (self->stop)
                 break;
         }
 
         GooseReceiver_stopThreadless(self);
     }
+
+    EthernetHandleSet_destroy(handleSet);
 }
 #endif