Merge pull request #36 from stv0g/ethernet-handleset

EthernetHandleSet

CMakeLists.txt

@@ -75,6 +75,7 @@ set(API_HEADERS
     src/hal/inc/hal_time.h 
     src/hal/inc/hal_thread.h
     src/hal/inc/hal_filesystem.h 
+    src/hal/inc/hal_ethernet.h
     src/hal/inc/platform_endian.h
     src/common/inc/libiec61850_common_api.h
     src/common/inc/libiec61850_platform_includes.h

examples/goose_subscriber/goose_subscriber_example.c

@@ -7,6 +7,7 @@
  */
 
 #include "goose_receiver.h"
+#include "goose_subscriber.h"
 #include "hal_thread.h"
 
 #include <stdlib.h>

src/goose/goose_receiver.c

@@ -811,3 +811,10 @@ GooseReceiver_tick(GooseReceiver self)
     else
         return false;
 }
+
+void
+GooseReceiver_addHandleSet(GooseReceiver self, EthernetHandleSet handles)
+{
+    return EthernetHandleSet_addSocket(handles, self->ethSocket);
+}
+

src/goose/goose_receiver.h

@@ -24,18 +24,19 @@
 #ifndef GOOSE_RECEIVER_H_
 #define GOOSE_RECEIVER_H_
 
-#include <goose_subscriber.h>
-
 #ifdef __cplusplus
 extern "C" {
 #endif
 
+#include <stdbool.h>
+
 
 /**
  * \addtogroup goose_api_group
  */
 /**@{*/
 
+typedef struct sGooseSubscriber* GooseSubscriber;
 
 typedef struct sGooseReceiver* GooseReceiver;
 
@@ -133,6 +134,20 @@ GooseReceiver_stopThreadless(GooseReceiver self);
 bool
 GooseReceiver_tick(GooseReceiver self);
 
+/* Forward declaration */
+typedef struct sEthernetHandleSet* EthernetHandleSet;
+
+/**
+ * \brief Add the receiver to a handleset for multiplexed asynchronous IO.
+ *
+ * Note: This function must only be called after GooseReceiver_startThreadless().
+ *
+ * \param[in] self The SVReceiver instance.
+ * \param[inout] handles The EthernetHandleSet to which the EthernetSocket of this receiver should be added.
+ */
+void
+GooseReceiver_addHandleSet(GooseReceiver self, EthernetHandleSet handles);
+
 /**@}*/
 
 #ifdef __cplusplus

src/hal/ethernet/bsd/ethernet_bsd.c

@@ -22,6 +22,7 @@
  */
 #include <sys/socket.h>
 #include <sys/ioctl.h>
+#include <sys/poll.h>
 #include <arpa/inet.h>
 #include <unistd.h>
 #include <net/if.h>
@@ -46,12 +47,82 @@ struct sEthernetSocket {
     struct bpf_program bpfProgram;  // BPF filter machine code program.
 };
 
-int _Ethernet_activateBpdFilter(EthernetSocket self)
+struct sEthernetHandleSet {
+    struct pollfd *handles;
+    int nhandles;
+};
+
+EthernetHandleSet
+EthernetHandleSet_new(void)
+{
+    EthernetHandleSet result = (EthernetHandleSet) GLOBAL_MALLOC(sizeof(struct sEthernetHandleSet));
+
+    if (result != NULL) {
+        result->handles = NULL;
+        result->nhandles = 0;
+    }
+
+    return result;
+}
+
+void
+EthernetHandleSet_addSocket(EthernetHandleSet self, const EthernetSocket sock)
+{
+    if (self != NULL && sock != NULL) {
+        int i = self->nhandles++;
+        self->handles = realloc(self->handles, self->nhandles * sizeof(struct pollfd));
+            
+        self->handles[i].fd = sock->bpf;
+        self->handles[i].events = POLLIN;
+    }
+}
+
+void
+EthernetHandleSet_removeSocket(EthernetHandleSet self, const EthernetSocket sock)
+{
+    if ((self != NULL) && (sock != NULL)) {
+        unsigned i;
+        for (i = 0; i < self->nhandles; i++) {
+            if (self->handles[i].fd == sock->bpf) {
+                memmove(&self->handles[i], &self->handles[i+1], sizeof(struct pollfd) * (self->nhandles - i - 1));
+                self->nhandles--;
+                return;
+            }
+        }
+    }
+}
+
+int
+EthernetHandleSet_waitReady(EthernetHandleSet self, unsigned int timeoutMs)
+{
+    int result;
+
+    if ((self != NULL) && (self->nhandles >= 0)) {
+        result = poll(self->handles, self->nhandles, timeoutMs);
+    }
+    else {
+       result = -1;
+    }
+
+    return result;
+}
+
+void
+EthernetHandleSet_destroy(EthernetHandleSet self)
+{
+    if (self->nhandles)
+        free(self->handles);
+
+    GLOBAL_FREEMEM(self);
+}
+int
+activateBpdFilter(EthernetSocket self)
 {
     return ioctl(self->bpf, BIOCSETF, &self->bpfProgram);
 }
 
-int _Ethernet_setBpfEthernetAddressFilter(EthernetSocket self, uint8_t *addr)
+static int
+setBpfEthernetAddressFilter(EthernetSocket self, uint8_t *addr)
 {
     if (addr)
     {
@@ -62,18 +133,19 @@ int _Ethernet_setBpfEthernetAddressFilter(EthernetSocket self, uint8_t *addr)
         memcpy((void *)&self->bpfProgram.bf_insns[3].k, &addr[2], 4);
         memcpy((void *)&self->bpfProgram.bf_insns[5].k, &addr, 2);
 
-        return _Ethernet_activateBpdFilter(self);
+        return activateBpdFilter(self);
     }
     else
     {
         // Disable Ethernet address filter.
         self->bpfProgram.bf_insns[0].k = 0;
 
-        return _Ethernet_activateBpdFilter(self);
+        return activateBpdFilter(self);
     }
 }
 
-int _Ethernet_setBpfEthertypeFilter(EthernetSocket self, uint16_t etherType)
+static int
+setBpfEthertypeFilter(EthernetSocket self, uint16_t etherType)
 {
     if (etherType)
     {
@@ -83,14 +155,14 @@ int _Ethernet_setBpfEthertypeFilter(EthernetSocket self, uint16_t etherType)
         // Set protocol.
         self->bpfProgram.bf_insns[9].k = etherType;
 
-        return _Ethernet_activateBpdFilter(self);
+        return activateBpdFilter(self);
     }
     else
     {
         // Disable Ethertype filter.
         self->bpfProgram.bf_insns[6].k = 0;
 
-        return _Ethernet_activateBpdFilter(self);
+        return activateBpdFilter(self);
     }
 }
 
@@ -283,13 +355,15 @@ Ethernet_createSocket(const char* interfaceId, uint8_t* destAddress)
     return self;
 }
 
-void Ethernet_setProtocolFilter(EthernetSocket self, uint16_t etherType)
+void
+Ethernet_setProtocolFilter(EthernetSocket self, uint16_t etherType)
 {
-    if (!self || !self->bpfProgram.bf_insns || _Ethernet_setBpfEthertypeFilter(self, etherType))
+    if (!self || !self->bpfProgram.bf_insns || setBpfEthertypeFilter(self, etherType))
         printf("Unable to set ethertype filter!\n");
 }
 
-int Ethernet_receivePacket(EthernetSocket self, uint8_t* buffer, int bufferSize)
+int
+Ethernet_receivePacket(EthernetSocket self, uint8_t* buffer, int bufferSize)
 {
     // If the actual buffer is empty, make a read call to the BSP device in order to get new data.
     if (self->bpfEnd - self->bpfPositon < 4)
@@ -335,13 +409,15 @@ int Ethernet_receivePacket(EthernetSocket self, uint8_t* buffer, int bufferSize)
         return 0;
 }
 
-void Ethernet_sendPacket(EthernetSocket self, uint8_t* buffer, int packetSize)
+void
+Ethernet_sendPacket(EthernetSocket self, uint8_t* buffer, int packetSize)
 {
     // Just send the packet as it is.
     write(self->bpf, buffer, packetSize);
 }
 
-void Ethernet_destroySocket(EthernetSocket self)
+void
+Ethernet_destroySocket(EthernetSocket self)
 {
     // Close the BPF device.
     close(self->bpf);

src/hal/ethernet/linux/ethernet_linux.c

@@ -23,6 +23,7 @@
 
 #include <sys/socket.h>
 #include <sys/ioctl.h>
+#include <sys/poll.h>
 #include <linux/if_packet.h>
 #include <linux/if_ether.h>
 #include <linux/if_arp.h>
@@ -40,6 +41,75 @@ struct sEthernetSocket {
     struct sockaddr_ll socketAddress;
 };
 
+struct sEthernetHandleSet {
+    struct pollfd *handles;
+    int nhandles;
+};
+
+EthernetHandleSet
+EthernetHandleSet_new(void)
+{
+    EthernetHandleSet result = (EthernetHandleSet) GLOBAL_MALLOC(sizeof(struct sEthernetHandleSet));
+
+    if (result != NULL) {
+        result->handles = NULL;
+        result->nhandles = 0;
+    }
+
+    return result;
+}
+
+void
+EthernetHandleSet_addSocket(EthernetHandleSet self, const EthernetSocket sock)
+{
+    if (self != NULL && sock != NULL) {
+        int i = self->nhandles++;
+        self->handles = realloc(self->handles, self->nhandles * sizeof(struct pollfd));
+
+        self->handles[i].fd = sock->rawSocket;
+        self->handles[i].events = POLLIN;
+    }
+}
+
+void
+EthernetHandleSet_removeSocket(EthernetHandleSet self, const EthernetSocket sock)
+{
+    if ((self != NULL) && (sock != NULL)) {
+        unsigned i;
+        for (i = 0; i < self->nhandles; i++) {
+            if (self->handles[i].fd == sock->rawSocket) {
+                memmove(&self->handles[i], &self->handles[i+1], sizeof(struct pollfd) * (self->nhandles - i - 1));
+                self->nhandles--;
+                return;
+            }
+        }
+    }
+}
+
+int
+EthernetHandleSet_waitReady(EthernetHandleSet self, unsigned int timeoutMs)
+{
+    int result;
+
+    if ((self != NULL) && (self->nhandles >= 0)) {
+        result = poll(self->handles, self->nhandles, timeoutMs);
+    }
+    else {
+       result = -1;
+    }
+
+    return result;
+}
+
+void
+EthernetHandleSet_destroy(EthernetHandleSet self)
+{
+    if (self->nhandles)
+        free(self->handles);
+
+    GLOBAL_FREEMEM(self);
+}
+
 static int
 getInterfaceIndex(int sock, const char* deviceName)
 {

src/hal/ethernet/win32/ethernet_win32.c

@@ -47,6 +47,8 @@
 
 #define HAVE_REMOTE
 
+// Enable WinPcap specific extension: pcap_getevent()
+#define WPCAP
 #include "pcap.h"
 
 struct sEthernetSocket {
@@ -54,6 +56,11 @@ struct sEthernetSocket {
     struct bpf_program etherTypeFilter;
 };
 
+struct sEthernetHandleSet {
+    HANDLE *handles;
+    int nhandles;
+};
+
 #ifdef __GNUC__ /* detect MINGW */
 
 #ifndef __MINGW64_VERSION_MAJOR
@@ -90,7 +97,6 @@ typedef ULONG (WINAPI* pgetadaptersaddresses)(ULONG family, ULONG flags, PVOID r
 
 static pgetadaptersaddresses GetAdaptersAddresses;
 
-
 static bool dllLoaded = false;
 
 static void
@@ -115,6 +121,71 @@ loadDLLs(void)
 
 #endif /* __GNUC__ */
 
+EthernetHandleSet
+EthernetHandleSet_new(void)
+{
+    EthernetHandleSet result = (EthernetHandleSet) GLOBAL_MALLOC(sizeof(struct sEthernetHandleSet));
+
+    if (result != NULL) {
+        result->handles = NULL;
+        result->nhandles = 0;
+    }
+
+    return result;
+}
+
+void
+EthernetHandleSet_addSocket(EthernetHandleSet self, const EthernetSocket sock)
+{
+    if (self != NULL && sock != NULL) {
+        int i = self->nhandles++;
+        self->handles = (HANDLE *) realloc(self->handles, self->nhandles * sizeof(HANDLE));
+            
+        self->handles[i] = pcap_getevent(sock->rawSocket);
+    }
+}
+
+void
+EthernetHandleSet_removeSocket(EthernetHandleSet self, const EthernetSocket sock)
+{
+    if ((self != NULL) && (sock != NULL)) {
+        HANDLE h = pcap_getevent(socket->rawSocket);
+
+        unsigned i;
+        for (i = 0; i < self->nhandles; i++) {
+            if (self->handles[i] == h) {
+                memmove(&self->handles[i], &self->handles[i+1], sizeof(HANDLE) * (self->nhandles - i - 1));
+                self->nhandles--;
+                return;
+            }
+        }
+    }
+}
+
+int
+EthernetHandleSet_waitReady(EthernetHandleSet self, unsigned int timeoutMs)
+{
+    int result;
+
+    if ((self != NULL) && (self->nhandles > 0)) {
+        result = WaitForMultipleObjects(self->nhandles, self->handles, 0, timeoutMs);
+    }
+    else {
+       result = -1;
+    }
+
+    return result;
+}
+
+void
+EthernetHandleSet_destroy(EthernetHandleSet self)
+{
+    if (self->handles)
+        free(self->handles);
+
+    GLOBAL_FREEMEM(self);
+}
+
 static char*
 getInterfaceName(int interfaceIndex)
 {
@@ -215,8 +286,6 @@ getAdapterMacAddress(char* pcapAdapterName, uint8_t* macAddress)
     }
 }
 
-
-
 void
 Ethernet_getInterfaceMACAddress(const char* interfaceId, uint8_t* addr)
 {
@@ -338,6 +407,28 @@ Ethernet_isSupported()
     return false;
 }
 
+EthernetHandleSet
+EthernetHandleSet_new(void)
+{
+    return NULL;
+}
+
+void
+EthernetHandleSet_addSocket(EthernetHandleSet self, const EthernetSocket sock)
+{
+}
+
+int
+EthernetHandleSet_waitReady(EthernetHandleSet self, unsigned int timeoutMs)
+{
+    return 0;
+}
+
+void
+EthernetHandleSet_destroy(EthernetHandleSet self)
+{
+}
+
 void
 Ethernet_getInterfaceMACAddress(const char* interfaceId, uint8_t* addr)
 {

src/hal/inc/hal_ethernet.h

@@ -48,6 +48,58 @@ extern "C" {
  */
 typedef struct sEthernetSocket* EthernetSocket;
 
+/** Opaque reference for a set of ethernet socket handles */
+typedef struct sEthernetHandleSet* EthernetHandleSet;
+
+/**
+ * \brief Create a new connection handle set (EthernetHandleSet)
+ *
+ * \return new EthernetHandleSet instance
+ */
+EthernetHandleSet
+EthernetHandleSet_new(void);
+
+/**
+ * \brief add a socket to an existing handle set
+ *
+ * \param self the HandleSet instance
+ * \param sock the socket to add
+ */
+void
+EthernetHandleSet_addSocket(EthernetHandleSet self, const EthernetSocket sock);
+
+/**
+ * \brief remove a socket from an existing handle set
+ *
+ * \param self the HandleSet instance
+ * \param sock the socket to add
+ */
+void
+EthernetHandleSet_removeSocket(EthernetHandleSet self, const EthernetSocket sock);
+
+/**
+ * \brief wait for a socket to become ready
+ *
+ * This function is corresponding to the BSD socket select function.
+ * The function will return after \p timeoutMs ms if no data is pending.
+ *
+ * \param self the HandleSet instance
+ * \param timeout in milliseconds (ms)
+ * \return It returns the number of sockets on which data is pending
+ *   or 0 if no data is pending on any of the monitored connections.
+ *   The function shall return -1 if a socket error occures.
+ */
+int
+EthernetHandleSet_waitReady(EthernetHandleSet self, unsigned int timeoutMs);
+
+/**
+ * \brief destroy the EthernetHandleSet instance
+ *
+ * \param self the HandleSet instance to destroy
+ */
+void
+EthernetHandleSet_destroy(EthernetHandleSet self);
+
 /**
  * \brief Return the MAC address of an Ethernet interface.
  *

src/hal/inc/hal_socket.h

@@ -63,7 +63,7 @@ HandleSet
 Handleset_new(void);
 
 /**
- * \brief add a soecket to an existing handle set
+ * \brief add a socket to an existing handle set
  *
  * \param self the HandleSet instance
  * \param sock the socket to add
@@ -71,18 +71,17 @@ Handleset_new(void);
 void
 Handleset_addSocket(HandleSet self, const Socket sock);
 
-
 /**
  * \brief wait for a socket to become ready
  *
  * This function is corresponding to the BSD socket select function.
- * It returns the number of sockets on which data is pending or 0 if no data is pending
- * on any of the monitored connections. The function will return after "timeout" ms if no
- * data is pending.
- * The function shall return -1 if a socket error occures.
+ * The function will return after \p timeoutMs ms if no data is pending.
  *
- *  \param self the HandleSet instance
- *  \param timeout in milliseconds (ms)
+ * \param self the HandleSet instance
+ * \param timeout in milliseconds (ms)
+ * \return It returns the number of sockets on which data is pending
+ *   or 0 if no data is pending on any of the monitored connections.
+ *   The function shall return -1 if a socket error occures.
  */
 int
 Handleset_waitReady(HandleSet self, unsigned int timeoutMs);

src/sampled_values/sv_subscriber.c

@@ -229,6 +229,11 @@ SVReceiver_stopThreadless(SVReceiver self)
     self->running = false;
 }
 
+void
+SVReceiver_addHandleSet(SVReceiver self, EthernetHandleSet handles)
+{
+    return EthernetHandleSet_addSocket(handles, self->ethSocket);
+}
 
 static void
 parseASDU(SVReceiver self, SVSubscriber subscriber, uint8_t* buffer, int length)

src/sampled_values/sv_subscriber.h

@@ -210,9 +210,32 @@ SVReceiver_startThreadless(SVReceiver self);
 void
 SVReceiver_stopThreadless(SVReceiver self);
 
+/**
+ * \brief Parse SV messages if they are available.
+ *
+ * Call after reception of ethernet frame and periodically to to house keeping tasks
+ *
+ * \param self the receiver object
+ *
+ * \return true if a message was available and has been parsed, false otherwise
+ */
 bool
 SVReceiver_tick(SVReceiver self);
 
+/* Forward declaration */
+typedef struct sEthernetHandleSet* EthernetHandleSet;
+
+/**
+ * \brief Add the receiver to a handleset for multiplexed asynchronous IO.
+ *
+ * Note: This function must only be called after SVReceiver_startThreadless().
+ *
+ * \param[in] self The SVReceiver instance.
+ * \param[inout] handles The EthernetHandleSet to which the EthernetSocket of this receiver should be added.
+ */
+void
+SVReceiver_addHandleSet(SVReceiver self, EthernetHandleSet handles);
+
 /*
  * Subscriber
  */