feat: add support for reth locators

eclipse-zenoh · Dec 20, 2023 · 636f088 · 636f088
1 parent a952247
commit 636f088
Show file tree

Hide file tree

Showing 3 changed files with 77 additions and 7 deletions.
diff --git a/include/zenoh-pico/link/config/raweth.h b/include/zenoh-pico/link/config/raweth.h
@@ -24,5 +24,8 @@
 
 int8_t _z_endpoint_raweth_valid(_z_endpoint_t *endpoint);
 int8_t _z_new_link_raweth(_z_link_t *zl, _z_endpoint_t endpoint);
+size_t _z_raweth_config_strlen(const _z_str_intmap_t *s);
+char *_z_raweth_config_to_str(const _z_str_intmap_t *s);
+int8_t _z_raweth_config_from_str(_z_str_intmap_t *strint, const char *s);
 
 #endif /* ZENOH_PICO_LINK_CONFIG_RAWETH_H */
diff --git a/src/link/endpoint.c b/src/link/endpoint.c
@@ -35,6 +35,8 @@
 #if Z_FEATURE_LINK_WS == 1
 #include "zenoh-pico/link/config/ws.h"
 #endif
+#include "zenoh-pico/link/config/raweth.h"
+
 /*------------------ Locator ------------------*/
 void _z_locator_init(_z_locator_t *locator) {
     locator->_protocol = NULL;
@@ -317,7 +319,9 @@ int8_t _z_endpoint_config_from_str(_z_str_intmap_t *strint, const char *str, con
             ret = _z_ws_config_from_str(strint, p_start);
         } else
 #endif
-        {
+            if (_z_str_eq(proto, RAWETH_SCHEMA) == true) {
+            _z_raweth_config_from_str(strint, p_start);
+        } else {
             ret = _Z_ERR_CONFIG_LOCATOR_SCHEMA_UNKNOWN;
         }
     }
@@ -354,7 +358,9 @@ size_t _z_endpoint_config_strlen(const _z_str_intmap_t *s, const char *proto) {
         len = _z_ws_config_strlen(s);
     } else
 #endif
-    {
+        if (_z_str_eq(proto, RAWETH_SCHEMA) == true) {
+        len = _z_raweth_config_strlen(s);
+    } else {
         __asm__("nop");
     }
 
@@ -390,7 +396,9 @@ char *_z_endpoint_config_to_str(const _z_str_intmap_t *s, const char *proto) {
         res = _z_ws_config_to_str(s);
     } else
 #endif
-    {
+        if (_z_str_eq(proto, RAWETH_SCHEMA) == true) {
+        _z_raweth_config_to_str(s);
+    } else {
         __asm__("nop");
     }
 

diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c
@@ -27,7 +27,26 @@
 
 #if Z_FEATURE_RAWETH_TRANSPORT == 1
 
-static _Bool __z_valid_address_raweth(const char *address) {
+#define RAWETH_CONFIG_ARGC 1
+
+#define RAWETH_CONFIG_IFACE_KEY 0x01
+#define RAWETH_CONFIG_IFACE_STR "iface"
+
+#define RAWETH_CONFIG_MAPPING_BUILD               \
+    _z_str_intmapping_t args[RAWETH_CONFIG_ARGC]; \
+    args[0]._key = RAWETH_CONFIG_IFACE_KEY;       \
+    args[0]._str = RAWETH_CONFIG_IFACE_STR;
+
+static _Bool _z_valid_iface_raweth(_z_str_intmap_t *config) {
+    const char *iface = _z_str_intmap_get(config, RAWETH_CONFIG_IFACE_KEY);
+    return (iface != NULL);
+}
+
+static const char *_z_get_iface_raweth(_z_str_intmap_t *config) {
+    return _z_str_intmap_get(config, RAWETH_CONFIG_IFACE_KEY);
+}
+
+static _Bool _z_valid_address_raweth(const char *address) {
     // Check if the string has the correct length
     size_t len = strlen(address);
     if (len != 17) {  // 6 pairs of hexadecimal digits and 5 colons
@@ -51,7 +70,7 @@ static _Bool __z_valid_address_raweth(const char *address) {
     return true;
 }
 
-static uint8_t *__z_parse_address_raweth(const char *address) {
+static uint8_t *_z_parse_address_raweth(const char *address) {
     size_t len = strlen(address);
     // Allocate data
     uint8_t *ret = (uint8_t *)z_malloc(_ZP_MAC_ADDR_LENGTH);
@@ -71,9 +90,19 @@ 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);
+    if (_z_valid_address_raweth(self->_endpoint._locator._address)) {
+        uint8_t *addr = _z_parse_address_raweth(self->_endpoint._locator._address);
+        memcpy(&self->_socket._raweth._smac, addr, _ZP_MAC_ADDR_LENGTH);
+        z_free(addr);
+    } else {
+        memcpy(&self->_socket._raweth._smac, _ZP_RAWETH_CFG_SMAC, _ZP_MAC_ADDR_LENGTH);
+    }
     // Init socket interface
-    self->_socket._raweth._interface = _ZP_RAWETH_CFG_INTERFACE;
+    if (_z_valid_iface_raweth(&self->_endpoint._config)) {
+        self->_socket._raweth._interface = _z_get_iface_raweth(&self->_endpoint._config);
+    } else {
+        self->_socket._raweth._interface = _ZP_RAWETH_CFG_INTERFACE;
+    }
     // Open raweth link
     return _z_open_raweth(&self->_socket._raweth._sock, _ZP_RAWETH_CFG_INTERFACE);
 }
@@ -150,6 +179,21 @@ int8_t _z_new_link_raweth(_z_link_t *zl, _z_endpoint_t endpoint) {
 
     return ret;
 }
+
+size_t _z_raweth_config_strlen(const _z_str_intmap_t *s) {
+    RAWETH_CONFIG_MAPPING_BUILD
+    return _z_str_intmap_strlen(s, RAWETH_CONFIG_ARGC, args);
+}
+char *_z_raweth_config_to_str(const _z_str_intmap_t *s) {
+    RAWETH_CONFIG_MAPPING_BUILD
+    return _z_str_intmap_to_str(s, RAWETH_CONFIG_ARGC, args);
+}
+
+int8_t _z_raweth_config_from_str(_z_str_intmap_t *strint, const char *s) {
+    RAWETH_CONFIG_MAPPING_BUILD
+    return _z_str_intmap_from_strn(strint, s, RAWETH_CONFIG_ARGC, args, strlen(s));
+}
+
 #else
 int8_t _z_endpoint_raweth_valid(_z_endpoint_t *endpoint) {
     _ZP_UNUSED(endpoint);
@@ -161,4 +205,19 @@ int8_t _z_new_link_raweth(_z_link_t *zl, _z_endpoint_t endpoint) {
     _ZP_UNUSED(endpoint);
     return _Z_ERR_TRANSPORT_NOT_AVAILABLE;
 }
+
+size_t _z_raweth_config_strlen(const _z_str_intmap_t *s) {
+    _ZP_UNUSED(s);
+    return 0;
+}
+char *_z_raweth_config_to_str(const _z_str_intmap_t *s) {
+    _ZP_UNUSED(s);
+    return NULL;
+}
+
+int8_t _z_raweth_config_from_str(_z_str_intmap_t *strint, const char *s) {
+    _ZP_UNUSED(strint);
+    _ZP_UNUSED(s);
+    return _Z_ERR_TRANSPORT_NOT_AVAILABLE;
+}
 #endif