diff --git a/CMakeLists.txt b/CMakeLists.txt index 6c62ad2d..7457fe4f 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/examples/goose_subscriber/goose_subscriber_example.c b/examples/goose_subscriber/goose_subscriber_example.c index 5d28534b..511a9cf8 100644 --- a/examples/goose_subscriber/goose_subscriber_example.c +++ b/examples/goose_subscriber/goose_subscriber_example.c @@ -7,6 +7,7 @@ */ #include "goose_receiver.h" +#include "goose_subscriber.h" #include "hal_thread.h" #include diff --git a/src/goose/goose_receiver.c b/src/goose/goose_receiver.c index 0f1b5355..c99e0426 100644 --- a/src/goose/goose_receiver.c +++ b/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); +} + diff --git a/src/goose/goose_receiver.h b/src/goose/goose_receiver.h index 8ba5648c..7d854ae0 100644 --- a/src/goose/goose_receiver.h +++ b/src/goose/goose_receiver.h @@ -24,18 +24,19 @@ #ifndef GOOSE_RECEIVER_H_ #define GOOSE_RECEIVER_H_ -#include - #ifdef __cplusplus extern "C" { #endif +#include + /** * \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 diff --git a/src/hal/ethernet/bsd/ethernet_bsd.c b/src/hal/ethernet/bsd/ethernet_bsd.c index fe601996..f35d8b5d 100644 --- a/src/hal/ethernet/bsd/ethernet_bsd.c +++ b/src/hal/ethernet/bsd/ethernet_bsd.c @@ -22,6 +22,7 @@ */ #include #include +#include #include #include #include @@ -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); diff --git a/src/hal/ethernet/linux/ethernet_linux.c b/src/hal/ethernet/linux/ethernet_linux.c index cff4f068..6715c526 100644 --- a/src/hal/ethernet/linux/ethernet_linux.c +++ b/src/hal/ethernet/linux/ethernet_linux.c @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -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) { diff --git a/src/hal/ethernet/win32/ethernet_win32.c b/src/hal/ethernet/win32/ethernet_win32.c index dddbaf40..df76eaab 100644 --- a/src/hal/ethernet/win32/ethernet_win32.c +++ b/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) { diff --git a/src/hal/inc/hal_ethernet.h b/src/hal/inc/hal_ethernet.h index 137e717c..4efb3670 100644 --- a/src/hal/inc/hal_ethernet.h +++ b/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. * diff --git a/src/hal/inc/hal_socket.h b/src/hal/inc/hal_socket.h index a6d64c08..3fddd3f2 100644 --- a/src/hal/inc/hal_socket.h +++ b/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); diff --git a/src/sampled_values/sv_subscriber.c b/src/sampled_values/sv_subscriber.c index 993e2bc9..c17d05b8 100644 --- a/src/sampled_values/sv_subscriber.c +++ b/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) diff --git a/src/sampled_values/sv_subscriber.h b/src/sampled_values/sv_subscriber.h index bc1ceb4f..bc62bf94 100644 --- a/src/sampled_values/sv_subscriber.h +++ b/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 */