feat: add config interface for socket binding

eclipse-zenoh · Nov 30, 2023 · 9ed1002 · 9ed1002
1 parent 35342a5
commit 9ed1002
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 40 deletions.
diff --git 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
@@ -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
@@ -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 <linux/if_packet.h>
 
-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
@@ -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
@@ -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); }