diff --git a/include/zenoh-pico/link/config/raweth.h b/include/zenoh-pico/link/config/raweth.h index 996261307..89313d8b6 100644 --- 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 index d06a67158..ff596a07b 100644 --- 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 index b669e1f02..b38f77154 100644 --- 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 diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index c9984bc48..80d96f7b6 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -37,6 +37,18 @@ static void _zp_raweth_unlock_tx_mutex(_z_transport_multicast_t *ztm) { _z_mutex static void _zp_raweth_unlock_tx_mutex(_z_transport_multicast_t *ztm) { _ZP_UNUSED(ztm); } #endif +static int _zp_raweth_find_map_entry(const _z_keyexpr_t *keyexpr, _z_raweth_socket_t *sock) { + for (int i = 1; i < _ZP_RAWETH_CFG_SIZE; i++) { + // Find matching keyexpr + if (zp_keyexpr_intersect_null_terminated(keyexpr->_suffix, _ZP_RAWETH_CFG_ARRAY[i]._keyexpr._suffix) != + _Z_RES_OK) { + continue; + } + return i; + } + return -1; +} + static int8_t _zp_raweth_set_socket(const _z_keyexpr_t *keyexpr, _z_raweth_socket_t *sock) { int8_t ret = _Z_RES_OK; @@ -53,22 +65,18 @@ static int8_t _zp_raweth_set_socket(const _z_keyexpr_t *keyexpr, _z_raweth_socke } } else { // Find config entry (linear) - ret = _Z_ERR_GENERIC; // Key not found case - for (int i = 1; i < _ZP_RAWETH_CFG_SIZE; i++) { - // Find matching keyexpr - if (zp_keyexpr_intersect_null_terminated(keyexpr->_suffix, _ZP_RAWETH_CFG_ARRAY[i]._keyexpr._suffix) != - _Z_RES_OK) { - continue; - } - // Store data into socket - memcpy(&sock->_dmac, &_ZP_RAWETH_CFG_ARRAY[i]._dmac, _ZP_MAC_ADDR_LENGTH); - uint16_t vlan = _ZP_RAWETH_CFG_ARRAY[i]._vlan; - sock->_has_vlan = _ZP_RAWETH_CFG_ARRAY[i]._has_vlan; - if (sock->_has_vlan) { - memcpy(&sock->_vlan, &vlan, sizeof(vlan)); - } - ret = _Z_RES_OK; - break; + int idx = _zp_raweth_find_map_entry(keyexpr, sock); + // Key not found case + if (idx < 0) { + idx = 0; // Set to default entry + _Z_DEBUG("Key '%s' wasn't found in config mapping, sending to default address\n", keyexpr->_suffix); + } + // Store data into socket + memcpy(&sock->_dmac, &_ZP_RAWETH_CFG_ARRAY[idx]._dmac, _ZP_MAC_ADDR_LENGTH); + uint16_t vlan = _ZP_RAWETH_CFG_ARRAY[idx]._vlan; + sock->_has_vlan = _ZP_RAWETH_CFG_ARRAY[idx]._has_vlan; + if (sock->_has_vlan) { + memcpy(&sock->_vlan, &vlan, sizeof(vlan)); } } return ret;