diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index afc1c7506..e73a647c3 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -46,6 +46,7 @@ typedef struct { } _zp_eth_vlan_header_t; typedef struct { + const char *_interface; _z_sys_net_socket_t _sock; uint16_t _vlan; uint16_t ethtype; @@ -55,7 +56,7 @@ typedef struct { } _z_raweth_socket_t; int8_t _z_get_smac_raweth(_z_raweth_socket_t *resock); -int8_t _z_open_raweth(_z_sys_net_socket_t *sock); +int8_t _z_open_raweth(_z_sys_net_socket_t *sock, const char *interface); size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t buff_len); size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buff_len, _z_bytes_t *addr); int8_t _z_close_raweth(_z_sys_net_socket_t *sock); diff --git a/include/zenoh-pico/transport/raweth/config.h b/include/zenoh-pico/transport/raweth/config.h index a465c1e57..f199a4797 100644 --- a/include/zenoh-pico/transport/raweth/config.h +++ b/include/zenoh-pico/transport/raweth/config.h @@ -38,6 +38,9 @@ typedef struct { // Ethertype to use in frame extern const uint16_t _ZP_RAWETH_CFG_ETHTYPE; +// Interface to use +extern const char *_ZP_RAWETH_CFG_INTERFACE; + // Source mac address extern const uint8_t _ZP_RAWETH_CFG_SMAC[_ZP_MAC_ADDR_LENGTH]; diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index de4d4afc3..745412da6 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -38,48 +38,35 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 -int8_t _z_get_smac_raweth(_z_raweth_socket_t *resock) { - int8_t ret = _Z_RES_OK; - struct ifaddrs *sys_ifa, *sys_ifa_root; - struct sockaddr *sa; - - // Retrieve all interfaces data - if (getifaddrs(&sys_ifa_root) == -1) { - switch (errno) { - case ENOMEM: - case ENOBUFS: - ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; - break; - case EACCES: - default: - ret = _Z_ERR_GENERIC; - break; - } - } else { - // Parse the interfaces - for (sys_ifa = sys_ifa_root; sys_ifa != NULL && ret == 0; sys_ifa = sys_ifa->ifa_next) { - // Skip interfaces without an address or not up or loopback - if (sys_ifa->ifa_addr == NULL /*|| (sys_ifa->ifa_flags & IFF_UP) == 0 || - (sys_ifa->ifa_flags & IFF_LOOPBACK) != 0*/) { - continue; - } - // Interface is ethernet - if (sys_ifa->ifa_addr->sa_family == AF_PACKET) { - // Copy data - memcpy(resock->_smac, sys_ifa->ifa_addr->sa_data, _ZP_MAC_ADDR_LENGTH); - break; - } - } - freeifaddrs(sys_ifa_root); - } - return ret; -} +#if !defined(__linux) +#error "Raweth transport only supported on linux systems" +#else +#include -int8_t _z_open_raweth(_z_sys_net_socket_t *sock) { +int8_t _z_open_raweth(_z_sys_net_socket_t *sock, const char *interface) { int8_t ret = _Z_RES_OK; // Open a raw network socket in promiscuous mode sock->_fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); if (sock->_fd == -1) { + return _Z_ERR_GENERIC; + } + // Get the index of the interface to send on + struct ifreq if_idx; + memset(&if_idx, 0, sizeof(struct ifreq)); + strncpy(if_idx.ifr_name, interface, strlen(interface)); + if (ioctl(sock->_fd, SIOCGIFINDEX, &if_idx) < 0) { + return _Z_ERR_GENERIC; + } + // Bind the socket + struct sockaddr_ll addr; + memset(&addr, 0, sizeof(addr)); + addr.sll_family = AF_PACKET; + addr.sll_protocol = htons(ETH_P_ALL); + addr.sll_ifindex = if_idx.ifr_ifindex; + addr.sll_pkttype = PACKET_HOST | PACKET_BROADCAST | PACKET_MULTICAST; + + if (bind(sock->_fd, (struct sockaddr *)&addr, sizeof(addr)) == -1) { + close(sock->_fd); ret = _Z_ERR_GENERIC; } return ret; @@ -128,4 +115,5 @@ size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buf return bytesRead; } -#endif +#endif // defined(__linux) +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c index 1132865dd..646865f5a 100644 --- a/src/transport/raweth/config.c +++ b/src/transport/raweth/config.c @@ -19,6 +19,9 @@ // Should be generated const uint16_t _ZP_RAWETH_CFG_ETHTYPE = 0x72e0; +// Should be generated +const char *_ZP_RAWETH_CFG_INTERFACE = "lo"; + // Should be generated const uint8_t _ZP_RAWETH_CFG_SMAC[_ZP_MAC_ADDR_LENGTH] = {0x30, 0x03, 0xc8, 0x37, 0x25, 0xa1}; diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index 162563567..b669e1f02 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -72,8 +72,10 @@ static uint8_t *__z_parse_address_raweth(const char *address) { static int8_t _z_f_link_open_raweth(_z_link_t *self) { // Init socket smac memcpy(&self->_socket._raweth._smac, _ZP_RAWETH_CFG_SMAC, _ZP_MAC_ADDR_LENGTH); + // Init socket interface + self->_socket._raweth._interface = _ZP_RAWETH_CFG_INTERFACE; // Open raweth link - return _z_open_raweth(&self->_socket._raweth._sock); + return _z_open_raweth(&self->_socket._raweth._sock, _ZP_RAWETH_CFG_INTERFACE); } static int8_t _z_f_link_listen_raweth(_z_link_t *self) { return _z_f_link_open_raweth(self); }