diff --git a/include/zenoh-pico/session/session.h b/include/zenoh-pico/session/session.h index a13b8a238..becf8f67a 100644 --- a/include/zenoh-pico/session/session.h +++ b/include/zenoh-pico/session/session.h @@ -51,7 +51,6 @@ void _z_reply_data_clear(_z_reply_data_t *rd); _Z_ELEM_DEFINE(_z_reply_data, _z_reply_data_t, _z_noop_size, _z_reply_data_clear, _z_noop_copy) _Z_LIST_DEFINE(_z_reply_data, _z_reply_data_t) -_Z_ARRAY_DEFINE(_z_reply_data, _z_reply_data_t) /** * An reply to a :c:func:`z_query`. diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index 6ce8ad67e..6ac5523d8 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -18,6 +18,7 @@ #include #include "zenoh-pico/collections/string.h" +#include "zenoh-pico/protocol/core.h" #include "zenoh-pico/system/platform.h" #if Z_FEATURE_RAWETH_TRANSPORT == 1 @@ -31,6 +32,27 @@ // Max frame size #define _ZP_MAX_ETH_FRAME_SIZE 1514 +// Endpoing config types +typedef struct { + _z_keyexpr_t _keyexpr; + uint16_t _vlan; // vlan tag (pcp + dei + id), big endian + uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; + _Bool _has_vlan; +} _zp_raweth_mapping_entry_t; + +void _z_raweth_clear_mapping_entry(_zp_raweth_mapping_entry_t *entry); + +_Z_ELEM_DEFINE(_zp_raweth_mapping, _zp_raweth_mapping_entry_t, _z_noop_size, _z_raweth_clear_mapping_entry, + _z_noop_copy) +_Z_ARRAY_DEFINE(_zp_raweth_mapping, _zp_raweth_mapping_entry_t) + +typedef struct { + uint8_t _mac[_ZP_MAC_ADDR_LENGTH]; +} _zp_raweth_whitelist_entry_t; + +_Z_ELEM_DEFINE(_zp_raweth_whitelist, _zp_raweth_whitelist_entry_t, _z_noop_size, _z_noop_clear, _z_noop_copy) +_Z_ARRAY_DEFINE(_zp_raweth_whitelist, _zp_raweth_whitelist_entry_t) + // Ethernet header structure type typedef struct { uint8_t dmac[_ZP_MAC_ADDR_LENGTH]; // Destination mac address @@ -51,8 +73,10 @@ typedef struct { typedef struct { const char *_interface; _z_sys_net_socket_t _sock; + _zp_raweth_mapping_array_t _mapping; + _zp_raweth_whitelist_array_t _whitelist; uint16_t _vlan; - uint16_t ethtype; + uint16_t _ethtype; uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; uint8_t _smac[_ZP_MAC_ADDR_LENGTH]; _Bool _has_vlan; @@ -61,7 +85,8 @@ typedef struct { int8_t _z_get_smac_raweth(_z_raweth_socket_t *resock); 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); +size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buff_len, _z_bytes_t *addr, + const _zp_raweth_whitelist_array_t *whitelist); int8_t _z_close_raweth(_z_sys_net_socket_t *sock); size_t _z_raweth_ntohs(size_t val); size_t _z_raweth_htons(size_t val); diff --git a/include/zenoh-pico/transport/raweth/config.h b/include/zenoh-pico/transport/raweth/config.h deleted file mode 100644 index c8d644747..000000000 --- a/include/zenoh-pico/transport/raweth/config.h +++ /dev/null @@ -1,58 +0,0 @@ -// -// Copyright (c) 2022 ZettaScale Technology -// -// This program and the accompanying materials are made available under the -// terms of the Eclipse Public License 2.0 which is available at -// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 -// which is available at https://www.apache.org/licenses/LICENSE-2.0. -// -// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 -// -// Contributors: -// ZettaScale Zenoh Team, -// - -#ifndef ZENOH_PICO_RAWETH_CONFIG_H -#define ZENOH_PICO_RAWETH_CONFIG_H - -#include - -#include "zenoh-pico/protocol/core.h" -#include "zenoh-pico/system/link/raweth.h" -#include "zenoh-pico/transport/transport.h" -#include "zenoh-pico/utils/result.h" - -#if Z_FEATURE_RAWETH_TRANSPORT == 1 - -typedef struct { - _z_keyexpr_t _keyexpr; - uint16_t _vlan; // vlan tag (pcp + dei + id), big endian - uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; - _Bool _has_vlan; -} _zp_raweth_cfg_entry; - -typedef struct { - uint8_t _mac[_ZP_MAC_ADDR_LENGTH]; -} _zp_raweth_cfg_whitelist_val; - -// 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]; - -// Main config array -extern const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[]; - -// Mac address rx whitelist array -extern const _zp_raweth_cfg_whitelist_val _ZP_RAWETH_CFG_WHITELIST[]; - -// Array size -extern const size_t _ZP_RAWETH_CFG_SIZE; -extern const size_t _ZP_RAWETH_CFG_WHITELIST_SIZE; - -#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 -#endif // ZENOH_PICO_RAWETH_CONFIG_H diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index 78a8aee92..10e35c565 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -31,8 +31,9 @@ #include "zenoh-pico/collections/string.h" #include "zenoh-pico/config.h" +#include "zenoh-pico/protocol/keyexpr.h" +#include "zenoh-pico/system/link/raweth.h" #include "zenoh-pico/system/platform/unix.h" -#include "zenoh-pico/transport/raweth/config.h" #include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/pointers.h" @@ -43,6 +44,8 @@ #else #include +void _z_raweth_clear_mapping_entry(_zp_raweth_mapping_entry_t *entry) { _z_keyexpr_clear(&entry->_keyexpr); } + 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 @@ -89,19 +92,24 @@ size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t return (size_t)wb; } -size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buff_len, _z_bytes_t *addr) { +size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buff_len, _z_bytes_t *addr, + const _zp_raweth_whitelist_array_t *whitelist) { // Read from socket ssize_t bytesRead = recvfrom(sock->_fd, buff, buff_len, 0, NULL, NULL); if ((bytesRead < 0) || (bytesRead < sizeof(_zp_eth_header_t))) { return SIZE_MAX; } - // Address filtering - _zp_eth_header_t *header = (_zp_eth_header_t *)buff; - _Bool is_valid = false; - for (size_t i = 0; i < _ZP_RAWETH_CFG_WHITELIST_SIZE; i++) { - if (memcmp(&header->smac, _ZP_RAWETH_CFG_WHITELIST[i]._mac, _ZP_MAC_ADDR_LENGTH) == 0) { // Test byte ordering - is_valid = true; - break; + _Bool is_valid = true; + // Address filtering (only if there is a whitelist) + if (_zp_raweth_whitelist_array_len(whitelist) > 0) { + is_valid = false; + const _zp_eth_header_t *header = (_zp_eth_header_t *)buff; + for (size_t i = 0; i < _zp_raweth_whitelist_array_len(whitelist); i++) { + const _zp_raweth_whitelist_entry_t *entry = _zp_raweth_whitelist_array_get(whitelist, i); + if (memcmp(&header->smac, entry->_mac, _ZP_MAC_ADDR_LENGTH) == 0) { + is_valid = true; + break; + } } } // Ignore packet from unknown sources diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c deleted file mode 100644 index 08e4684e7..000000000 --- a/src/transport/raweth/config.c +++ /dev/null @@ -1,47 +0,0 @@ -// -// Copyright (c) 2022 ZettaScale Technology -// -// This program and the accompanying materials are made available under the -// terms of the Eclipse Public License 2.0 which is available at -// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 -// which is available at https://www.apache.org/licenses/LICENSE-2.0. -// -// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 -// -// Contributors: -// ZettaScale Zenoh Team, -// - -#include "zenoh-pico/transport/raweth/config.h" - -#if Z_FEATURE_RAWETH_TRANSPORT == 1 - -// Should be generated (big endian format) -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}; - -// Should be generated -const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[] = { - {{0, {0}, ""}, 0x0000, {0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}, false}, // Default mac addr - {{0, {0}, "some/key/expr"}, 0x8c00, {0x00, 0x11, 0x22, 0x33, 0x44, 0x55}, true}, // entry1 - {{0, {0}, "demo/example/zenoh-pico-pub"}, 0xab00, {0x41, 0x55, 0xa8, 0x00, 0x9d, 0xc0}, true}, // entry2 - {{0, {0}, "another/keyexpr"}, 0x4300, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true}, // entry3 -}; - -// Should be generated -const _zp_raweth_cfg_whitelist_val _ZP_RAWETH_CFG_WHITELIST[] = { - {{0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}}, - {{0x00, 0x11, 0x22, 0x33, 0x44, 0x55}}, - {{0x30, 0x03, 0xc8, 0x37, 0x25, 0xa1}}, -}; - -// Don't modify -const size_t _ZP_RAWETH_CFG_SIZE = _ZP_ARRAY_SIZE(_ZP_RAWETH_CFG_ARRAY); -const size_t _ZP_RAWETH_CFG_WHITELIST_SIZE = _ZP_ARRAY_SIZE(_ZP_RAWETH_CFG_WHITELIST); - -#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index 8eb42abc9..6cb90877c 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -20,22 +20,69 @@ #include "zenoh-pico/config.h" #include "zenoh-pico/link/config/raweth.h" #include "zenoh-pico/link/manager.h" +#include "zenoh-pico/protocol/codec/core.h" #include "zenoh-pico/system/link/raweth.h" #include "zenoh-pico/system/platform.h" -#include "zenoh-pico/transport/raweth/config.h" +#include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/pointers.h" #if Z_FEATURE_RAWETH_TRANSPORT == 1 -#define RAWETH_CONFIG_ARGC 1 +#define RAWETH_CFG_TUPLE_SEPARATOR '#' +#define RAWETH_CFG_LIST_SEPARATOR "," + +#define RAWETH_CONFIG_ARGC 4 #define RAWETH_CONFIG_IFACE_KEY 0x01 #define RAWETH_CONFIG_IFACE_STR "iface" +#define RAWETH_CONFIG_ETHTYPE_KEY 0x02 +#define RAWETH_CONFIG_ETHTYPE_STR "ethtype" + +#define RAWETH_CONFIG_MAPPING_KEY 0x03 +#define RAWETH_CONFIG_MAPPING_STR "mapping" + +#define RAWETH_CONFIG_WHITELIST_KEY 0x04 +#define RAWETH_CONFIG_WHITELIST_STR "whitelist" + #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; + args[0]._str = RAWETH_CONFIG_IFACE_STR; \ + args[1]._key = RAWETH_CONFIG_ETHTYPE_KEY; \ + args[1]._str = RAWETH_CONFIG_ETHTYPE_STR; \ + args[2]._key = RAWETH_CONFIG_MAPPING_KEY; \ + args[2]._str = RAWETH_CONFIG_MAPPING_STR; \ + args[3]._key = RAWETH_CONFIG_WHITELIST_KEY; \ + args[3]._str = RAWETH_CONFIG_WHITELIST_STR; + +const uint16_t _ZP_RAWETH_DEFAULT_ETHTYPE = 0x72e0; +const char *_ZP_RAWETH_DEFAULT_INTERFACE = "lo"; +const uint8_t _ZP_RAWETH_DEFAULT_SMAC[_ZP_MAC_ADDR_LENGTH] = {0x30, 0x03, 0xc8, 0x37, 0x25, 0xa1}; +const _zp_raweth_mapping_entry_t _ZP_RAWETH_DEFAULT_MAPPING = { + {0, {0}, ""}, 0x0000, {0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}, false}; + +static _Bool _z_valid_iface_raweth(_z_str_intmap_t *config); +static const char *_z_get_iface_raweth(_z_str_intmap_t *config); +static _Bool _z_valid_ethtype_raweth(_z_str_intmap_t *config); +static long _z_get_ethtype_raweth(_z_str_intmap_t *config); +static size_t _z_valid_mapping_raweth(_z_str_intmap_t *config); +static int8_t _z_get_mapping_raweth(_z_str_intmap_t *config, _zp_raweth_mapping_array_t *array, size_t size); +static size_t _z_valid_whitelist_raweth(_z_str_intmap_t *config); +static int8_t _z_get_whitelist_raweth(_z_str_intmap_t *config, _zp_raweth_whitelist_array_t *array, size_t size); +static int8_t _z_get_mapping_entry(char *entry, _zp_raweth_mapping_entry_t *storage); +static _Bool _z_valid_mapping_entry(char *entry); +static _Bool _z_valid_address_raweth(const char *address); +static uint8_t *_z_parse_address_raweth(const char *address); +static int8_t _z_f_link_open_raweth(_z_link_t *self); +static int8_t _z_f_link_listen_raweth(_z_link_t *self); +static void _z_f_link_close_raweth(_z_link_t *self); +static void _z_f_link_free_raweth(_z_link_t *self); +static size_t _z_f_link_write_raweth(const _z_link_t *self, const uint8_t *ptr, size_t len); +static size_t _z_f_link_write_all_raweth(const _z_link_t *self, const uint8_t *ptr, size_t len); +static size_t _z_f_link_read_raweth(const _z_link_t *self, uint8_t *ptr, size_t len, _z_bytes_t *addr); +static size_t _z_f_link_read_exact_raweth(const _z_link_t *self, uint8_t *ptr, size_t len, _z_bytes_t *addr); +static uint16_t _z_get_link_mtu_raweth(void); static _Bool _z_valid_iface_raweth(_z_str_intmap_t *config) { const char *iface = _z_str_intmap_get(config, RAWETH_CONFIG_IFACE_KEY); @@ -46,6 +93,220 @@ 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_ethtype_raweth(_z_str_intmap_t *config) { + const char *s_ethtype = _z_str_intmap_get(config, RAWETH_CONFIG_ETHTYPE_KEY); + if (s_ethtype == NULL) { + return false; + } + long ethtype = strtol(s_ethtype, NULL, 16); + return (_z_raweth_htons(ethtype) > 0x600); // Ethtype must be at least 0x600 in network order +} + +static long _z_get_ethtype_raweth(_z_str_intmap_t *config) { + const char *s_ethtype = _z_str_intmap_get(config, RAWETH_CONFIG_ETHTYPE_KEY); + return strtol(s_ethtype, NULL, 16); +} + +static size_t _z_valid_mapping_raweth(_z_str_intmap_t *config) { + // Retrieve list + const char *cfg_str = _z_str_intmap_get(config, RAWETH_CONFIG_MAPPING_KEY); + if (cfg_str == NULL) { + return 0; + } + char *s_mapping = zp_malloc(strlen(cfg_str)); + if (s_mapping == NULL) { + return 0; + } + size_t size = 0; + strcpy(s_mapping, cfg_str); + // Parse list + const char *delim = RAWETH_CFG_LIST_SEPARATOR; + char *entry = strtok(s_mapping, delim); + while (entry != NULL) { + // Check entry + if (!_z_valid_mapping_entry(entry)) { + zp_free(s_mapping); + return 0; + } + size++; + entry = strtok(NULL, delim); + } + // Clean up + zp_free(s_mapping); + return size; +} + +static int8_t _z_get_mapping_raweth(_z_str_intmap_t *config, _zp_raweth_mapping_array_t *array, size_t size) { + // Retrieve data + const char *cfg_str = _z_str_intmap_get(config, RAWETH_CONFIG_MAPPING_KEY); + if (cfg_str == NULL) { + return _Z_ERR_GENERIC; + } + // Copy data + char *s_mapping = zp_malloc(strlen(cfg_str)); + if (s_mapping == NULL) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + strcpy(s_mapping, cfg_str); + // Allocate array + *array = _zp_raweth_mapping_array_make(size); + if (_zp_raweth_mapping_array_len(array) == 0) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + size_t idx = 0; + // Parse list + const char *delim = RAWETH_CFG_LIST_SEPARATOR; + char *entry = strtok(s_mapping, delim); + while ((entry != NULL) && (idx < _zp_raweth_mapping_array_len(array))) { + // Copy data into array + _Z_CLEAN_RETURN_IF_ERR(_z_get_mapping_entry(entry, _zp_raweth_mapping_array_get(array, idx)), + zp_free(s_mapping)); + // Next iteration + idx++; + entry = strtok(NULL, delim); + } + // Clean up + zp_free(s_mapping); + return _Z_RES_OK; +} + +static const size_t _z_valid_whitelist_raweth(_z_str_intmap_t *config) { + // Retrieve data + const char *cfg_str = _z_str_intmap_get(config, RAWETH_CONFIG_WHITELIST_KEY); + if (cfg_str == NULL) { + return 0; + } + // Copy data + char *s_whitelist = zp_malloc(strlen(cfg_str)); + if (s_whitelist == NULL) { + return 0; + } + strcpy(s_whitelist, cfg_str); + // Parse list + size_t size = 0; + const char *delim = RAWETH_CFG_LIST_SEPARATOR; + char *entry = strtok(s_whitelist, delim); + while (entry != NULL) { + // Check entry + if (!_z_valid_address_raweth(entry)) { + zp_free(s_whitelist); + return 0; + } + size++; + entry = strtok(NULL, delim); + } + // Parse last entry + + // Clean up + zp_free(s_whitelist); + return size; +} + +static int8_t _z_get_whitelist_raweth(_z_str_intmap_t *config, _zp_raweth_whitelist_array_t *array, size_t size) { + // Retrieve data + const char *cfg_str = _z_str_intmap_get(config, RAWETH_CONFIG_WHITELIST_KEY); + if (cfg_str == NULL) { + return _Z_ERR_GENERIC; + } + // Copy data + char *s_whitelist = zp_malloc(strlen(cfg_str)); + if (s_whitelist == NULL) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + strcpy(s_whitelist, cfg_str); + // Allocate array + *array = _zp_raweth_whitelist_array_make(size); + if (_zp_raweth_whitelist_array_len(array) == 0) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + size_t idx = 0; + // Parse list + const char *delim = RAWETH_CFG_LIST_SEPARATOR; + char *entry = strtok(s_whitelist, delim); + while ((entry != NULL) && (idx < _zp_raweth_whitelist_array_len(array))) { + // Convert address from string to int array + uint8_t *addr = _z_parse_address_raweth(entry); + if (addr == NULL) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + // Copy address to entry + _zp_raweth_whitelist_entry_t *elem = _zp_raweth_whitelist_array_get(array, idx); + memcpy(elem->_mac, addr, _ZP_MAC_ADDR_LENGTH); + zp_free(addr); + // Next iteration + idx++; + entry = strtok(NULL, delim); + } + // Clean up + zp_free(s_whitelist); + return _Z_RES_OK; +} + +static int8_t _z_get_mapping_entry(char *entry, _zp_raweth_mapping_entry_t *storage) { + size_t len = strlen(entry); + const char *entry_end = &entry[len - (size_t)1]; + + // Get first tuple member (keyexpr) + char *p_start = &entry[0]; + char *p_end = strchr(p_start, RAWETH_CFG_TUPLE_SEPARATOR); + size_t ke_len = (uintptr_t)p_end - (uintptr_t)p_start; + char *ke_suffix = (char *)zp_malloc(ke_len); + if (ke_suffix == NULL) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + memcpy(ke_suffix, p_start, ke_len); + storage->_keyexpr = _z_rid_with_suffix(Z_RESOURCE_ID_NONE, ke_suffix); + + // Check second entry (address) + p_start = p_end; + p_start++; + p_end = strchr(p_start, RAWETH_CFG_TUPLE_SEPARATOR); + *p_end = '\0'; + uint8_t *addr = _z_parse_address_raweth(p_start); + memcpy(storage->_dmac, addr, _ZP_MAC_ADDR_LENGTH); + zp_free(addr); + *p_end = RAWETH_CFG_TUPLE_SEPARATOR; + + // Check optional third entry (vlan id) + p_start = p_end; + p_start++; + if (p_start >= entry_end) { // No entry + storage->_has_vlan = false; + } else { + storage->_has_vlan = true; + storage->_vlan = (uint16_t)strtol(p_start, NULL, 16); + } + return _Z_RES_OK; +} +static _Bool _z_valid_mapping_entry(char *entry) { + size_t len = strlen(entry); + const char *entry_end = &entry[len - (size_t)1]; + + // Check first tuple member (keyexpr) + char *p_start = &entry[0]; + char *p_end = strchr(p_start, RAWETH_CFG_TUPLE_SEPARATOR); + if (p_end == NULL) { + return false; + } + // Check second entry (address) + p_start = p_end; + p_start++; + if (p_start > entry_end) { + return false; + } + p_end = strchr(p_start, RAWETH_CFG_TUPLE_SEPARATOR); + if (p_end == NULL) { + return false; + } + *p_end = '\0'; + if (!_z_valid_address_raweth(p_start)) { + *p_end = RAWETH_CFG_TUPLE_SEPARATOR; + return false; + } + *p_end = RAWETH_CFG_TUPLE_SEPARATOR; + return true; +} + static _Bool _z_valid_address_raweth(const char *address) { // Check if the string has the correct length size_t len = strlen(address); @@ -89,19 +350,51 @@ static uint8_t *_z_parse_address_raweth(const char *address) { } static int8_t _z_f_link_open_raweth(_z_link_t *self) { + // Init arrays + self->_socket._raweth._mapping = _zp_raweth_mapping_array_empty(); + self->_socket._raweth._whitelist = _zp_raweth_whitelist_array_empty(); // Init socket smac 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); zp_free(addr); } else { - memcpy(&self->_socket._raweth._smac, _ZP_RAWETH_CFG_SMAC, _ZP_MAC_ADDR_LENGTH); + _Z_DEBUG("Invalid locator source mac addr, using default value."); + memcpy(&self->_socket._raweth._smac, _ZP_RAWETH_DEFAULT_SMAC, _ZP_MAC_ADDR_LENGTH); } // Init socket 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; + _Z_DEBUG("Invalid locator interface, using default value %s", _ZP_RAWETH_DEFAULT_INTERFACE); + self->_socket._raweth._interface = _ZP_RAWETH_DEFAULT_INTERFACE; + } + // Init socket ethtype + if (_z_valid_ethtype_raweth(&self->_endpoint._config)) { + self->_socket._raweth._ethtype = (uint16_t)_z_get_ethtype_raweth(&self->_endpoint._config); + } else { + _Z_DEBUG("Invalid locator ethtype, using default value 0x%04x", _ZP_RAWETH_DEFAULT_ETHTYPE); + self->_socket._raweth._ethtype = _ZP_RAWETH_DEFAULT_ETHTYPE; + } + // Init socket mapping + size_t size = _z_valid_mapping_raweth(&self->_endpoint._config); + if (size != (size_t)0) { + _Z_RETURN_IF_ERR(_z_get_mapping_raweth(&self->_endpoint._config, &self->_socket._raweth._mapping, size)); + } else { + _Z_DEBUG("Invalid locator mapping, using default value."); + self->_socket._raweth._mapping = _zp_raweth_mapping_array_make(1); + if (_zp_raweth_mapping_array_len(&self->_socket._raweth._mapping) == 0) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + _zp_raweth_mapping_entry_t *entry = _zp_raweth_mapping_array_get(&self->_socket._raweth._mapping, 0); + *entry = _ZP_RAWETH_DEFAULT_MAPPING; + } + // Init socket whitelist + size = _z_valid_whitelist_raweth(&self->_endpoint._config); + if (size != (size_t)0) { + _Z_RETURN_IF_ERR(_z_get_whitelist_raweth(&self->_endpoint._config, &self->_socket._raweth._whitelist, size)); + } else { + _Z_DEBUG("Invalid locator whitelist, filtering deactivated."); } // Open raweth link return _z_open_raweth(&self->_socket._raweth._sock, self->_socket._raweth._interface); diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c index 162c66c28..09f2fb2e7 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/rx.c @@ -29,7 +29,8 @@ static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z_bytes_t *addr) { uint8_t *buff = _z_zbuf_get_wptr(zbf); - size_t rb = _z_receive_raweth(&link->_socket._raweth._sock, buff, _z_zbuf_space_left(zbf), addr); + size_t rb = _z_receive_raweth(&link->_socket._raweth._sock, buff, _z_zbuf_space_left(zbf), addr, + &link->_socket._raweth._whitelist); // Check validity if ((rb == SIZE_MAX) || (rb < sizeof(_zp_eth_header_t))) { return SIZE_MAX; diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index ac360cc9e..3eb399d6f 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -24,7 +24,6 @@ #include "zenoh-pico/protocol/iobuf.h" #include "zenoh-pico/protocol/keyexpr.h" #include "zenoh-pico/system/link/raweth.h" -#include "zenoh-pico/transport/raweth/config.h" #include "zenoh-pico/transport/transport.h" #include "zenoh-pico/transport/utils.h" #include "zenoh-pico/utils/logging.h" @@ -38,10 +37,10 @@ static void _zp_raweth_unlock_tx_mutex(_z_transport_multicast_t *ztm) { _ZP_UNUS #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++) { + for (int i = 0; i < _zp_raweth_mapping_array_len(&sock->_mapping); i++) { // Find matching keyexpr - if (zp_keyexpr_intersect_null_terminated(keyexpr->_suffix, _ZP_RAWETH_CFG_ARRAY[i]._keyexpr._suffix) != - _Z_RES_OK) { + const _zp_raweth_mapping_entry_t *entry = _zp_raweth_mapping_array_get(&sock->_mapping, i); + if (zp_keyexpr_intersect_null_terminated(keyexpr->_suffix, entry->_keyexpr._suffix) != _Z_RES_OK) { continue; } return i; @@ -52,14 +51,15 @@ static int _zp_raweth_find_map_entry(const _z_keyexpr_t *keyexpr, _z_raweth_sock static int8_t _zp_raweth_set_socket(const _z_keyexpr_t *keyexpr, _z_raweth_socket_t *sock) { int8_t ret = _Z_RES_OK; - if (_ZP_RAWETH_CFG_SIZE < 1) { + if (_zp_raweth_mapping_array_len(&sock->_mapping) < 1) { return _Z_ERR_GENERIC; } if (keyexpr == NULL) { // Store default value into socket - memcpy(&sock->_dmac, &_ZP_RAWETH_CFG_ARRAY[0]._dmac, _ZP_MAC_ADDR_LENGTH); - uint16_t vlan = _ZP_RAWETH_CFG_ARRAY[0]._vlan; - sock->_has_vlan = _ZP_RAWETH_CFG_ARRAY[0]._has_vlan; + const _zp_raweth_mapping_entry_t *entry = _zp_raweth_mapping_array_get(&sock->_mapping, 0); + memcpy(&sock->_dmac, &entry->_dmac, _ZP_MAC_ADDR_LENGTH); + uint16_t vlan = entry->_vlan; + sock->_has_vlan = entry->_has_vlan; if (sock->_has_vlan) { memcpy(&sock->_vlan, &vlan, sizeof(vlan)); } @@ -72,9 +72,10 @@ static int8_t _zp_raweth_set_socket(const _z_keyexpr_t *keyexpr, _z_raweth_socke _Z_DEBUG("Key '%s' wasn't found in config mapping, sending to default address", 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; + const _zp_raweth_mapping_entry_t *entry = _zp_raweth_mapping_array_get(&sock->_mapping, idx); + memcpy(&sock->_dmac, &entry->_dmac, _ZP_MAC_ADDR_LENGTH); + uint16_t vlan = entry->_vlan; + sock->_has_vlan = entry->_has_vlan; if (sock->_has_vlan) { memcpy(&sock->_vlan, &vlan, sizeof(vlan)); } @@ -128,7 +129,7 @@ static int8_t __unsafe_z_raweth_write_header(_z_link_t *zl, _z_wbuf_t *wbf) { memcpy(&header.smac, &resocket->_smac, _ZP_MAC_ADDR_LENGTH); header.vlan_type = _ZP_ETH_TYPE_VLAN; header.tag = resocket->_vlan; - header.ethtype = _ZP_RAWETH_CFG_ETHTYPE; + header.ethtype = resocket->_ethtype; header.data_length = _z_raweth_htons(wpos - sizeof(header)); // Write header _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(wbf, (uint8_t *)&header, 0, sizeof(header))); @@ -137,7 +138,7 @@ static int8_t __unsafe_z_raweth_write_header(_z_link_t *zl, _z_wbuf_t *wbf) { // Set header memcpy(&header.dmac, &resocket->_dmac, _ZP_MAC_ADDR_LENGTH); memcpy(&header.smac, &resocket->_smac, _ZP_MAC_ADDR_LENGTH); - header.ethtype = _ZP_RAWETH_CFG_ETHTYPE; + header.ethtype = resocket->_ethtype; header.data_length = _z_raweth_htons(wpos - sizeof(header)); // Write header _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(wbf, (uint8_t *)&header, 0, sizeof(header)));