diff --git a/hal/ethernet/bsd/ethernet_bsd.c b/hal/ethernet/bsd/ethernet_bsd.c index 536de0fb..355110df 100644 --- a/hal/ethernet/bsd/ethernet_bsd.c +++ b/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) { diff --git a/hal/ethernet/linux/ethernet_linux.c b/hal/ethernet/linux/ethernet_linux.c index 57d4d3b2..2d97a826 100644 --- a/hal/ethernet/linux/ethernet_linux.c +++ b/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) { diff --git a/hal/ethernet/win32/ethernet_win32.c b/hal/ethernet/win32/ethernet_win32.c index c6269f37..06138da2 100644 --- a/hal/ethernet/win32/ethernet_win32.c +++ b/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) { diff --git a/hal/inc/hal_ethernet.h b/hal/inc/hal_ethernet.h index ecb03f35..d7e3d207 100644 --- a/hal/inc/hal_ethernet.h +++ b/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 * diff --git a/src/goose/goose_receiver.c b/src/goose/goose_receiver.c index b6ea074c..017b1c00 100644 --- a/src/goose/goose_receiver.c +++ b/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