Skip to content

Commit

Permalink
Add support for raweth locator (#303)
Browse files Browse the repository at this point in the history 
* feat: add support for reth locators

* feat: send to default address if key not found
  • Loading branch information
@jean-roland
jean-roland authored Dec 20, 2023
1 parent a952247 commit 103d716
Show file tree
Hide file tree
Showing 4 changed files with 101 additions and 23 deletions.
3 changes: 3 additions & 0 deletions include/zenoh-pico/link/config/raweth.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
14 changes: 11 additions & 3 deletions src/link/endpoint.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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");
}

Expand Down Expand Up @@ -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");
}

Expand Down
67 changes: 63 additions & 4 deletions src/transport/raweth/link.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
Expand All @@ -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);
}
Expand Down Expand Up @@ -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);
Expand All @@ -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
40 changes: 24 additions & 16 deletions src/transport/raweth/tx.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;
Expand Down

0 comments on commit 103d716

Please sign in to comment.