From 976ae717c87de069ed4179616689d98e6815a80f Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Wed, 22 Nov 2023 16:40:50 +0100 Subject: [PATCH 01/38] feat: add new transport config token --- include/zenoh-pico/config.h | 7 +++++++ src/system/arduino/esp32/network.cpp | 4 ++++ src/system/arduino/opencr/network.cpp | 4 ++++ src/system/emscripten/network.c | 4 ++++ src/system/espidf/network.c | 4 ++++ src/system/freertos_plus_tcp/network.c | 12 ++++++++++++ src/system/mbed/network.cpp | 4 ++++ src/system/windows/network.c | 4 ++++ src/system/zephyr/network.c | 4 ++++ src/transport/multicast/tx.c | 4 ++-- 10 files changed, 49 insertions(+), 2 deletions(-) diff --git a/include/zenoh-pico/config.h b/include/zenoh-pico/config.h index fc7fe5f97..35d19ec38 100644 --- a/include/zenoh-pico/config.h +++ b/include/zenoh-pico/config.h @@ -218,6 +218,13 @@ #endif #endif +/** + * Enable raweth transport/link. + */ +#ifndef Z_FEATURE_RAWETH_TRANSPORT +#define Z_FEATURE_RAWETH_TRANSPORT 1 +#endif + /*------------------ Compile-time configuration properties ------------------*/ /** * Default length for Zenoh ID. Maximum size is 16 bytes. diff --git a/src/system/arduino/esp32/network.cpp b/src/system/arduino/esp32/network.cpp index c02557fac..c088f1ce1 100644 --- a/src/system/arduino/esp32/network.cpp +++ b/src/system/arduino/esp32/network.cpp @@ -858,4 +858,8 @@ size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t } #endif +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported yet on ESP32 port of Zenoh-Pico" +#endif + } // extern "C" diff --git a/src/system/arduino/opencr/network.cpp b/src/system/arduino/opencr/network.cpp index bdbb9939a..b99046729 100644 --- a/src/system/arduino/opencr/network.cpp +++ b/src/system/arduino/opencr/network.cpp @@ -347,4 +347,8 @@ size_t _z_send_udp_multicast(const _z_sys_net_socket_t sock, const uint8_t *ptr, #if Z_FEATURE_LINK_SERIAL == 1 #error "Serial not supported yet on OpenCR port of Zenoh-Pico" #endif + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported yet on OpenCR port of Zenoh-Pico" +#endif } diff --git a/src/system/emscripten/network.c b/src/system/emscripten/network.c index 8b2f14caf..14d4dcace 100644 --- a/src/system/emscripten/network.c +++ b/src/system/emscripten/network.c @@ -173,3 +173,7 @@ size_t _z_send_ws(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len #if Z_FEATURE_LINK_SERIAL == 1 #error "Serial not supported yet on Emscripten port of Zenoh-Pico" #endif + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported yet on Emscripten port of Zenoh-Pico" +#endif \ No newline at end of file diff --git a/src/system/espidf/network.c b/src/system/espidf/network.c index 9a49bf157..512635fe9 100644 --- a/src/system/espidf/network.c +++ b/src/system/espidf/network.c @@ -739,3 +739,7 @@ size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t #if Z_FEATURE_LINK_BLUETOOTH == 1 #error "Bluetooth not supported yet on ESP-IDF port of Zenoh-Pico" #endif + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported yet on ESP-IDF port of Zenoh-Pico" +#endif \ No newline at end of file diff --git a/src/system/freertos_plus_tcp/network.c b/src/system/freertos_plus_tcp/network.c index 60d717ddd..57057c58b 100644 --- a/src/system/freertos_plus_tcp/network.c +++ b/src/system/freertos_plus_tcp/network.c @@ -209,3 +209,15 @@ size_t _z_send_udp_unicast(const _z_sys_net_socket_t sock, const uint8_t *ptr, s #if Z_FEATURE_LINK_UDP_MULTICAST == 1 #error "UDP Multicast not supported yet on FreeRTOS-Plus-TCP port of Zenoh-Pico" #endif + +#if Z_FEATURE_LINK_BLUETOOTH == 1 +#error "Bluetooth not supported yet on FreeRTOS-Plus-TCP port of Zenoh-Pico" +#endif + +#if Z_FEATURE_LINK_SERIAL == 1 +#error "Serial not supported yet on FreeRTOS-Plus-TCP port of Zenoh-Pico" +#endif + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported yet on FreeRTOS-Plus-TCP port of Zenoh-Pico" +#endif diff --git a/src/system/mbed/network.cpp b/src/system/mbed/network.cpp index 744b92df0..12e11d8f0 100644 --- a/src/system/mbed/network.cpp +++ b/src/system/mbed/network.cpp @@ -483,4 +483,8 @@ size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t #error "Bluetooth not supported yet on MBED port of Zenoh-Pico" #endif +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported yet on MBED port of Zenoh-Pico" +#endif + } // extern "C" \ No newline at end of file diff --git a/src/system/windows/network.c b/src/system/windows/network.c index 76d37027c..08543da3e 100644 --- a/src/system/windows/network.c +++ b/src/system/windows/network.c @@ -610,3 +610,7 @@ size_t _z_send_udp_multicast(const _z_sys_net_socket_t sock, const uint8_t *ptr, #if Z_FEATURE_LINK_SERIAL == 1 #error "Serial not supported yet on Windows port of Zenoh-Pico" #endif + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported yet on Windows port of Zenoh-Pico" +#endif \ No newline at end of file diff --git a/src/system/zephyr/network.c b/src/system/zephyr/network.c index e07381914..91a693e0a 100644 --- a/src/system/zephyr/network.c +++ b/src/system/zephyr/network.c @@ -728,3 +728,7 @@ size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t #if Z_FEATURE_LINK_BLUETOOTH == 1 #error "Bluetooth not supported yet on Zephyr port of Zenoh-Pico" #endif + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported yet on Zephyr port of Zenoh-Pico" +#endif diff --git a/src/transport/multicast/tx.c b/src/transport/multicast/tx.c index 230c8c85a..e3e0c9b71 100644 --- a/src/transport/multicast/tx.c +++ b/src/transport/multicast/tx.c @@ -155,8 +155,8 @@ int8_t _z_multicast_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_m } #else -int8_t _z_multicast_send_t_msg(_z_transport_multicast_t *ztu, const _z_transport_message_t *t_msg) { - _ZP_UNUSED(ztu); +int8_t _z_multicast_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg) { + _ZP_UNUSED(ztm); _ZP_UNUSED(t_msg); return _Z_ERR_TRANSPORT_NOT_AVAILABLE; } From ae19d39a79cdc63fa5e3ea2e2bdfca12cb42b9c5 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Wed, 22 Nov 2023 16:52:00 +0100 Subject: [PATCH 02/38] feat: add new transport files (wip) --- include/zenoh-pico/system/link/raweth.h | 73 ++++ include/zenoh-pico/transport/raweth/rx.h | 25 ++ .../zenoh-pico/transport/raweth/transport.h | 29 ++ include/zenoh-pico/transport/raweth/tx.h | 25 ++ src/system/unix/link/raweth.c | 108 ++++++ src/transport/raweth/link.c | 184 ++++++++++ src/transport/raweth/rx.c | 339 ++++++++++++++++++ src/transport/raweth/transport.c | 225 ++++++++++++ src/transport/raweth/tx.c | 247 +++++++++++++ 9 files changed, 1255 insertions(+) create mode 100644 include/zenoh-pico/system/link/raweth.h create mode 100644 include/zenoh-pico/transport/raweth/rx.h create mode 100644 include/zenoh-pico/transport/raweth/transport.h create mode 100644 include/zenoh-pico/transport/raweth/tx.h create mode 100644 src/system/unix/link/raweth.c create mode 100644 src/transport/raweth/link.c create mode 100644 src/transport/raweth/rx.c create mode 100644 src/transport/raweth/transport.c create mode 100644 src/transport/raweth/tx.c diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h new file mode 100644 index 000000000..ef2b3e38b --- /dev/null +++ b/include/zenoh-pico/system/link/raweth.h @@ -0,0 +1,73 @@ +// +// 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_SYSTEM_LINK_RAWETH_H +#define ZENOH_PICO_SYSTEM_LINK_RAWETH_H + +#include +#include +#include +#include + +#include "zenoh-pico/collections/string.h" +#include "zenoh-pico/system/platform.h" + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +// Ethernet types +#define _ZP_ETH_TYPE_VLAN 0x8100 // 2048 (0x0800) IPv4 + +// Address Sizes +#define _ZP_MAC_ADDR_LENGTH 6 + +// Max frame size +#define _ZP_MAX_ETH_FRAME_SIZE 1500 + +typedef struct { + uint16_t tpid; // Tag ethertype + uint16_t pcp : 3; // Priority code point + uint16_t dei : 1; // Drop eligible indicator + uint16_t vid : 12; // VLAN id +} _zp_vlan_tag_t; + +// Ethernet header structure type +typedef struct { + uint8_t dmac[_ZP_MAC_ADDR_LENGTH]; // Destination mac address + uint8_t smac[_ZP_MAC_ADDR_LENGTH]; // Source mac address + uint16_t length; // Size of frame +} _zp_eth_header_t; + +typedef struct { + uint8_t dmac[_ZP_MAC_ADDR_LENGTH]; // Destination mac address + uint8_t smac[_ZP_MAC_ADDR_LENGTH]; // Source mac address + _zp_vlan_tag_t tag; + uint16_t length; // Size of frame +} _zp_eth_vlan_header_t; + +typedef struct { + void *_config; // Pointer to config data + _z_sys_net_socket_t _sock; + uint16_t _vlan_val; + uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; + uint8_t _smac[_ZP_MAC_ADDR_LENGTH]; + _Bool has_vlan; +} _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); +size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t buff_len); + +#endif + +#endif /* ZENOH_PICO_SYSTEM_LINK_RAWETH_H */ diff --git a/include/zenoh-pico/transport/raweth/rx.h b/include/zenoh-pico/transport/raweth/rx.h new file mode 100644 index 000000000..5e23c6c9d --- /dev/null +++ b/include/zenoh-pico/transport/raweth/rx.h @@ -0,0 +1,25 @@ +// +// 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_RX_H +#define ZENOH_PICO_RAWETH_RX_H + +#include "zenoh-pico/transport/transport.h" + +int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr); +int8_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr); +int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, + _z_bytes_t *addr); + +#endif /* ZENOH_PICO_RAWETH_RX_H */ diff --git a/include/zenoh-pico/transport/raweth/transport.h b/include/zenoh-pico/transport/raweth/transport.h new file mode 100644 index 000000000..6c747a217 --- /dev/null +++ b/include/zenoh-pico/transport/raweth/transport.h @@ -0,0 +1,29 @@ +// +// 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_TRANSPORT_H +#define ZENOH_PICO_RAWETH_TRANSPORT_H + +#include "zenoh-pico/api/types.h" + +int8_t _z_raweth_transport_create(_z_transport_t *zt, _z_link_t *zl, + _z_transport_multicast_establish_param_t *param); +int8_t _z_raweth_open_peer(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid); +int8_t _z_raweth_open_client(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid); +int8_t _z_raweth_send_close(_z_transport_multicast_t *ztm, uint8_t reason, _Bool link_only); +int8_t _z_raweth_transport_close(_z_transport_multicast_t *ztm, uint8_t reason); +void _z_raweth_transport_clear(_z_transport_t *zt); +#endif /* ZENOH_PICO_RAWETH_TRANSPORT_H */ diff --git a/include/zenoh-pico/transport/raweth/tx.h b/include/zenoh-pico/transport/raweth/tx.h new file mode 100644 index 000000000..39b4758b6 --- /dev/null +++ b/include/zenoh-pico/transport/raweth/tx.h @@ -0,0 +1,25 @@ +// +// 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_TX_H +#define ZENOH_PICO_RAWETH_TX_H + +#include "zenoh-pico/net/session.h" +#include "zenoh-pico/transport/transport.h" + +int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, + z_congestion_control_t cong_ctrl); +int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg); + +#endif /* ZENOH_PICO_RAWETH_TX_H */ diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c new file mode 100644 index 000000000..448a6a62b --- /dev/null +++ b/src/system/unix/link/raweth.c @@ -0,0 +1,108 @@ +// +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "zenoh-pico/collections/string.h" +#include "zenoh-pico/config.h" +#include "zenoh-pico/system/platform/unix.h" +#include "zenoh-pico/system/link/raweth.h" +#include "zenoh-pico/utils/logging.h" +#include "zenoh-pico/utils/pointers.h" + +#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; +} + +int8_t _z_open_raweth(_z_sys_net_socket_t *sock) { + 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) { + ret = _Z_ERR_GENERIC; + } + return ret; +} + +size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t buff_len) { + struct sockaddr_ll saddr; + memset(&saddr, 0, sizeof(saddr)); + saddr.sll_family = AF_PACKET; + saddr.sll_protocol = htons(ETH_P_ALL); + saddr.sll_halen = ETH_ALEN; + + // Retrieve dmac from buff + if (buff_len < ETH_ALEN) { + return SIZE_MAX; + } + memcpy(&saddr.sll_addr, buff, ETH_ALEN); + + // Send data + ssize_t wb = sendto(sock->_fd, buff, buff_len, 0, (struct sockaddr *)&saddr, sizeof(saddr)); + if (wb < 0) { + return SIZE_MAX; + } + return (size_t)wb; +} + +#endif \ No newline at end of file diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c new file mode 100644 index 000000000..1cadcd8f4 --- /dev/null +++ b/src/transport/raweth/link.c @@ -0,0 +1,184 @@ +// +// 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 +#include +#include +#include +#include + +#include "zenoh-pico/config.h" +#include "zenoh-pico/link/manager.h" +#include "zenoh-pico/system/platform.h" +#include "zenoh-pico/utils/pointers.h" + +// Address Sizes +#define MAC_ADDR_LENGTH 6 + +#define RAWETH_SCHEMA "reth" + +#define RAWETH_CFG_ARGC 2 + +#define RAWETH_CFG_IFACE_KEY 0x01 +#define RAWETH_CFG_IFACE_STR "iface" + +#define RAWETH_CFG_VLAN_KEY 0x02 +#define RAWETH_CFG_VLAN_STR "vlan" + +#define RAWETH_CFG_MAPPING_BUILD \ + _z_str_intmapping_t args[RAWETH_CFG_ARGC]; \ + args[0]._key = RAWETH_CFG_IFACE_KEY; \ + args[0]._str = RAWETH_CFG_IFACE_STR; \ + args[1]._key = RAWETH_CFG_VLAN_KEY; \ + args[1]._str = RAWETH_CFG_VLAN_STR; + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +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 + return false; + } + // Check if the colons are at the correct positions + for (int i = 2; i < len; i += 3) { + if (address[i] != ':') { + return false; + } + } + // Check if each character is a valid hexadecimal digit + for (int i = 0; i < len; ++i) { + if (i % 3 != 2) { + char c = address[i]; + if (!((c >= '0' && c <= '9') || (c >= 'A' && c <= 'F') || (c >= 'a' && c <= 'f'))) { + return false; + } + } + } + return true; +} + +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); + if (ret == NULL) { + return ret; + } + for (int i = 0; i < _ZP_MAC_ADDR_LENGTH; ++i) { + // Extract a pair of hexadecimal digits from the MAC address string + char byteString[3]; + strncpy(byteString, address + i * 3, 2); + byteString[2] = '\0'; + // Convert the hexadecimal string to an integer + ret[i] = (unsigned char)strtol(byteString, NULL, 16); + } + return ret; +} + +static int8_t _z_f_link_open_raweth(_z_link_t *self) { + // Open raweth link + int8_t ret = _z_open_raweth(&self->_socket._raweth._sock); + if (ret != _Z_RES_OK) { + return ret; + } + // Retrieve smac + return _z_get_smac_raweth(&self->_socket._raweth); +} + +static int8_t _z_f_link_listen_raweth(_z_link_t *self) { return _z_f_link_open_raweth(self); } + +static void _z_f_link_close_raweth(_z_link_t *self) { _ZP_UNUSED(self); } + +static void _z_f_link_free_raweth(_z_link_t *self) { _ZP_UNUSED(self); } + +static size_t _z_f_link_write_raweth(const _z_link_t *self, const uint8_t *ptr, size_t len) { + _ZP_UNUSED(self); + _ZP_UNUSED(ptr); + _ZP_UNUSED(len); + return SIZE_MAX; +} + +static size_t _z_f_link_write_all_raweth(const _z_link_t *self, const uint8_t *ptr, size_t len) { + _ZP_UNUSED(self); + _ZP_UNUSED(ptr); + _ZP_UNUSED(len); + return SIZE_MAX; +} + +static size_t _z_f_link_read_raweth(const _z_link_t *self, uint8_t *ptr, size_t len, _z_bytes_t *addr) { + _ZP_UNUSED(self); + _ZP_UNUSED(ptr); + _ZP_UNUSED(len); + _ZP_UNUSED(addr); + return SIZE_MAX; +} + +static size_t _z_f_link_read_exact_raweth(const _z_link_t *self, uint8_t *ptr, size_t len, _z_bytes_t *addr) { + _ZP_UNUSED(self); + _ZP_UNUSED(ptr); + _ZP_UNUSED(len); + _ZP_UNUSED(addr); + return SIZE_MAX; +} + +static uint16_t _z_get_link_mtu_raweth(void) { + return _ZP_MAX_ETH_FRAME_SIZE; +} + +int8_t _z_endpoint_raweth_valid(_z_endpoint_t *endpoint) { + int8_t ret = _Z_RES_OK; + + // Check the root + if (!_z_str_eq(endpoint->_locator._protocol, RAWETH_SCHEMA)) { + ret = _Z_ERR_CONFIG_LOCATOR_INVALID; + } + // Check address + if (!__z_valid_address_raweth(endpoint->_locator._address)) { + ret = _Z_ERR_CONFIG_LOCATOR_INVALID; + } + return ret; +} + +int8_t _z_new_link_raweth(_z_link_t *zl, _z_endpoint_t endpoint) { + int8_t ret = _Z_RES_OK; + + zl->_cap._transport = Z_LINK_CAP_TRANSPORT_RAWETH; + zl->_cap._is_reliable = false; + zl->_mtu = _z_get_link_mtu_raweth(); + + zl->_endpoint = endpoint; + + // Init socket + memset(&zl->_socket._raweth, 0, sizeof(zl->_socket._raweth)); + + // Note locator address + uint8_t *b_address = __z_parse_address_raweth(endpoint._locator._address); + memcpy(zl->_socket._raweth._dmac, b_address, _ZP_MAC_ADDR_LENGTH); + z_free(b_address); + + zl->_open_f = _z_f_link_open_raweth; + zl->_listen_f = _z_f_link_listen_raweth; + zl->_close_f = _z_f_link_close_raweth; + zl->_free_f = _z_f_link_free_raweth; + + zl->_write_f = _z_f_link_write_raweth; + zl->_write_all_f = _z_f_link_write_all_raweth; + zl->_read_f = _z_f_link_read_raweth; + zl->_read_exact_f = _z_f_link_read_exact_raweth; + + return ret; +} + +#endif diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c new file mode 100644 index 000000000..e919b817b --- /dev/null +++ b/src/transport/raweth/rx.c @@ -0,0 +1,339 @@ +// +// 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/link/rx.h" + +#include + +#include "zenoh-pico/config.h" +#include "zenoh-pico/protocol/codec/network.h" +#include "zenoh-pico/protocol/codec/transport.h" +#include "zenoh-pico/protocol/core.h" +#include "zenoh-pico/protocol/iobuf.h" +#include "zenoh-pico/session/utils.h" +#include "zenoh-pico/transport/utils.h" +#include "zenoh-pico/utils/logging.h" + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +_z_transport_peer_entry_t *_z_find_peer_entry(_z_transport_peer_entry_list_t *l, _z_bytes_t *remote_addr) { + _z_transport_peer_entry_t *ret = NULL; + + _z_transport_peer_entry_list_t *xs = l; + for (; xs != NULL; xs = _z_transport_peer_entry_list_tail(xs)) { + _z_transport_peer_entry_t *val = _z_transport_peer_entry_list_head(xs); + if (val->_remote_addr.len != remote_addr->len) { + continue; + } + + if (memcmp(val->_remote_addr.start, remote_addr->start, remote_addr->len) == 0) { + ret = val; + } + } + + return ret; +} + +/*------------------ Reception helper ------------------*/ +int8_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr) { + _Z_DEBUG(">> recv session msg\n"); + int8_t ret = _Z_RES_OK; + +#if Z_FEATURE_MULTI_THREAD == 1 + // Acquire the lock + _z_mutex_lock(&ztm->_mutex_rx); +#endif // Z_FEATURE_MULTI_THREAD == 1 + + size_t to_read = 0; + do { + switch (ztm->_link._cap._flow) { + case Z_LINK_CAP_FLOW_STREAM: + if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); + if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_zbuf_compact(&ztm->_zbuf); + ret = _Z_ERR_TRANSPORT_NOT_ENOUGH_BYTES; + break; + } + } + for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) { + to_read |= _z_zbuf_read(&ztm->_zbuf) << (i * (uint8_t)8); + } + if (_z_zbuf_len(&ztm->_zbuf) < to_read) { + _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); + if (_z_zbuf_len(&ztm->_zbuf) < to_read) { + _z_zbuf_set_rpos(&ztm->_zbuf, _z_zbuf_get_rpos(&ztm->_zbuf) - _Z_MSG_LEN_ENC_SIZE); + _z_zbuf_compact(&ztm->_zbuf); + ret = _Z_ERR_TRANSPORT_NOT_ENOUGH_BYTES; + break; + } + } + break; + // Datagram capable links + case Z_LINK_CAP_FLOW_DATAGRAM: + _z_zbuf_compact(&ztm->_zbuf); + to_read = _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); + if (to_read == SIZE_MAX) { + ret = _Z_ERR_TRANSPORT_RX_FAILED; + } + break; + default: + break; + } + } while (false); // The 1-iteration loop to use continue to break the entire loop on error + + if (ret == _Z_RES_OK) { + _Z_DEBUG(">> \t transport_message_decode: %ju\n", (uintmax_t)_z_zbuf_len(&ztm->_zbuf)); + ret = _z_transport_message_decode(t_msg, &ztm->_zbuf); + } + +#if Z_FEATURE_MULTI_THREAD == 1 + _z_mutex_unlock(&ztm->_mutex_rx); +#endif // Z_FEATURE_MULTI_THREAD == 1 + + return ret; +} + +int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr) { + return _z_raweth_recv_t_msg_na(ztm, t_msg, addr); +} + +int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, + _z_bytes_t *addr) { + int8_t ret = _Z_RES_OK; +#if Z_FEATURE_MULTI_THREAD == 1 + // Acquire and keep the lock + _z_mutex_lock(&ztm->_mutex_peer); +#endif // Z_FEATURE_MULTI_THREAD == 1 + + // Mark the session that we have received data from this peer + _z_transport_peer_entry_t *entry = _z_find_peer_entry(ztm->_peers, addr); + switch (_Z_MID(t_msg->_header)) { + case _Z_MID_T_FRAME: { + _Z_INFO("Received _Z_FRAME message\n"); + if (entry == NULL) { + break; + } + entry->_received = true; + + // Check if the SN is correct + if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAME_R) == true) { + // @TODO: amend once reliability is in place. For the time being only + // monotonic SNs are ensured + if (_z_sn_precedes(entry->_sn_res, entry->_sn_rx_sns._val._plain._reliable, t_msg->_body._frame._sn) == + true) { + entry->_sn_rx_sns._val._plain._reliable = t_msg->_body._frame._sn; + } else { + _z_wbuf_clear(&entry->_dbuf_reliable); + _Z_INFO("Reliable message dropped because it is out of order\n"); + break; + } + } else { + if (_z_sn_precedes(entry->_sn_res, entry->_sn_rx_sns._val._plain._best_effort, + t_msg->_body._frame._sn) == true) { + entry->_sn_rx_sns._val._plain._best_effort = t_msg->_body._frame._sn; + } else { + _z_wbuf_clear(&entry->_dbuf_best_effort); + _Z_INFO("Best effort message dropped because it is out of order\n"); + break; + } + } + + // Handle all the zenoh message, one by one + uint16_t mapping = entry->_peer_id; + size_t len = _z_vec_len(&t_msg->_body._frame._messages); + for (size_t i = 0; i < len; i++) { + _z_network_message_t *zm = _z_network_message_vec_get(&t_msg->_body._frame._messages, i); + _z_msg_fix_mapping(zm, mapping); + _z_handle_network_message(ztm->_session, zm, mapping); + } + + break; + } + + case _Z_MID_T_FRAGMENT: { + _Z_INFO("Received Z_FRAGMENT message\n"); + if (entry == NULL) { + break; + } + entry->_received = true; + + _z_wbuf_t *dbuf = _Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAGMENT_R) + ? &entry->_dbuf_reliable + : &entry->_dbuf_best_effort; // Select the right defragmentation buffer + + _Bool drop = false; + if ((_z_wbuf_len(dbuf) + t_msg->_body._fragment._payload.len) > Z_FRAG_MAX_SIZE) { + // Filling the wbuf capacity as a way to signaling the last fragment to reset the dbuf + // Otherwise, last (smaller) fragments can be understood as a complete message + _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, _z_wbuf_space_left(dbuf)); + drop = true; + } else { + _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, + t_msg->_body._fragment._payload.len); + } + + if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAGMENT_M) == false) { + if (drop == true) { // Drop message if it exceeds the fragmentation size + _z_wbuf_reset(dbuf); + break; + } + + _z_zbuf_t zbf = _z_wbuf_to_zbuf(dbuf); // Convert the defragmentation buffer into a decoding buffer + + _z_zenoh_message_t zm; + ret = _z_network_message_decode(&zm, &zbf); + if (ret == _Z_RES_OK) { + uint16_t mapping = entry->_peer_id; + _z_msg_fix_mapping(&zm, mapping); + _z_handle_network_message(ztm->_session, &zm, mapping); + _z_msg_clear(&zm); // Clear must be explicitly called for fragmented zenoh messages. Non-fragmented + // zenoh messages are released when their transport message is released. + } + + // Free the decoding buffer + _z_zbuf_clear(&zbf); + // Reset the defragmentation buffer + _z_wbuf_reset(dbuf); + } + break; + } + + case _Z_MID_T_KEEP_ALIVE: { + _Z_INFO("Received _Z_KEEP_ALIVE message\n"); + if (entry == NULL) { + break; + } + entry->_received = true; + + break; + } + + case _Z_MID_T_INIT: { + // Do nothing, multicast transports are not expected to handle INIT messages + break; + } + + case _Z_MID_T_OPEN: { + // Do nothing, multicast transports are not expected to handle OPEN messages + break; + } + + case _Z_MID_T_JOIN: { + _Z_INFO("Received _Z_JOIN message\n"); + if (t_msg->_body._join._version != Z_PROTO_VERSION) { + break; + } + + if (entry == NULL) // New peer + { + entry = (_z_transport_peer_entry_t *)z_malloc(sizeof(_z_transport_peer_entry_t)); + if (entry != NULL) { + entry->_sn_res = _z_sn_max(t_msg->_body._join._seq_num_res); + + // If the new node has less representing capabilities then it is incompatible to communication + if ((t_msg->_body._join._seq_num_res != Z_SN_RESOLUTION) || + (t_msg->_body._join._req_id_res != Z_REQ_RESOLUTION) || + (t_msg->_body._join._batch_size != Z_BATCH_MULTICAST_SIZE)) { + ret = _Z_ERR_TRANSPORT_OPEN_SN_RESOLUTION; + } + + if (ret == _Z_RES_OK) { + entry->_remote_addr = _z_bytes_duplicate(addr); + entry->_remote_zid = t_msg->_body._join._zid; + + _z_conduit_sn_list_copy(&entry->_sn_rx_sns, &t_msg->_body._join._next_sn); + _z_conduit_sn_list_decrement(entry->_sn_res, &entry->_sn_rx_sns); + +#if Z_FEATURE_DYNAMIC_MEMORY_ALLOCATION == 1 + entry->_dbuf_reliable = _z_wbuf_make(0, true); + entry->_dbuf_best_effort = _z_wbuf_make(0, true); +#else + entry->_dbuf_reliable = _z_wbuf_make(Z_FRAG_MAX_SIZE, false); + entry->_dbuf_best_effort = _z_wbuf_make(Z_FRAG_MAX_SIZE, false); +#endif + + // Update lease time (set as ms during) + entry->_lease = t_msg->_body._join._lease; + entry->_next_lease = entry->_lease; + entry->_received = true; + + ztm->_peers = _z_transport_peer_entry_list_insert(ztm->_peers, entry); + } else { + z_free(entry); + } + } else { + ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + } else { // Existing peer + entry->_received = true; + + // Check if the representing capabilities are still the same + if ((t_msg->_body._join._seq_num_res != Z_SN_RESOLUTION) || + (t_msg->_body._join._req_id_res != Z_REQ_RESOLUTION) || + (t_msg->_body._join._batch_size != Z_BATCH_MULTICAST_SIZE)) { + _z_transport_peer_entry_list_drop_filter(ztm->_peers, _z_transport_peer_entry_eq, entry); + // TODO: cleanup here should also be done on mappings/subs/etc... + break; + } + + // Update SNs + _z_conduit_sn_list_copy(&entry->_sn_rx_sns, &t_msg->_body._join._next_sn); + _z_conduit_sn_list_decrement(entry->_sn_res, &entry->_sn_rx_sns); + + // Update lease time (set as ms during) + entry->_lease = t_msg->_body._join._lease; + } + break; + } + + case _Z_MID_T_CLOSE: { + _Z_INFO("Closing session as requested by the remote peer\n"); + + if (entry == NULL) { + break; + } + ztm->_peers = _z_transport_peer_entry_list_drop_filter(ztm->_peers, _z_transport_peer_entry_eq, entry); + + break; + } + + default: { + _Z_ERROR("Unknown session message ID\n"); + break; + } + } + +#if Z_FEATURE_MULTI_THREAD == 1 + _z_mutex_unlock(&ztm->_mutex_peer); +#endif // Z_FEATURE_MULTI_THREAD == 1 + + return ret; +} +#else +int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr) { + _ZP_UNUSED(ztm); + _ZP_UNUSED(t_msg); + _ZP_UNUSED(addr); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, + _z_bytes_t *addr) { + _ZP_UNUSED(ztm); + _ZP_UNUSED(t_msg); + _ZP_UNUSED(addr); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 \ No newline at end of file diff --git a/src/transport/raweth/transport.c b/src/transport/raweth/transport.c new file mode 100644 index 000000000..f920deeed --- /dev/null +++ b/src/transport/raweth/transport.c @@ -0,0 +1,225 @@ +// +// 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 +#include +#include +#include +#include + +#include "zenoh-pico/link/link.h" +#include "zenoh-pico/transport/common/lease.h" +#include "zenoh-pico/transport/common/read.h" +#include "zenoh-pico/transport/common/tx.h" +#include "zenoh-pico/transport/raweth/rx.h" +#include "zenoh-pico/transport/raweth/tx.h" +#include "zenoh-pico/transport/utils.h" +#include "zenoh-pico/utils/logging.h" + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +int8_t _z_raweth_transport_create(_z_transport_t *zt, _z_link_t *zl, _z_transport_multicast_establish_param_t *param) { + int8_t ret = _Z_RES_OK; + + zt->_type = _Z_TRANSPORT_RAWETH_TYPE; + +#if Z_FEATURE_MULTI_THREAD == 1 + // Initialize the mutexes + ret = _z_mutex_init(&zt->_transport._raweth._mutex_tx); + if (ret == _Z_RES_OK) { + ret = _z_mutex_init(&zt->_transport._raweth._mutex_rx); + if (ret == _Z_RES_OK) { + ret = _z_mutex_init(&zt->_transport._raweth._mutex_peer); + if (ret != _Z_RES_OK) { + _z_mutex_free(&zt->_transport._raweth._mutex_tx); + _z_mutex_free(&zt->_transport._raweth._mutex_rx); + } + } else { + _z_mutex_free(&zt->_transport._raweth._mutex_tx); + } + } +#endif // Z_FEATURE_MULTI_THREAD == 1 + + // Initialize the read and write buffers + if (ret == _Z_RES_OK) { + uint16_t mtu = (zl->_mtu < Z_BATCH_MULTICAST_SIZE) ? zl->_mtu : Z_BATCH_MULTICAST_SIZE; + zt->_transport._raweth._wbuf = _z_wbuf_make(mtu, false); + zt->_transport._raweth._zbuf = _z_zbuf_make(Z_BATCH_MULTICAST_SIZE); + + // Clean up the buffers if one of them failed to be allocated + if ((_z_wbuf_capacity(&zt->_transport._raweth._wbuf) != mtu) || + (_z_zbuf_capacity(&zt->_transport._raweth._zbuf) != Z_BATCH_MULTICAST_SIZE)) { + ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; + +#if Z_FEATURE_MULTI_THREAD == 1 + _z_mutex_free(&zt->_transport._raweth._mutex_tx); + _z_mutex_free(&zt->_transport._raweth._mutex_rx); + _z_mutex_free(&zt->_transport._raweth._mutex_peer); +#endif // Z_FEATURE_MULTI_THREAD == 1 + + _z_wbuf_clear(&zt->_transport._raweth._wbuf); + _z_zbuf_clear(&zt->_transport._raweth._zbuf); + } + } + + if (ret == _Z_RES_OK) { + // Set default SN resolution + zt->_transport._raweth._sn_res = _z_sn_max(param->_seq_num_res); + + // The initial SN at TX side + zt->_transport._raweth._sn_tx_reliable = param->_initial_sn_tx._val._plain._reliable; + zt->_transport._raweth._sn_tx_best_effort = param->_initial_sn_tx._val._plain._best_effort; + + // Initialize peer list + zt->_transport._raweth._peers = _z_transport_peer_entry_list_new(); + +#if Z_FEATURE_MULTI_THREAD == 1 + // Tasks + zt->_transport._raweth._read_task_running = false; + zt->_transport._raweth._read_task = NULL; + zt->_transport._raweth._lease_task_running = false; + zt->_transport._raweth._lease_task = NULL; +#endif // Z_FEATURE_MULTI_THREAD == 1 + + zt->_transport._raweth._lease = Z_TRANSPORT_LEASE; + + // Notifiers + zt->_transport._raweth._transmitted = false; + + // Transport link for raweth + zt->_transport._raweth._link = *zl; + } + + return ret; +} + +int8_t _z_raweth_open_peer(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid) { + int8_t ret = _Z_RES_OK; + + _z_zint_t initial_sn_tx = 0; + z_random_fill(&initial_sn_tx, sizeof(initial_sn_tx)); + initial_sn_tx = initial_sn_tx & !_z_sn_modulo_mask(Z_SN_RESOLUTION); + + _z_conduit_sn_list_t next_sn; + next_sn._is_qos = false; + next_sn._val._plain._best_effort = initial_sn_tx; + next_sn._val._plain._reliable = initial_sn_tx; + + _z_id_t zid = *local_zid; + _z_transport_message_t jsm = _z_t_msg_make_join(Z_WHATAMI_PEER, Z_TRANSPORT_LEASE, zid, next_sn); + + // Encode and send the message + _Z_INFO("Sending Z_JOIN message\n"); + ret = _z_link_send_t_msg(zl, &jsm); + _z_t_msg_clear(&jsm); + + if (ret == _Z_RES_OK) { + param->_seq_num_res = jsm._body._join._seq_num_res; + param->_initial_sn_tx = next_sn; + } + + return ret; +} + +int8_t _z_raweth_open_client(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid) { + _ZP_UNUSED(param); + _ZP_UNUSED(zl); + _ZP_UNUSED(local_zid); + int8_t ret = _Z_ERR_CONFIG_UNSUPPORTED_CLIENT_MULTICAST; + // @TODO: not implemented + return ret; +} + +int8_t _z_raweth_send_close(_z_transport_multicast_t *ztm, uint8_t reason, _Bool link_only) { + int8_t ret = _Z_RES_OK; + // Send and clear message + _z_transport_message_t cm = _z_t_msg_make_close(reason, link_only); + ret = _z_raweth_send_t_msg(ztm, &cm); + _z_t_msg_clear(&cm); + return ret; +} + +int8_t _z_raweth_transport_close(_z_transport_multicast_t *ztm, uint8_t reason) { + return _z_raweth_send_close(ztm, reason, false); +} + +void _z_raweth_transport_clear(_z_transport_t *zt) { + _z_transport_multicast_t *ztm = &zt->_transport._raweth; +#if Z_FEATURE_MULTI_THREAD == 1 + // Clean up tasks + if (ztm->_read_task != NULL) { + _z_task_join(ztm->_read_task); + _z_task_free(&ztm->_read_task); + } + if (ztm->_lease_task != NULL) { + _z_task_join(ztm->_lease_task); + _z_task_free(&ztm->_lease_task); + } + // Clean up the mutexes + _z_mutex_free(&ztm->_mutex_tx); + _z_mutex_free(&ztm->_mutex_rx); + _z_mutex_free(&ztm->_mutex_peer); +#endif // Z_FEATURE_MULTI_THREAD == 1 + + // Clean up the buffers + _z_wbuf_clear(&ztm->_wbuf); + _z_zbuf_clear(&ztm->_zbuf); + + // Clean up peer list + _z_transport_peer_entry_list_free(&ztm->_peers); + _z_link_clear(&ztm->_link); +} + +#else + +int8_t _z_raweth_transport_create(_z_transport_t *zt, _z_link_t *zl, _z_transport_multicast_establish_param_t *param) { + _ZP_UNUSED(zt); + _ZP_UNUSED(zl); + _ZP_UNUSED(param); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _z_raweth_open_peer(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid) { + _ZP_UNUSED(param); + _ZP_UNUSED(zl); + _ZP_UNUSED(local_zid); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _z_raweth_open_client(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid) { + _ZP_UNUSED(param); + _ZP_UNUSED(zl); + _ZP_UNUSED(local_zid); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _z_raweth_send_close(_z_transport_multicast_t *ztm, uint8_t reason, _Bool link_only) { + _ZP_UNUSED(ztm); + _ZP_UNUSED(reason); + _ZP_UNUSED(link_only); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _z_raweth_transport_close(_z_transport_multicast_t *ztm, uint8_t reason) { + _ZP_UNUSED(ztm); + _ZP_UNUSED(reason); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; + ; +} + +void _z_raweth_transport_clear(_z_transport_t *zt) { _ZP_UNUSED(zt); } +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c new file mode 100644 index 000000000..54eab34d3 --- /dev/null +++ b/src/transport/raweth/tx.c @@ -0,0 +1,247 @@ +// +// 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/link/tx.h" + +#include "zenoh-pico/transport/common/tx.h" + +#include "zenoh-pico/config.h" +#include "zenoh-pico/protocol/codec/core.h" +#include "zenoh-pico/protocol/codec/network.h" +#include "zenoh-pico/protocol/codec/transport.h" +#include "zenoh-pico/protocol/iobuf.h" +#include "zenoh-pico/system/link/raweth.h" +#include "zenoh-pico/transport/transport.h" +#include "zenoh-pico/transport/utils.h" +#include "zenoh-pico/utils/logging.h" + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +/** + * This function is unsafe because it operates in potentially concurrent data. + * Make sure that the following mutexes are locked before calling this function: + * - ztm->_mutex_inner + */ +static _z_zint_t __unsafe_z_raweth_get_sn(_z_transport_multicast_t *ztm, z_reliability_t reliability) { + _z_zint_t sn; + if (reliability == Z_RELIABILITY_RELIABLE) { + sn = ztm->_sn_tx_reliable; + ztm->_sn_tx_reliable = _z_sn_increment(ztm->_sn_res, ztm->_sn_tx_reliable); + } else { + sn = ztm->_sn_tx_best_effort; + ztm->_sn_tx_best_effort = _z_sn_increment(ztm->_sn_res, ztm->_sn_tx_best_effort); + } + return sn; +} + +/** + * This function is unsafe because it operates in potentially concurrent data. + * Make sure that the following mutexes are locked before calling this function: + * - ztm->_mutex_inner + */ +static void __unsafe_z_raweth_prepare_header(_z_transport_multicast_t *ztm) { + // Reserve space for ethernet header + if (&ztm->_link._socket._raweth.has_vlan) { + _z_wbuf_set_wpos(&ztm->_wbuf, sizeof(_zp_eth_vlan_header_t)); + } else { + _z_wbuf_set_wpos(&ztm->_wbuf, sizeof(_zp_eth_header_t)); + } +} + +/** + * This function is unsafe because it operates in potentially concurrent data. + * Make sure that the following mutexes are locked before calling this function: + * - ztm->_mutex_inner + */ +static int8_t __unsafe_z_raweth_write_header(_z_transport_multicast_t *ztm) { + size_t len = _z_wbuf_len(&ztm->_wbuf); + _z_raweth_socket_t *resocket = &ztm->_link._socket._raweth; + + // FIXME config function call to set the correct dmac & vlan value + // Write eth header in buffer + if (resocket->has_vlan) { + _zp_eth_vlan_header_t header; + memset(&header, 0, sizeof(header)); + memcpy(&header.dmac, &resocket->_dmac, _ZP_MAC_ADDR_LENGTH); + memcpy(&header.smac, &resocket->_smac, _ZP_MAC_ADDR_LENGTH); + header.tag.tpid = _ZP_ETH_TYPE_VLAN; + header.tag.pcp = 0; // Get from locator/config + header.tag.vid = 0; // Get from locator/config + header.length = (uint16_t)len; + // Save wpos + size_t final_wpos = _z_wbuf_get_wpos(&ztm->_wbuf); + // Write header + _z_wbuf_set_wpos(&ztm->_wbuf, 0); + _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(&ztm->_wbuf, (uint8_t *)&header, 0, sizeof(header))); + // Restore wpos + _z_wbuf_set_wpos(&ztm->_wbuf, final_wpos); + } else { + _zp_eth_header_t header; + memcpy(&header.dmac, &resocket->_dmac, _ZP_MAC_ADDR_LENGTH); + memcpy(&header.smac, &resocket->_smac, _ZP_MAC_ADDR_LENGTH); + header.length = (uint16_t)len; + // Save wpos + size_t final_wpos = _z_wbuf_get_wpos(&ztm->_wbuf); + // Write header + _z_wbuf_set_wpos(&ztm->_wbuf, 0); + _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(&ztm->_wbuf, (uint8_t *)&header, 0, sizeof(header))); + // Restore wpos + _z_wbuf_set_wpos(&ztm->_wbuf, final_wpos); + } + return _Z_RES_OK; +} + +static int8_t _z_raweth_link_send_wbuf(const _z_transport_multicast_t *ztm) { + int8_t ret = _Z_RES_OK; + for (size_t i = 0; (i < _z_wbuf_len_iosli(&ztm->_wbuf)) && (ret == _Z_RES_OK); i++) { + _z_bytes_t bs = _z_iosli_to_bytes(_z_wbuf_get_iosli(&ztm->_wbuf, i)); + size_t n = bs.len; + + do { + // Retrieve addr from config + vlan tag above (locator) + size_t wb = _z_send_raweth(&ztm->_link._socket._raweth._sock, bs.start, n); // Unix + if (wb == SIZE_MAX) { + return _Z_ERR_TRANSPORT_TX_FAILED; + } + n = n - wb; + bs.start = bs.start + (bs.len - n); + } while (n > (size_t)0); + } + return ret; +} + +// static void _z_raweth_check_config(_z_transport_multicast_t *ztm, const _z_keyexpr_t *key) { +// // Check config +// if (ztm->_link._socket._raweth._config != NULL) { +// // Check key +// if ((key != NULL)) { +// // Get send info from config keyexpr mapping +// } else { +// // Get send info from config default values +// } +// } else { +// // Nothing to do +// } +// } + +int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg) { + int8_t ret = _Z_RES_OK; + _Z_DEBUG(">> send session message\n"); + +#if Z_FEATURE_MULTI_THREAD == 1 + _z_mutex_lock(&ztm->_mutex_tx); +#endif + + // Reserve space for eth header + __unsafe_z_raweth_prepare_header(ztm); + // Encode the session message + _Z_RETURN_IF_ERR(_z_transport_message_encode(&ztm->_wbuf, t_msg)); + // Write the message header + _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); + // Send the wbuf on the socket + _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(ztm)); + // Mark the session that we have transmitted data + ztm->_transmitted = true; + +#if Z_FEATURE_MULTI_THREAD == 1 + _z_mutex_unlock(&ztm->_mutex_tx); +#endif + + return ret; +} + +int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, z_reliability_t reliability, + z_congestion_control_t cong_ctrl) { + int8_t ret = _Z_RES_OK; + _z_transport_multicast_t *ztm = &zn->_tp._transport._raweth; + _Z_DEBUG(">> send network message\n"); + + // Acquire the lock and drop the message if needed +#if Z_FEATURE_MULTI_THREAD == 1 + if (cong_ctrl == Z_CONGESTION_CONTROL_BLOCK) { + _z_mutex_lock(&ztm->_mutex_tx); + } else { + if (_z_mutex_trylock(&ztm->_mutex_tx) != (int8_t)0) { + _Z_INFO("Dropping zenoh message because of congestion control\n"); + // We failed to acquire the lock, drop the message + return ret; + } + } +#else + _ZP_UNUSED(cong_ctrl); +#endif // Z_FEATURE_MULTI_THREAD == 1 + + // Reserve space for eth header + __unsafe_z_raweth_prepare_header(ztm); + // Set the frame header + _z_zint_t sn = __unsafe_z_raweth_get_sn(ztm, reliability); + _z_transport_message_t t_msg = _z_t_msg_make_frame_header(sn, reliability); + // Encode the frame header + _Z_RETURN_IF_ERR(_z_transport_message_encode(&ztm->_wbuf, &t_msg)); + // Encode the network message + ret = _z_network_message_encode(&ztm->_wbuf, n_msg); + if (ret == _Z_RES_OK) { + // Write the eth header + _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); + // Send the wbuf on the socket + _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(ztm)); + // Mark the session that we have transmitted data + ztm->_transmitted = true; + } else { // The message does not fit in the current batch, let's fragment it + // Create an expandable wbuf for fragmentation + _z_wbuf_t fbf = _z_wbuf_make(ztm->_wbuf._capacity - 12, true); + // Encode the message on the expandable wbuf + _Z_RETURN_IF_ERR(_z_network_message_encode(&fbf, n_msg)); + // Fragment and send the message + _Bool is_first = true; + while (_z_wbuf_len(&fbf) > 0) { + if (is_first) { + // Get the fragment sequence number + sn = __unsafe_z_raweth_get_sn(ztm, reliability); + } + is_first = false; + // Reserve space for eth header + __unsafe_z_raweth_prepare_header(ztm); + // Serialize one fragment + _Z_RETURN_IF_ERR(__unsafe_z_serialize_zenoh_fragment(&ztm->_wbuf, &fbf, reliability, sn)); + // Write the eth header + _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); + // Send the wbuf on the socket + _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(ztm)); + // Mark the session that we have transmitted data + ztm->_transmitted = true; + } + } +#if Z_FEATURE_MULTI_THREAD == 1 + _z_mutex_unlock(&ztm->_mutex_tx); +#endif // Z_FEATURE_MULTI_THREAD == 1 + return ret; +} + +#else +int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg) { + _ZP_UNUSED(ztm); + _ZP_UNUSED(t_msg); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, z_reliability_t reliability, + z_congestion_control_t cong_ctrl) { + _ZP_UNUSED(zn); + _ZP_UNUSED(n_msg); + _ZP_UNUSED(reliability); + _ZP_UNUSED(cong_ctrl); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 \ No newline at end of file From 289a0ee4d79047ee8bbe04de6921459c65e5aacc Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Wed, 22 Nov 2023 16:52:20 +0100 Subject: [PATCH 03/38] fix: remove obsolete transport function declaration --- include/zenoh-pico/transport/multicast/tx.h | 2 -- include/zenoh-pico/transport/unicast/tx.h | 2 -- 2 files changed, 4 deletions(-) diff --git a/include/zenoh-pico/transport/multicast/tx.h b/include/zenoh-pico/transport/multicast/tx.h index d8171b719..6b48266db 100644 --- a/include/zenoh-pico/transport/multicast/tx.h +++ b/include/zenoh-pico/transport/multicast/tx.h @@ -18,8 +18,6 @@ #include "zenoh-pico/net/session.h" #include "zenoh-pico/transport/transport.h" -int8_t _z_multicast_send_z_msg(_z_session_t *zn, _z_zenoh_message_t *z_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl); int8_t _z_multicast_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, z_congestion_control_t cong_ctrl); int8_t _z_multicast_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg); diff --git a/include/zenoh-pico/transport/unicast/tx.h b/include/zenoh-pico/transport/unicast/tx.h index ed42e0c73..d087dd696 100644 --- a/include/zenoh-pico/transport/unicast/tx.h +++ b/include/zenoh-pico/transport/unicast/tx.h @@ -18,8 +18,6 @@ #include "zenoh-pico/net/session.h" #include "zenoh-pico/transport/transport.h" -int8_t _z_unicast_send_z_msg(_z_session_t *zn, _z_zenoh_message_t *z_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl); int8_t _z_unicast_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, z_congestion_control_t cong_ctrl); int8_t _z_unicast_send_t_msg(_z_transport_unicast_t *ztu, const _z_transport_message_t *t_msg); From 820eb2c2abaae95512673f9c5092f66a1fcb44d0 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Thu, 23 Nov 2023 17:06:12 +0100 Subject: [PATCH 04/38] feat: add rx function --- include/zenoh-pico/system/link/raweth.h | 5 +++-- src/system/unix/link/raweth.c | 17 ++++++++++++++++- src/transport/raweth/rx.c | 11 +++++++++-- src/transport/raweth/tx.c | 4 ++-- 4 files changed, 30 insertions(+), 7 deletions(-) diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index ef2b3e38b..c00c65925 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -58,15 +58,16 @@ typedef struct { typedef struct { void *_config; // Pointer to config data _z_sys_net_socket_t _sock; - uint16_t _vlan_val; + uint16_t _vlan; uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; uint8_t _smac[_ZP_MAC_ADDR_LENGTH]; - _Bool has_vlan; + _Bool _has_vlan; } _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); 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); #endif diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index 448a6a62b..ebdbd82b5 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -12,6 +12,8 @@ // ZettaScale Zenoh Team, // +#include "zenoh-pico/system/link/raweth.h" + #include #include #include @@ -31,7 +33,6 @@ #include "zenoh-pico/collections/string.h" #include "zenoh-pico/config.h" #include "zenoh-pico/system/platform/unix.h" -#include "zenoh-pico/system/link/raweth.h" #include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/pointers.h" @@ -105,4 +106,18 @@ 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 bytesRead = recvfrom(sock->_fd, buff, buff_len, 0, NULL, NULL); + if (bytesRead < 0) { + return SIZE_MAX; + } + // Soft Filtering ? + // Copy sender mac if needed + if ((addr != NULL) && (bytesRead > 2 * ETH_ALEN)) { + *addr = _z_bytes_make(sizeof(ETH_ALEN)); + (void)memcpy((uint8_t *)addr->start, &(buff + ETH_ALEN), sizeof(ETH_ALEN)); + } + return bytesRead; +} + #endif \ No newline at end of file diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c index e919b817b..ae8ada39e 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/rx.c @@ -27,6 +27,14 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 +static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z_bytes_t *addr) { + size_t rb = _z_receive_raweth(link->_socket._raweth._sock._fd, _z_zbuf_get_wptr(zbf), _z_zbuf_space_left(zbf), addr); + if (rb != SIZE_MAX) { + _z_zbuf_set_wpos(zbf, _z_zbuf_get_wpos(zbf) + rb); + } + return rb; +} + _z_transport_peer_entry_t *_z_find_peer_entry(_z_transport_peer_entry_list_t *l, _z_bytes_t *remote_addr) { _z_transport_peer_entry_t *ret = NULL; @@ -41,7 +49,6 @@ _z_transport_peer_entry_t *_z_find_peer_entry(_z_transport_peer_entry_list_t *l, ret = val; } } - return ret; } @@ -60,7 +67,7 @@ int8_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_messa switch (ztm->_link._cap._flow) { case Z_LINK_CAP_FLOW_STREAM: if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); + _z_raweth_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { _z_zbuf_compact(&ztm->_zbuf); ret = _Z_ERR_TRANSPORT_NOT_ENOUGH_BYTES; diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index 54eab34d3..c5dd1c5c7 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -52,7 +52,7 @@ static _z_zint_t __unsafe_z_raweth_get_sn(_z_transport_multicast_t *ztm, z_relia */ static void __unsafe_z_raweth_prepare_header(_z_transport_multicast_t *ztm) { // Reserve space for ethernet header - if (&ztm->_link._socket._raweth.has_vlan) { + if (&ztm->_link._socket._raweth._has_vlan) { _z_wbuf_set_wpos(&ztm->_wbuf, sizeof(_zp_eth_vlan_header_t)); } else { _z_wbuf_set_wpos(&ztm->_wbuf, sizeof(_zp_eth_header_t)); @@ -70,7 +70,7 @@ static int8_t __unsafe_z_raweth_write_header(_z_transport_multicast_t *ztm) { // FIXME config function call to set the correct dmac & vlan value // Write eth header in buffer - if (resocket->has_vlan) { + if (resocket->_has_vlan) { _zp_eth_vlan_header_t header; memset(&header, 0, sizeof(header)); memcpy(&header.dmac, &resocket->_dmac, _ZP_MAC_ADDR_LENGTH); From 6349597e1b90d944fb08bdec4223e7a556b19945 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Thu, 23 Nov 2023 17:10:05 +0100 Subject: [PATCH 05/38] fix: remove raweth by default --- include/zenoh-pico/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/zenoh-pico/config.h b/include/zenoh-pico/config.h index 35d19ec38..c1bb5ad83 100644 --- a/include/zenoh-pico/config.h +++ b/include/zenoh-pico/config.h @@ -222,7 +222,7 @@ * Enable raweth transport/link. */ #ifndef Z_FEATURE_RAWETH_TRANSPORT -#define Z_FEATURE_RAWETH_TRANSPORT 1 +#define Z_FEATURE_RAWETH_TRANSPORT 0 #endif /*------------------ Compile-time configuration properties ------------------*/ From 37a8e1f137a61cc0f56e1a2c5a1f1fb20611fff6 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 10:12:38 +0100 Subject: [PATCH 06/38] build: add unix link folder --- CMakeLists.txt | 2 +- src/system/unix/link/raweth.c | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ac9c77c5d..73db9fa37 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -186,7 +186,7 @@ elseif(WITH_FREERTOS_PLUS_TCP) file (GLOB Sources_Freertos_Plus_TCP "src/system/freertos_plus_tcp/*.c") list(APPEND Sources ${Sources_Freertos_Plus_TCP}) elseif(CMAKE_SYSTEM_NAME MATCHES "Linux" OR CMAKE_SYSTEM_NAME MATCHES "Darwin" OR CMAKE_SYSTEM_NAME MATCHES "BSD" OR POSIX_COMPATIBLE) - file (GLOB Sources_Unix "src/system/unix/*.c") + file (GLOB Sources_Unix "src/system/unix/*.c" "src/system/unix/link/*.c") list(APPEND Sources ${Sources_Unix}) elseif(CMAKE_SYSTEM_NAME MATCHES "Emscripten") file (GLOB Sources_Emscripten "src/system/emscripten/*.c") diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index ebdbd82b5..ee0bca0eb 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -115,8 +115,9 @@ size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buf // Copy sender mac if needed if ((addr != NULL) && (bytesRead > 2 * ETH_ALEN)) { *addr = _z_bytes_make(sizeof(ETH_ALEN)); - (void)memcpy((uint8_t *)addr->start, &(buff + ETH_ALEN), sizeof(ETH_ALEN)); + (void)memcpy((uint8_t *)addr->start, (buff + ETH_ALEN), sizeof(ETH_ALEN)); } + // Dump ethernet header ? return bytesRead; } From 66da3b7f154a2ec738e0178b0c46e7960cfa43d6 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 11:05:51 +0100 Subject: [PATCH 07/38] fix: remove unix header --- include/zenoh-pico/system/link/raweth.h | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index c00c65925..8f675f3ab 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -15,10 +15,7 @@ #ifndef ZENOH_PICO_SYSTEM_LINK_RAWETH_H #define ZENOH_PICO_SYSTEM_LINK_RAWETH_H -#include -#include #include -#include #include "zenoh-pico/collections/string.h" #include "zenoh-pico/system/platform.h" @@ -45,7 +42,7 @@ typedef struct { typedef struct { uint8_t dmac[_ZP_MAC_ADDR_LENGTH]; // Destination mac address uint8_t smac[_ZP_MAC_ADDR_LENGTH]; // Source mac address - uint16_t length; // Size of frame + uint16_t length; // Size of frame } _zp_eth_header_t; typedef struct { @@ -56,7 +53,7 @@ typedef struct { } _zp_eth_vlan_header_t; typedef struct { - void *_config; // Pointer to config data + void *_config; // Pointer to config data _z_sys_net_socket_t _sock; uint16_t _vlan; uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; From b1296f997488f8bc0e771d1620f099e4f6a12e41 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 12:13:49 +0100 Subject: [PATCH 08/38] fix: run clang format --- include/zenoh-pico/transport/raweth/rx.h | 2 +- include/zenoh-pico/transport/raweth/transport.h | 7 +++---- include/zenoh-pico/transport/raweth/tx.h | 2 +- src/transport/raweth/link.c | 4 +--- 4 files changed, 6 insertions(+), 9 deletions(-) diff --git a/include/zenoh-pico/transport/raweth/rx.h b/include/zenoh-pico/transport/raweth/rx.h index 5e23c6c9d..c1412196a 100644 --- a/include/zenoh-pico/transport/raweth/rx.h +++ b/include/zenoh-pico/transport/raweth/rx.h @@ -20,6 +20,6 @@ int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr); int8_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr); int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, - _z_bytes_t *addr); + _z_bytes_t *addr); #endif /* ZENOH_PICO_RAWETH_RX_H */ diff --git a/include/zenoh-pico/transport/raweth/transport.h b/include/zenoh-pico/transport/raweth/transport.h index 6c747a217..a91eba18a 100644 --- a/include/zenoh-pico/transport/raweth/transport.h +++ b/include/zenoh-pico/transport/raweth/transport.h @@ -17,12 +17,11 @@ #include "zenoh-pico/api/types.h" -int8_t _z_raweth_transport_create(_z_transport_t *zt, _z_link_t *zl, - _z_transport_multicast_establish_param_t *param); +int8_t _z_raweth_transport_create(_z_transport_t *zt, _z_link_t *zl, _z_transport_multicast_establish_param_t *param); int8_t _z_raweth_open_peer(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, - const _z_id_t *local_zid); + const _z_id_t *local_zid); int8_t _z_raweth_open_client(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, - const _z_id_t *local_zid); + const _z_id_t *local_zid); int8_t _z_raweth_send_close(_z_transport_multicast_t *ztm, uint8_t reason, _Bool link_only); int8_t _z_raweth_transport_close(_z_transport_multicast_t *ztm, uint8_t reason); void _z_raweth_transport_clear(_z_transport_t *zt); diff --git a/include/zenoh-pico/transport/raweth/tx.h b/include/zenoh-pico/transport/raweth/tx.h index 39b4758b6..2595e78eb 100644 --- a/include/zenoh-pico/transport/raweth/tx.h +++ b/include/zenoh-pico/transport/raweth/tx.h @@ -19,7 +19,7 @@ #include "zenoh-pico/transport/transport.h" int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl); + z_congestion_control_t cong_ctrl); int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg); #endif /* ZENOH_PICO_RAWETH_TX_H */ diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index 1cadcd8f4..7da7f2ae0 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -133,9 +133,7 @@ static size_t _z_f_link_read_exact_raweth(const _z_link_t *self, uint8_t *ptr, s return SIZE_MAX; } -static uint16_t _z_get_link_mtu_raweth(void) { - return _ZP_MAX_ETH_FRAME_SIZE; -} +static uint16_t _z_get_link_mtu_raweth(void) { return _ZP_MAX_ETH_FRAME_SIZE; } int8_t _z_endpoint_raweth_valid(_z_endpoint_t *endpoint) { int8_t ret = _Z_RES_OK; From 26c101e791fd7c1d07c09a06133c27455ce77c98 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 12:14:13 +0100 Subject: [PATCH 09/38] feat: add eth header skip in rx --- include/zenoh-pico/system/link/raweth.h | 3 ++- src/transport/raweth/rx.c | 31 ++++++++++++++++++++----- 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index 8f675f3ab..20f84812e 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -23,7 +23,8 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 // Ethernet types -#define _ZP_ETH_TYPE_VLAN 0x8100 // 2048 (0x0800) IPv4 +#define _ZP_ETH_TYPE_VLAN 0x8100 // 2048 (0x0800) IPv4 +#define _ZP_ETH_TYPE_CUTOFF 0x600 // Values above are ethertype // Address Sizes #define _ZP_MAC_ADDR_LENGTH 6 diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c index ae8ada39e..d5feebcee 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/rx.c @@ -28,9 +28,28 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z_bytes_t *addr) { - size_t rb = _z_receive_raweth(link->_socket._raweth._sock._fd, _z_zbuf_get_wptr(zbf), _z_zbuf_space_left(zbf), addr); - if (rb != SIZE_MAX) { - _z_zbuf_set_wpos(zbf, _z_zbuf_get_wpos(zbf) + rb); + 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); + // Check validity + if ((rb == SIZE_MAX) || (rb < sizeof(_zp_eth_header_t))) { + return rb; + } + // Check if header has vlan + _Bool has_vlan = false; + _zp_eth_header_t *header = (_zp_eth_header_t *)buff; + if (header->length > _ZP_ETH_TYPE_CUTOFF) { + has_vlan = true; + } + // Check validity + if (has_vlan && (rb < sizeof(_zp_eth_vlan_header_t))) { + return rb; + } + // Update buffer but skip eth header + _z_zbuf_set_wpos(zbf, _z_zbuf_get_wpos(zbf) + rb); + if (has_vlan) { + _z_zbuf_set_rpos(zbf, _z_zbuf_get_wpos(zbf) - (rb - sizeof(_zp_eth_vlan_header_t))); + } else { + _z_zbuf_set_rpos(zbf, _z_zbuf_get_wpos(zbf) - (rb - sizeof(_zp_eth_header_t))); } return rb; } @@ -117,7 +136,7 @@ int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_ } int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, - _z_bytes_t *addr) { + _z_bytes_t *addr) { int8_t ret = _Z_RES_OK; #if Z_FEATURE_MULTI_THREAD == 1 // Acquire and keep the lock @@ -337,10 +356,10 @@ int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_ } int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, - _z_bytes_t *addr) { + _z_bytes_t *addr) { _ZP_UNUSED(ztm); _ZP_UNUSED(t_msg); _ZP_UNUSED(addr); return _Z_ERR_TRANSPORT_NOT_AVAILABLE; } -#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 \ No newline at end of file +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 \ No newline at end of file From 5dc2235e394a2feff4e7eb0fafec49651a923856 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 12:14:42 +0100 Subject: [PATCH 10/38] fix: remove linux dependency --- src/system/unix/link/raweth.c | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index ee0bca0eb..8a2af3e04 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -86,20 +85,8 @@ int8_t _z_open_raweth(_z_sys_net_socket_t *sock) { } size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t buff_len) { - struct sockaddr_ll saddr; - memset(&saddr, 0, sizeof(saddr)); - saddr.sll_family = AF_PACKET; - saddr.sll_protocol = htons(ETH_P_ALL); - saddr.sll_halen = ETH_ALEN; - - // Retrieve dmac from buff - if (buff_len < ETH_ALEN) { - return SIZE_MAX; - } - memcpy(&saddr.sll_addr, buff, ETH_ALEN); - // Send data - ssize_t wb = sendto(sock->_fd, buff, buff_len, 0, (struct sockaddr *)&saddr, sizeof(saddr)); + ssize_t wb = write(sock->_fd, buff, buff_len); if (wb < 0) { return SIZE_MAX; } @@ -112,12 +99,12 @@ size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buf return SIZE_MAX; } // Soft Filtering ? + // Copy sender mac if needed if ((addr != NULL) && (bytesRead > 2 * ETH_ALEN)) { *addr = _z_bytes_make(sizeof(ETH_ALEN)); (void)memcpy((uint8_t *)addr->start, (buff + ETH_ALEN), sizeof(ETH_ALEN)); } - // Dump ethernet header ? return bytesRead; } From 8b6ed7c7a15e277a418f322ef28417c8405e1853 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 14:56:45 +0100 Subject: [PATCH 11/38] fix: windows build --- src/transport/raweth/link.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index 7da7f2ae0..f9a934335 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -16,7 +16,6 @@ #include #include #include -#include #include "zenoh-pico/config.h" #include "zenoh-pico/link/manager.h" From 9cf5c97ea6ee2601a42cb33e32c1dbf701aaf8f1 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 16:04:49 +0100 Subject: [PATCH 12/38] feat: add raweth config files --- include/zenoh-pico/transport/raweth/config.h | 41 ++++++++++++++++++++ src/transport/raweth/config.c | 29 ++++++++++++++ 2 files changed, 70 insertions(+) create mode 100644 include/zenoh-pico/transport/raweth/config.h create mode 100644 src/transport/raweth/config.c diff --git a/include/zenoh-pico/transport/raweth/config.h b/include/zenoh-pico/transport/raweth/config.h new file mode 100644 index 000000000..7a038b324 --- /dev/null +++ b/include/zenoh-pico/transport/raweth/config.h @@ -0,0 +1,41 @@ +// +// 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/system/link/raweth.h" +#include "zenoh-pico/transport/transport.h" +#include "zenoh-pico/utils/result.h" +#include "zenoh-pico/protocol/core.h" + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +typedef struct { + _z_keyexpr_t _keyexpr; + uint16_t _vlan; // vlan tag (pcp + dei + id) + uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; + _Bool _has_vlan; +} _zp_raweth_cfg_entry; + +// Source mac address +extern const uint8_t _zp_raweth_cfg_smac[_ZP_MAC_ADDR_LENGTH]; + +// Sort the keyexpr alphabetically to use binary search (if size ~100+), otherwise use simple linear search +extern const _zp_raweth_cfg_entry _zp_raweth_cfg_array[]; + +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 +#endif // ZENOH_PICO_RAWETH_CONFIG_H diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c new file mode 100644 index 000000000..4f6d4dea2 --- /dev/null +++ b/src/transport/raweth/config.c @@ -0,0 +1,29 @@ +// +// 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 +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}, ""}, 0x00, {0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}, false}, // Default mac addr + {{0, {0},"some/key/expr"}, 0x8c, {0x00, 0x11, 0x22, 0x33, 0x44, 0x55}, true}, // entry1 + {{0, {0},"another/keyexpr"}, 0x43, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true}, // entry2 +}; + +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 \ No newline at end of file From c7304e8bd6f5778c6f7708693cceb289f82a8d54 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 16:05:01 +0100 Subject: [PATCH 13/38] feat: add array size macro --- include/zenoh-pico/utils/result.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/zenoh-pico/utils/result.h b/include/zenoh-pico/utils/result.h index bae8d7b17..19b808075 100644 --- a/include/zenoh-pico/utils/result.h +++ b/include/zenoh-pico/utils/result.h @@ -16,6 +16,7 @@ #define ZENOH_PICO_UTILS_RESULT_H #define _ZP_UNUSED(x) (void)(x) +#define _ZP_ARRAY_SIZE(array) (sizeof(array)/sizeof(array[0])) #define _Z_ERR_MESSAGE_MASK 0x88 #define _Z_ERR_ENTITY_MASK 0x90 From 5a0d72c593d81de56a4d1a8f555d7a77aa869bad Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 17:44:17 +0100 Subject: [PATCH 14/38] feat: use raweth config --- include/zenoh-pico/system/link/raweth.h | 1 - include/zenoh-pico/transport/raweth/config.h | 7 ++- src/transport/raweth/config.c | 7 ++- src/transport/raweth/link.c | 16 ++---- src/transport/raweth/tx.c | 59 +++++++++++++++++++- 5 files changed, 73 insertions(+), 17 deletions(-) diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index 20f84812e..21b752ace 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -54,7 +54,6 @@ typedef struct { } _zp_eth_vlan_header_t; typedef struct { - void *_config; // Pointer to config data _z_sys_net_socket_t _sock; uint16_t _vlan; uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; diff --git a/include/zenoh-pico/transport/raweth/config.h b/include/zenoh-pico/transport/raweth/config.h index 7a038b324..01771ab99 100644 --- a/include/zenoh-pico/transport/raweth/config.h +++ b/include/zenoh-pico/transport/raweth/config.h @@ -32,10 +32,13 @@ typedef struct { } _zp_raweth_cfg_entry; // Source mac address -extern const uint8_t _zp_raweth_cfg_smac[_ZP_MAC_ADDR_LENGTH]; +extern const uint8_t _ZP_RAWETH_CFG_SMAC[_ZP_MAC_ADDR_LENGTH]; // Sort the keyexpr alphabetically to use binary search (if size ~100+), otherwise use simple linear search -extern const _zp_raweth_cfg_entry _zp_raweth_cfg_array[]; +extern const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[]; + +// Array size +extern const size_t _ZP_RAWETH_CFG_SIZE; #endif // Z_FEATURE_RAWETH_TRANSPORT == 1 #endif // ZENOH_PICO_RAWETH_CONFIG_H diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c index 4f6d4dea2..9b73422ac 100644 --- a/src/transport/raweth/config.c +++ b/src/transport/raweth/config.c @@ -17,13 +17,16 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 // Should be generated -const uint8_t _zp_raweth_cfg_smac[_ZP_MAC_ADDR_LENGTH] = {0x30, 0x03, 0xc8, 0x37, 0x25, 0xa1}; +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[] = { +const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[] = { {{0, {0}, ""}, 0x00, {0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}, false}, // Default mac addr {{0, {0},"some/key/expr"}, 0x8c, {0x00, 0x11, 0x22, 0x33, 0x44, 0x55}, true}, // entry1 {{0, {0},"another/keyexpr"}, 0x43, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true}, // entry2 }; +// Don't modify +const size_t _ZP_RAWETH_CFG_SIZE = _ZP_ARRAY_SIZE(_ZP_RAWETH_CFG_ARRAY); + #endif // Z_FEATURE_RAWETH_TRANSPORT == 1 \ No newline at end of file diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index f9a934335..9edb76973 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -19,7 +19,9 @@ #include "zenoh-pico/config.h" #include "zenoh-pico/link/manager.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/pointers.h" // Address Sizes @@ -87,13 +89,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); // Open raweth link - int8_t ret = _z_open_raweth(&self->_socket._raweth._sock); - if (ret != _Z_RES_OK) { - return ret; - } - // Retrieve smac - return _z_get_smac_raweth(&self->_socket._raweth); + return _z_open_raweth(&self->_socket._raweth._sock); } static int8_t _z_f_link_listen_raweth(_z_link_t *self) { return _z_f_link_open_raweth(self); } @@ -160,11 +159,6 @@ int8_t _z_new_link_raweth(_z_link_t *zl, _z_endpoint_t endpoint) { // Init socket memset(&zl->_socket._raweth, 0, sizeof(zl->_socket._raweth)); - // Note locator address - uint8_t *b_address = __z_parse_address_raweth(endpoint._locator._address); - memcpy(zl->_socket._raweth._dmac, b_address, _ZP_MAC_ADDR_LENGTH); - z_free(b_address); - zl->_open_f = _z_f_link_open_raweth; zl->_listen_f = _z_f_link_listen_raweth; zl->_close_f = _z_f_link_close_raweth; diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index c5dd1c5c7..f5186dacf 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -16,18 +16,56 @@ #include "zenoh-pico/transport/common/tx.h" +#include "zenoh-pico/api/primitives.h" #include "zenoh-pico/config.h" #include "zenoh-pico/protocol/codec/core.h" #include "zenoh-pico/protocol/codec/network.h" #include "zenoh-pico/protocol/codec/transport.h" #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" #if Z_FEATURE_RAWETH_TRANSPORT == 1 +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) { + 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; + if (sock->_has_vlan) { + memcpy(&sock->_vlan, &vlan, sizeof(vlan)); + } + } 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)) { + 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; + } + } + return ret; +} + /** * This function is unsafe because it operates in potentially concurrent data. * Make sure that the following mutexes are locked before calling this function: @@ -142,7 +180,8 @@ int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_me #if Z_FEATURE_MULTI_THREAD == 1 _z_mutex_lock(&ztm->_mutex_tx); #endif - + // Set socket info + _zp_raweth_set_socket(NULL, &ztm->_link._socket._raweth); // Reserve space for eth header __unsafe_z_raweth_prepare_header(ztm); // Encode the session message @@ -182,6 +221,24 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, _ZP_UNUSED(cong_ctrl); #endif // Z_FEATURE_MULTI_THREAD == 1 + const z_keyexpr_t *keyexpr = NULL; + switch (n_msg->_tag) { + case _Z_N_PUSH: + keyexpr = &n_msg->_body._push._key; + break; + case _Z_N_REQUEST: + keyexpr = &n_msg->_body._request._key; + break; + case _Z_N_RESPONSE: + keyexpr = &n_msg->_body._response._key; + break; + case _Z_N_RESPONSE_FINAL: + case _Z_N_DECLARE: + default: + break; + } + // Set socket info + _zp_raweth_set_socket(keyexpr, &ztm->_link._socket._raweth); // Reserve space for eth header __unsafe_z_raweth_prepare_header(ztm); // Set the frame header From fbdb6dd203697ef2aeac6dfb6fe7ff32ef405694 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 24 Nov 2023 17:45:23 +0100 Subject: [PATCH 15/38] fix: run clang --- include/zenoh-pico/transport/raweth/config.h | 2 +- include/zenoh-pico/utils/result.h | 2 +- src/transport/raweth/config.c | 6 +++--- src/transport/raweth/tx.c | 8 ++++---- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/zenoh-pico/transport/raweth/config.h b/include/zenoh-pico/transport/raweth/config.h index 01771ab99..4dc7c3c1c 100644 --- a/include/zenoh-pico/transport/raweth/config.h +++ b/include/zenoh-pico/transport/raweth/config.h @@ -17,10 +17,10 @@ #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" -#include "zenoh-pico/protocol/core.h" #if Z_FEATURE_RAWETH_TRANSPORT == 1 diff --git a/include/zenoh-pico/utils/result.h b/include/zenoh-pico/utils/result.h index 19b808075..514473947 100644 --- a/include/zenoh-pico/utils/result.h +++ b/include/zenoh-pico/utils/result.h @@ -16,7 +16,7 @@ #define ZENOH_PICO_UTILS_RESULT_H #define _ZP_UNUSED(x) (void)(x) -#define _ZP_ARRAY_SIZE(array) (sizeof(array)/sizeof(array[0])) +#define _ZP_ARRAY_SIZE(array) (sizeof(array) / sizeof(array[0])) #define _Z_ERR_MESSAGE_MASK 0x88 #define _Z_ERR_ENTITY_MASK 0x90 diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c index 9b73422ac..4548c45df 100644 --- a/src/transport/raweth/config.c +++ b/src/transport/raweth/config.c @@ -21,9 +21,9 @@ const uint8_t _ZP_RAWETH_CFG_SMAC[_ZP_MAC_ADDR_LENGTH] = {0x30, 0x03, 0xc8, 0x37 // Should be generated const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[] = { - {{0, {0}, ""}, 0x00, {0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}, false}, // Default mac addr - {{0, {0},"some/key/expr"}, 0x8c, {0x00, 0x11, 0x22, 0x33, 0x44, 0x55}, true}, // entry1 - {{0, {0},"another/keyexpr"}, 0x43, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true}, // entry2 + {{0, {0}, ""}, 0x00, {0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}, false}, // Default mac addr + {{0, {0}, "some/key/expr"}, 0x8c, {0x00, 0x11, 0x22, 0x33, 0x44, 0x55}, true}, // entry1 + {{0, {0}, "another/keyexpr"}, 0x43, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true}, // entry2 }; // Don't modify diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index f5186dacf..f4155ef74 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -225,17 +225,17 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, switch (n_msg->_tag) { case _Z_N_PUSH: keyexpr = &n_msg->_body._push._key; - break; + break; case _Z_N_REQUEST: keyexpr = &n_msg->_body._request._key; - break; + break; case _Z_N_RESPONSE: keyexpr = &n_msg->_body._response._key; - break; + break; case _Z_N_RESPONSE_FINAL: case _Z_N_DECLARE: default: - break; + break; } // Set socket info _zp_raweth_set_socket(keyexpr, &ztm->_link._socket._raweth); From ecc07b76b08383c550f21524191fd89d0019f91b Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Mon, 27 Nov 2023 10:09:42 +0100 Subject: [PATCH 16/38] fix: don't check address in locator --- src/transport/raweth/link.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index 9edb76973..77f019f37 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -140,10 +140,6 @@ int8_t _z_endpoint_raweth_valid(_z_endpoint_t *endpoint) { if (!_z_str_eq(endpoint->_locator._protocol, RAWETH_SCHEMA)) { ret = _Z_ERR_CONFIG_LOCATOR_INVALID; } - // Check address - if (!__z_valid_address_raweth(endpoint->_locator._address)) { - ret = _Z_ERR_CONFIG_LOCATOR_INVALID; - } return ret; } From 2a9d68c5fe0968ac2edc4a72000ef373f5f7bb07 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Mon, 27 Nov 2023 10:19:31 +0100 Subject: [PATCH 17/38] fix: missing newline end of file --- src/system/emscripten/network.c | 2 +- src/system/espidf/network.c | 2 +- src/system/mbed/network.cpp | 2 +- src/system/unix/link/raweth.c | 2 +- src/system/windows/network.c | 2 +- src/transport/raweth/config.c | 2 +- src/transport/raweth/rx.c | 2 +- src/transport/raweth/tx.c | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/system/emscripten/network.c b/src/system/emscripten/network.c index 14d4dcace..95c0d2f09 100644 --- a/src/system/emscripten/network.c +++ b/src/system/emscripten/network.c @@ -176,4 +176,4 @@ size_t _z_send_ws(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len #if Z_FEATURE_RAWETH_TRANSPORT == 1 #error "Raw ethernet transport not supported yet on Emscripten port of Zenoh-Pico" -#endif \ No newline at end of file +#endif diff --git a/src/system/espidf/network.c b/src/system/espidf/network.c index 512635fe9..ad40e5078 100644 --- a/src/system/espidf/network.c +++ b/src/system/espidf/network.c @@ -742,4 +742,4 @@ size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t #if Z_FEATURE_RAWETH_TRANSPORT == 1 #error "Raw ethernet transport not supported yet on ESP-IDF port of Zenoh-Pico" -#endif \ No newline at end of file +#endif diff --git a/src/system/mbed/network.cpp b/src/system/mbed/network.cpp index 12e11d8f0..72cd3adc3 100644 --- a/src/system/mbed/network.cpp +++ b/src/system/mbed/network.cpp @@ -487,4 +487,4 @@ size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t #error "Raw ethernet transport not supported yet on MBED port of Zenoh-Pico" #endif -} // extern "C" \ No newline at end of file +} // extern "C" diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index 8a2af3e04..1774f301a 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -108,4 +108,4 @@ size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buf return bytesRead; } -#endif \ No newline at end of file +#endif diff --git a/src/system/windows/network.c b/src/system/windows/network.c index 08543da3e..833e43a97 100644 --- a/src/system/windows/network.c +++ b/src/system/windows/network.c @@ -613,4 +613,4 @@ size_t _z_send_udp_multicast(const _z_sys_net_socket_t sock, const uint8_t *ptr, #if Z_FEATURE_RAWETH_TRANSPORT == 1 #error "Raw ethernet transport not supported yet on Windows port of Zenoh-Pico" -#endif \ No newline at end of file +#endif diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c index 4548c45df..c3ee298f6 100644 --- a/src/transport/raweth/config.c +++ b/src/transport/raweth/config.c @@ -29,4 +29,4 @@ const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[] = { // Don't modify const size_t _ZP_RAWETH_CFG_SIZE = _ZP_ARRAY_SIZE(_ZP_RAWETH_CFG_ARRAY); -#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 \ No newline at end of file +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c index d5feebcee..ef073c944 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/rx.c @@ -362,4 +362,4 @@ int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_tran _ZP_UNUSED(addr); return _Z_ERR_TRANSPORT_NOT_AVAILABLE; } -#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 \ No newline at end of file +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index f4155ef74..c9d1bfa5a 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -301,4 +301,4 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, _ZP_UNUSED(cong_ctrl); return _Z_ERR_TRANSPORT_NOT_AVAILABLE; } -#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 \ No newline at end of file +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 From 3474b446cbc1aef98df65d361d1389f8b16a8847 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Mon, 27 Nov 2023 14:27:47 +0100 Subject: [PATCH 18/38] chore: remove obsolete comments --- include/zenoh-pico/system/link/raweth.h | 2 +- src/transport/raweth/tx.c | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index 21b752ace..f2d30d0df 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -23,7 +23,7 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 // Ethernet types -#define _ZP_ETH_TYPE_VLAN 0x8100 // 2048 (0x0800) IPv4 +#define _ZP_ETH_TYPE_VLAN 0x8100 #define _ZP_ETH_TYPE_CUTOFF 0x600 // Values above are ethertype // Address Sizes diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index c9d1bfa5a..3382292a2 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -106,7 +106,6 @@ static int8_t __unsafe_z_raweth_write_header(_z_transport_multicast_t *ztm) { size_t len = _z_wbuf_len(&ztm->_wbuf); _z_raweth_socket_t *resocket = &ztm->_link._socket._raweth; - // FIXME config function call to set the correct dmac & vlan value // Write eth header in buffer if (resocket->_has_vlan) { _zp_eth_vlan_header_t header; From c715855c476895c224828ede52b871bc80add6f9 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Mon, 27 Nov 2023 15:10:45 +0100 Subject: [PATCH 19/38] fix: remove unused intmap --- src/transport/raweth/link.c | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index 77f019f37..c6a0196e2 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -25,25 +25,8 @@ #include "zenoh-pico/utils/pointers.h" // Address Sizes -#define MAC_ADDR_LENGTH 6 - #define RAWETH_SCHEMA "reth" -#define RAWETH_CFG_ARGC 2 - -#define RAWETH_CFG_IFACE_KEY 0x01 -#define RAWETH_CFG_IFACE_STR "iface" - -#define RAWETH_CFG_VLAN_KEY 0x02 -#define RAWETH_CFG_VLAN_STR "vlan" - -#define RAWETH_CFG_MAPPING_BUILD \ - _z_str_intmapping_t args[RAWETH_CFG_ARGC]; \ - args[0]._key = RAWETH_CFG_IFACE_KEY; \ - args[0]._str = RAWETH_CFG_IFACE_STR; \ - args[1]._key = RAWETH_CFG_VLAN_KEY; \ - args[1]._str = RAWETH_CFG_VLAN_STR; - #if Z_FEATURE_RAWETH_TRANSPORT == 1 static _Bool __z_valid_address_raweth(const char *address) { From eb993e05f65537a9638ab7597542ebb3f4a296c5 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Mon, 27 Nov 2023 15:32:02 +0100 Subject: [PATCH 20/38] feat: add missing link header --- include/zenoh-pico/link/config/raweth.h | 32 +++++++++++++++++++++++++ src/transport/raweth/link.c | 4 +--- 2 files changed, 33 insertions(+), 3 deletions(-) create mode 100644 include/zenoh-pico/link/config/raweth.h diff --git a/include/zenoh-pico/link/config/raweth.h b/include/zenoh-pico/link/config/raweth.h new file mode 100644 index 000000000..6682805ad --- /dev/null +++ b/include/zenoh-pico/link/config/raweth.h @@ -0,0 +1,32 @@ +// +// 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_LINK_CONFIG_RAWETH_H +#define ZENOH_PICO_LINK_CONFIG_RAWETH_H + +#include "zenoh-pico/collections/intmap.h" +#include "zenoh-pico/collections/string.h" +#include "zenoh-pico/config.h" +#include "zenoh-pico/link/link.h" + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +#define RAWETH_SCHEMA "reth" + +int8_t _z_endpoint_raweth_valid(_z_endpoint_t *endpoint); + +int8_t _z_new_link_raweth(_z_link_t *zl, _z_endpoint_t endpoint); + +#endif /* Z_FEATURE_RAWETH_TRANSPORT */ +#endif /* ZENOH_PICO_LINK_CONFIG_RAWETH_H */ diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index c6a0196e2..e2cc8c5dc 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -18,15 +18,13 @@ #include #include "zenoh-pico/config.h" +#include "zenoh-pico/link/config/raweth.h" #include "zenoh-pico/link/manager.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/pointers.h" -// Address Sizes -#define RAWETH_SCHEMA "reth" - #if Z_FEATURE_RAWETH_TRANSPORT == 1 static _Bool __z_valid_address_raweth(const char *address) { From 9542ec89de240541a0130260a4524c26daf73a20 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Mon, 27 Nov 2023 15:49:58 +0100 Subject: [PATCH 21/38] fix: add missing functions --- src/transport/raweth/link.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index e2cc8c5dc..23cf31156 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -148,5 +148,15 @@ int8_t _z_new_link_raweth(_z_link_t *zl, _z_endpoint_t endpoint) { return ret; } +#else +int8_t _z_endpoint_raweth_valid(_z_endpoint_t *endpoint) { + _ZP_UNUSED(endpoint); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} +int8_t _z_new_link_raweth(_z_link_t *zl, _z_endpoint_t endpoint) { + _ZP_UNUSED(zl); + _ZP_UNUSED(endpoint); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} #endif From 560505c53de8915ed5901ee0c7da23e649add00d Mon Sep 17 00:00:00 2001 From: Jean-Roland Gosse Date: Tue, 28 Nov 2023 10:57:13 +0100 Subject: [PATCH 22/38] feat: add raweth join lease read --- include/zenoh-pico/transport/multicast/read.h | 2 +- include/zenoh-pico/transport/raweth/join.h | 22 ++ include/zenoh-pico/transport/raweth/lease.h | 25 +++ include/zenoh-pico/transport/raweth/read.h | 25 +++ src/transport/raweth/join.c | 38 ++++ src/transport/raweth/lease.c | 208 ++++++++++++++++++ src/transport/raweth/read.c | 167 ++++++++++++++ 7 files changed, 486 insertions(+), 1 deletion(-) create mode 100644 include/zenoh-pico/transport/raweth/join.h create mode 100644 include/zenoh-pico/transport/raweth/lease.h create mode 100644 include/zenoh-pico/transport/raweth/read.h create mode 100644 src/transport/raweth/join.c create mode 100644 src/transport/raweth/lease.c create mode 100644 src/transport/raweth/read.c diff --git a/include/zenoh-pico/transport/multicast/read.h b/include/zenoh-pico/transport/multicast/read.h index b2d53f770..308fe1249 100644 --- a/include/zenoh-pico/transport/multicast/read.h +++ b/include/zenoh-pico/transport/multicast/read.h @@ -22,4 +22,4 @@ int8_t _zp_multicast_start_read_task(_z_transport_t *zt, _z_task_attr_t *attr, _ int8_t _zp_multicast_stop_read_task(_z_transport_t *zt); void *_zp_multicast_read_task(void *ztm_arg); // The argument is void* to avoid incompatible pointer types in tasks -#endif /* ZENOH_PICO_TRANSPORT_LINK_TASK_READ_H */ +#endif /* ZENOH_PICO_MULTICAST_READ_H */ diff --git a/include/zenoh-pico/transport/raweth/join.h b/include/zenoh-pico/transport/raweth/join.h new file mode 100644 index 000000000..da8f875d9 --- /dev/null +++ b/include/zenoh-pico/transport/raweth/join.h @@ -0,0 +1,22 @@ +// +// 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_RAWETH_JOIN_H +#define ZENOH_RAWETH_JOIN_H + +#include "zenoh-pico/transport/transport.h" + +int8_t _zp_raweth_send_join(_z_transport_multicast_t *ztm); + +#endif /* ZENOH_RAWETH_JOIN_H */ diff --git a/include/zenoh-pico/transport/raweth/lease.h b/include/zenoh-pico/transport/raweth/lease.h new file mode 100644 index 000000000..924fd38a0 --- /dev/null +++ b/include/zenoh-pico/transport/raweth/lease.h @@ -0,0 +1,25 @@ +// +// 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_LEASE_H +#define ZENOH_PICO_RAWETH_LEASE_H + +#include "zenoh-pico/transport/transport.h" + +int8_t _zp_raweth_send_keep_alive(_z_transport_multicast_t *ztm); +int8_t _zp_raweth_start_lease_task(_z_transport_t *zt, _z_task_attr_t *attr, _z_task_t *task); +int8_t _zp_raweth_stop_lease_task(_z_transport_t *zt); +void *_zp_raweth_lease_task(void *ztm_arg); // The argument is void* to avoid incompatible pointer types in tasks + +#endif /* ZENOH_PICO_RAWETH_LEASE_H */ diff --git a/include/zenoh-pico/transport/raweth/read.h b/include/zenoh-pico/transport/raweth/read.h new file mode 100644 index 000000000..fa881b4ba --- /dev/null +++ b/include/zenoh-pico/transport/raweth/read.h @@ -0,0 +1,25 @@ +// +// 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_READ_H +#define ZENOH_PICO_RAWETH_READ_H + +#include "zenoh-pico/transport/transport.h" + +int8_t _zp_raweth_read(_z_transport_multicast_t *ztm); +int8_t _zp_raweth_start_read_task(_z_transport_t *zt, _z_task_attr_t *attr, _z_task_t *task); +int8_t _zp_raweth_stop_read_task(_z_transport_t *zt); +void *_zp_raweth_read_task(void *ztm_arg); // The argument is void* to avoid incompatible pointer types in tasks + +#endif /* ZENOH_PICO_RAWETH_READ_H */ diff --git a/src/transport/raweth/join.c b/src/transport/raweth/join.c new file mode 100644 index 000000000..7ac9e7506 --- /dev/null +++ b/src/transport/raweth/join.c @@ -0,0 +1,38 @@ +// +// 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/join.h" + +#include "zenoh-pico/session/utils.h" +#include "zenoh-pico/transport/raweth/tx.h" + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +int8_t _zp_raweth_send_join(_z_transport_multicast_t *ztm) { + _z_conduit_sn_list_t next_sn; + next_sn._is_qos = false; + next_sn._val._plain._best_effort = ztm->_sn_tx_best_effort; + next_sn._val._plain._reliable = ztm->_sn_tx_reliable; + + _z_id_t zid = ((_z_session_t *)ztm->_session)->_local_zid; + _z_transport_message_t jsm = _z_t_msg_make_join(Z_WHATAMI_PEER, Z_TRANSPORT_LEASE, zid, next_sn); + + return _z_raweth_send_t_msg(ztm, &jsm); +} +#else +int8_t _zp_raweth_send_join(_z_transport_multicast_t *ztm) { + _ZP_UNUSED(ztm); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 diff --git a/src/transport/raweth/lease.c b/src/transport/raweth/lease.c new file mode 100644 index 000000000..672c04def --- /dev/null +++ b/src/transport/raweth/lease.c @@ -0,0 +1,208 @@ +// +// 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/lease.h" + +#include + +#include "zenoh-pico/config.h" +#include "zenoh-pico/session/utils.h" +#include "zenoh-pico/transport/common/join.h" +#include "zenoh-pico/transport/raweth/join.h" +#include "zenoh-pico/transport/raweth/transport.h" +#include "zenoh-pico/transport/raweth/tx.h" +#include "zenoh-pico/utils/logging.h" + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +static _z_zint_t _z_get_minimum_lease(_z_transport_peer_entry_list_t *peers, _z_zint_t local_lease) { + _z_zint_t ret = local_lease; + + _z_transport_peer_entry_list_t *it = peers; + while (it != NULL) { + _z_transport_peer_entry_t *val = _z_transport_peer_entry_list_head(it); + _z_zint_t lease = val->_lease; + if (lease < ret) { + ret = lease; + } + + it = _z_transport_peer_entry_list_tail(it); + } + + return ret; +} + +static _z_zint_t _z_get_next_lease(_z_transport_peer_entry_list_t *peers) { + _z_zint_t ret = SIZE_MAX; + + _z_transport_peer_entry_list_t *it = peers; + while (it != NULL) { + _z_transport_peer_entry_t *val = _z_transport_peer_entry_list_head(it); + _z_zint_t next_lease = val->_next_lease; + if (next_lease < ret) { + ret = next_lease; + } + + it = _z_transport_peer_entry_list_tail(it); + } + + return ret; +} + +int8_t _zp_raweth_send_keep_alive(_z_transport_multicast_t *ztm) { + int8_t ret = _Z_RES_OK; + + _z_transport_message_t t_msg = _z_t_msg_make_keep_alive(); + ret = _z_raweth_send_t_msg(ztm, &t_msg); + + return ret; +} + +int8_t _zp_raweth_start_lease_task(_z_transport_t *zt, _z_task_attr_t *attr, _z_task_t *task) { + // Init memory + (void)memset(task, 0, sizeof(_z_task_t)); + // Attach task + zt->_transport._multicast._lease_task = task; + zt->_transport._multicast._lease_task_running = true; + // Init task + if (_z_task_init(task, attr, _zp_raweth_lease_task, &zt->_transport._multicast) != _Z_RES_OK) { + zt->_transport._multicast._lease_task_running = false; + return _Z_ERR_SYSTEM_TASK_FAILED; + } + return _Z_RES_OK; +} + +int8_t _zp_raweth_stop_lease_task(_z_transport_t *zt) { + zt->_transport._multicast._lease_task_running = false; + return _Z_RES_OK; +} + +void *_zp_raweth_lease_task(void *ztm_arg) { +#if Z_FEATURE_MULTI_THREAD == 1 + _z_transport_multicast_t *ztm = (_z_transport_multicast_t *)ztm_arg; + ztm->_transmitted = false; + + // From all peers, get the next lease time (minimum) + _z_zint_t next_lease = _z_get_minimum_lease(ztm->_peers, ztm->_lease); + _z_zint_t next_keep_alive = (_z_zint_t)(next_lease / Z_TRANSPORT_LEASE_EXPIRE_FACTOR); + _z_zint_t next_join = Z_JOIN_INTERVAL; + + _z_transport_peer_entry_list_t *it = NULL; + while (ztm->_lease_task_running == true) { + _z_mutex_lock(&ztm->_mutex_peer); + + if (next_lease <= 0) { + it = ztm->_peers; + while (it != NULL) { + _z_transport_peer_entry_t *entry = _z_transport_peer_entry_list_head(it); + if (entry->_received == true) { + // Reset the lease parameters + entry->_received = false; + entry->_next_lease = entry->_lease; + it = _z_transport_peer_entry_list_tail(it); + } else { + _Z_INFO("Remove peer from know list because it has expired after %zums\n", entry->_lease); + ztm->_peers = + _z_transport_peer_entry_list_drop_filter(ztm->_peers, _z_transport_peer_entry_eq, entry); + it = ztm->_peers; + } + } + } + + if (next_join <= 0) { + _zp_raweth_send_join(ztm); + ztm->_transmitted = true; + + // Reset the join parameters + next_join = Z_JOIN_INTERVAL; + } + + if (next_keep_alive <= 0) { + // Check if need to send a keep alive + if (ztm->_transmitted == false) { + if (_zp_raweth_send_keep_alive(ztm) < 0) { + // TODO: Handle retransmission or error + } + } + + // Reset the keep alive parameters + ztm->_transmitted = false; + next_keep_alive = + (_z_zint_t)(_z_get_minimum_lease(ztm->_peers, ztm->_lease) / Z_TRANSPORT_LEASE_EXPIRE_FACTOR); + } + + // Compute the target interval to sleep + _z_zint_t interval; + if (next_lease > 0) { + interval = next_lease; + if (next_keep_alive < interval) { + interval = next_keep_alive; + } + if (next_join < interval) { + interval = next_join; + } + } else { + interval = next_keep_alive; + if (next_join < interval) { + interval = next_join; + } + } + + _z_mutex_unlock(&ztm->_mutex_peer); + + // The keep alive and lease intervals are expressed in milliseconds + z_sleep_ms(interval); + + // Decrement all intervals + _z_mutex_lock(&ztm->_mutex_peer); + + it = ztm->_peers; + while (it != NULL) { + _z_transport_peer_entry_t *entry = _z_transport_peer_entry_list_head(it); + entry->_next_lease = entry->_next_lease - interval; + it = _z_transport_peer_entry_list_tail(it); + } + next_lease = _z_get_next_lease(ztm->_peers); + next_keep_alive = next_keep_alive - interval; + next_join = next_join - interval; + + _z_mutex_unlock(&ztm->_mutex_peer); + } +#endif // Z_FEATURE_MULTI_THREAD == 1 + + return 0; +} +#else +int8_t _zp_raweth_send_keep_alive(_z_transport_multicast_t *ztm) { + _ZP_UNUSED(ztm); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _zp_raweth_start_lease_task(_z_transport_t *zt, _z_task_attr_t *attr, _z_task_t *task) { + _ZP_UNUSED(zt); + _ZP_UNUSED(attr); + _ZP_UNUSED(task); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _zp_raweth_stop_lease_task(_z_transport_t *zt) { + _ZP_UNUSED(zt); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +void *_zp_raweth_lease_task(void *ztm_arg) { + _ZP_UNUSED(ztm_arg); + return NULL; +} +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 diff --git a/src/transport/raweth/read.c b/src/transport/raweth/read.c new file mode 100644 index 000000000..5bc727a03 --- /dev/null +++ b/src/transport/raweth/read.c @@ -0,0 +1,167 @@ +// +// 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/read.h" + +#include + +#include "zenoh-pico/config.h" +#include "zenoh-pico/protocol/codec/transport.h" +#include "zenoh-pico/protocol/iobuf.h" +#include "zenoh-pico/transport/raweth/rx.h" +#include "zenoh-pico/transport/unicast/rx.h" +#include "zenoh-pico/utils/logging.h" + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 + +int8_t _zp_raweth_read(_z_transport_multicast_t *ztm) { + int8_t ret = _Z_RES_OK; + + _z_bytes_t addr; + _z_transport_message_t t_msg; + ret = _z_raweth_recv_t_msg(ztm, &t_msg, &addr); + if (ret == _Z_RES_OK) { + ret = _z_raweth_handle_transport_message(ztm, &t_msg, &addr); + _z_t_msg_clear(&t_msg); + } + + return ret; +} + +int8_t _zp_raweth_start_read_task(_z_transport_t *zt, _z_task_attr_t *attr, _z_task_t *task) { + // Init memory + (void)memset(task, 0, sizeof(_z_task_t)); + // Attach task + zt->_transport._multicast._read_task = task; + zt->_transport._multicast._read_task_running = true; + // Init task + if (_z_task_init(task, attr, _zp_raweth_read_task, &zt->_transport._multicast) != _Z_RES_OK) { + zt->_transport._multicast._read_task_running = false; + return _Z_ERR_SYSTEM_TASK_FAILED; + } + return _Z_RES_OK; +} + +int8_t _zp_raweth_stop_read_task(_z_transport_t *zt) { + zt->_transport._multicast._read_task_running = false; + return _Z_RES_OK; +} + +void *_zp_raweth_read_task(void *ztm_arg) { +#if Z_FEATURE_MULTI_THREAD == 1 + _z_transport_multicast_t *ztm = (_z_transport_multicast_t *)ztm_arg; + + // Acquire and keep the lock + _z_mutex_lock(&ztm->_mutex_rx); + + // Prepare the buffer + _z_zbuf_reset(&ztm->_zbuf); + + _z_bytes_t addr = _z_bytes_wrap(NULL, 0); + while (ztm->_read_task_running == true) { + // Read bytes from socket to the main buffer + size_t to_read = 0; + + switch (ztm->_link._cap._flow) { + case Z_LINK_CAP_FLOW_STREAM: + if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, &addr); + if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_bytes_clear(&addr); + _z_zbuf_compact(&ztm->_zbuf); + continue; + } + } + + for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) { + to_read |= _z_zbuf_read(&ztm->_zbuf) << (i * (uint8_t)8); + } + + if (_z_zbuf_len(&ztm->_zbuf) < to_read) { + _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, NULL); + if (_z_zbuf_len(&ztm->_zbuf) < to_read) { + _z_zbuf_set_rpos(&ztm->_zbuf, _z_zbuf_get_rpos(&ztm->_zbuf) - _Z_MSG_LEN_ENC_SIZE); + _z_zbuf_compact(&ztm->_zbuf); + continue; + } + } + break; + case Z_LINK_CAP_FLOW_DATAGRAM: + _z_zbuf_compact(&ztm->_zbuf); + to_read = _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, &addr); + if (to_read == SIZE_MAX) { + continue; + } + break; + default: + break; + } + // Wrap the main buffer for to_read bytes + _z_zbuf_t zbuf = _z_zbuf_view(&ztm->_zbuf, to_read); + + while (_z_zbuf_len(&zbuf) > 0) { + int8_t ret = _Z_RES_OK; + + // Decode one session message + _z_transport_message_t t_msg; + ret = _z_transport_message_decode(&t_msg, &zbuf); + if (ret == _Z_RES_OK) { + ret = _z_raweth_handle_transport_message(ztm, &t_msg, &addr); + + if (ret == _Z_RES_OK) { + _z_t_msg_clear(&t_msg); + _z_bytes_clear(&addr); + } else { + ztm->_read_task_running = false; + continue; + } + } else { + _Z_ERROR("Connection closed due to malformed message\n"); + ztm->_read_task_running = false; + continue; + } + } + + // Move the read position of the read buffer + _z_zbuf_set_rpos(&ztm->_zbuf, _z_zbuf_get_rpos(&ztm->_zbuf) + to_read); + } + + _z_mutex_unlock(&ztm->_mutex_rx); +#endif // Z_FEATURE_MULTI_THREAD == 1 + + return NULL; +} +#else +int8_t _zp_raweth_read(_z_transport_multicast_t *ztm) { + _ZP_UNUSED(ztm); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _zp_raweth_start_read_task(_z_transport_t *zt, _z_task_attr_t *attr, _z_task_t *task) { + _ZP_UNUSED(zt); + _ZP_UNUSED(attr); + _ZP_UNUSED(task); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +int8_t _zp_raweth_stop_read_task(_z_transport_t *zt) { + _ZP_UNUSED(zt); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +void *_zp_raweth_read_task(void *ztm_arg) { + _ZP_UNUSED(ztm_arg); + return NULL; +} +#endif // Z_FEATURE_RAWETH_TRANSPORT == 1 From 85af6b0f1918aafc6eb31a553d300e15be975833 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Tue, 28 Nov 2023 14:48:30 +0100 Subject: [PATCH 23/38] feat: switch to ethtype field and remove vlan struct --- include/zenoh-pico/system/link/raweth.h | 15 ++---- include/zenoh-pico/transport/raweth/config.h | 3 ++ src/transport/raweth/config.c | 3 ++ src/transport/raweth/rx.c | 4 +- src/transport/raweth/tx.c | 51 ++++---------------- 5 files changed, 21 insertions(+), 55 deletions(-) diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index f2d30d0df..41c6738fa 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -24,7 +24,6 @@ // Ethernet types #define _ZP_ETH_TYPE_VLAN 0x8100 -#define _ZP_ETH_TYPE_CUTOFF 0x600 // Values above are ethertype // Address Sizes #define _ZP_MAC_ADDR_LENGTH 6 @@ -32,30 +31,24 @@ // Max frame size #define _ZP_MAX_ETH_FRAME_SIZE 1500 -typedef struct { - uint16_t tpid; // Tag ethertype - uint16_t pcp : 3; // Priority code point - uint16_t dei : 1; // Drop eligible indicator - uint16_t vid : 12; // VLAN id -} _zp_vlan_tag_t; - // Ethernet header structure type typedef struct { uint8_t dmac[_ZP_MAC_ADDR_LENGTH]; // Destination mac address uint8_t smac[_ZP_MAC_ADDR_LENGTH]; // Source mac address - uint16_t length; // Size of frame + uint16_t ethtype; // Ethertype of frame } _zp_eth_header_t; typedef struct { uint8_t dmac[_ZP_MAC_ADDR_LENGTH]; // Destination mac address uint8_t smac[_ZP_MAC_ADDR_LENGTH]; // Source mac address - _zp_vlan_tag_t tag; - uint16_t length; // Size of frame + uint16_t tag; // Vlan tag + uint16_t ethtype; // Ethertype of frame } _zp_eth_vlan_header_t; typedef struct { _z_sys_net_socket_t _sock; uint16_t _vlan; + uint16_t ethtype; uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; uint8_t _smac[_ZP_MAC_ADDR_LENGTH]; _Bool _has_vlan; diff --git a/include/zenoh-pico/transport/raweth/config.h b/include/zenoh-pico/transport/raweth/config.h index 4dc7c3c1c..7921f8389 100644 --- a/include/zenoh-pico/transport/raweth/config.h +++ b/include/zenoh-pico/transport/raweth/config.h @@ -31,6 +31,9 @@ typedef struct { _Bool _has_vlan; } _zp_raweth_cfg_entry; +// Ethertype to use in frame +extern const uint16_t _ZP_RAWETH_CFG_ETHTYPE; + // Source mac address extern const uint8_t _ZP_RAWETH_CFG_SMAC[_ZP_MAC_ADDR_LENGTH]; diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c index c3ee298f6..fb5186462 100644 --- a/src/transport/raweth/config.c +++ b/src/transport/raweth/config.c @@ -16,6 +16,9 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 +// Should be generated +const uint16_t _ZP_RAWETH_CFG_ETHTYPE = 0x72e0; + // 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/rx.c b/src/transport/raweth/rx.c index ef073c944..64ddf2ff9 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/rx.c @@ -37,7 +37,7 @@ static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z // Check if header has vlan _Bool has_vlan = false; _zp_eth_header_t *header = (_zp_eth_header_t *)buff; - if (header->length > _ZP_ETH_TYPE_CUTOFF) { + if (header->ethtype == _ZP_ETH_TYPE_VLAN) { has_vlan = true; } // Check validity @@ -54,7 +54,7 @@ static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z return rb; } -_z_transport_peer_entry_t *_z_find_peer_entry(_z_transport_peer_entry_list_t *l, _z_bytes_t *remote_addr) { +static _z_transport_peer_entry_t *_z_find_peer_entry(_z_transport_peer_entry_list_t *l, _z_bytes_t *remote_addr) { _z_transport_peer_entry_t *ret = NULL; _z_transport_peer_entry_list_t *xs = l; diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index 3382292a2..2834f5ff9 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -83,27 +83,12 @@ static _z_zint_t __unsafe_z_raweth_get_sn(_z_transport_multicast_t *ztm, z_relia return sn; } -/** - * This function is unsafe because it operates in potentially concurrent data. - * Make sure that the following mutexes are locked before calling this function: - * - ztm->_mutex_inner - */ -static void __unsafe_z_raweth_prepare_header(_z_transport_multicast_t *ztm) { - // Reserve space for ethernet header - if (&ztm->_link._socket._raweth._has_vlan) { - _z_wbuf_set_wpos(&ztm->_wbuf, sizeof(_zp_eth_vlan_header_t)); - } else { - _z_wbuf_set_wpos(&ztm->_wbuf, sizeof(_zp_eth_header_t)); - } -} - /** * This function is unsafe because it operates in potentially concurrent data. * Make sure that the following mutexes are locked before calling this function: * - ztm->_mutex_inner */ static int8_t __unsafe_z_raweth_write_header(_z_transport_multicast_t *ztm) { - size_t len = _z_wbuf_len(&ztm->_wbuf); _z_raweth_socket_t *resocket = &ztm->_link._socket._raweth; // Write eth header in buffer @@ -112,29 +97,17 @@ static int8_t __unsafe_z_raweth_write_header(_z_transport_multicast_t *ztm) { memset(&header, 0, sizeof(header)); memcpy(&header.dmac, &resocket->_dmac, _ZP_MAC_ADDR_LENGTH); memcpy(&header.smac, &resocket->_smac, _ZP_MAC_ADDR_LENGTH); - header.tag.tpid = _ZP_ETH_TYPE_VLAN; - header.tag.pcp = 0; // Get from locator/config - header.tag.vid = 0; // Get from locator/config - header.length = (uint16_t)len; - // Save wpos - size_t final_wpos = _z_wbuf_get_wpos(&ztm->_wbuf); + header.tag = _ZP_ETH_TYPE_VLAN; + header.ethtype = _ZP_RAWETH_CFG_ETHTYPE; // Write header - _z_wbuf_set_wpos(&ztm->_wbuf, 0); _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(&ztm->_wbuf, (uint8_t *)&header, 0, sizeof(header))); - // Restore wpos - _z_wbuf_set_wpos(&ztm->_wbuf, final_wpos); } else { _zp_eth_header_t header; memcpy(&header.dmac, &resocket->_dmac, _ZP_MAC_ADDR_LENGTH); memcpy(&header.smac, &resocket->_smac, _ZP_MAC_ADDR_LENGTH); - header.length = (uint16_t)len; - // Save wpos - size_t final_wpos = _z_wbuf_get_wpos(&ztm->_wbuf); + header.ethtype = _ZP_RAWETH_CFG_ETHTYPE; // Write header - _z_wbuf_set_wpos(&ztm->_wbuf, 0); _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(&ztm->_wbuf, (uint8_t *)&header, 0, sizeof(header))); - // Restore wpos - _z_wbuf_set_wpos(&ztm->_wbuf, final_wpos); } return _Z_RES_OK; } @@ -181,12 +154,10 @@ int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_me #endif // Set socket info _zp_raweth_set_socket(NULL, &ztm->_link._socket._raweth); - // Reserve space for eth header - __unsafe_z_raweth_prepare_header(ztm); - // Encode the session message - _Z_RETURN_IF_ERR(_z_transport_message_encode(&ztm->_wbuf, t_msg)); // Write the message header _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); + // Encode the session message + _Z_RETURN_IF_ERR(_z_transport_message_encode(&ztm->_wbuf, t_msg)); // Send the wbuf on the socket _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(ztm)); // Mark the session that we have transmitted data @@ -238,8 +209,8 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, } // Set socket info _zp_raweth_set_socket(keyexpr, &ztm->_link._socket._raweth); - // Reserve space for eth header - __unsafe_z_raweth_prepare_header(ztm); + // Write the eth header + _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); // Set the frame header _z_zint_t sn = __unsafe_z_raweth_get_sn(ztm, reliability); _z_transport_message_t t_msg = _z_t_msg_make_frame_header(sn, reliability); @@ -248,8 +219,6 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, // Encode the network message ret = _z_network_message_encode(&ztm->_wbuf, n_msg); if (ret == _Z_RES_OK) { - // Write the eth header - _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); // Send the wbuf on the socket _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(ztm)); // Mark the session that we have transmitted data @@ -267,12 +236,10 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, sn = __unsafe_z_raweth_get_sn(ztm, reliability); } is_first = false; - // Reserve space for eth header - __unsafe_z_raweth_prepare_header(ztm); - // Serialize one fragment - _Z_RETURN_IF_ERR(__unsafe_z_serialize_zenoh_fragment(&ztm->_wbuf, &fbf, reliability, sn)); // Write the eth header _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); + // Serialize one fragment + _Z_RETURN_IF_ERR(__unsafe_z_serialize_zenoh_fragment(&ztm->_wbuf, &fbf, reliability, sn)); // Send the wbuf on the socket _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(ztm)); // Mark the session that we have transmitted data From 788eb072ecd5cea97a3363af3fe3e434d18bd2f4 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Tue, 28 Nov 2023 14:48:51 +0100 Subject: [PATCH 24/38] fix: missing functions --- .../zenoh-pico/transport/raweth/transport.h | 3 +++ src/transport/raweth/transport.c | 24 +++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/include/zenoh-pico/transport/raweth/transport.h b/include/zenoh-pico/transport/raweth/transport.h index a91eba18a..721a2a68d 100644 --- a/include/zenoh-pico/transport/raweth/transport.h +++ b/include/zenoh-pico/transport/raweth/transport.h @@ -17,6 +17,9 @@ #include "zenoh-pico/api/types.h" +void _zp_raweth_fetch_zid(const _z_transport_t *zt, z_owned_closure_zid_t *callback); +void _zp_raweth_info_session(const _z_transport_t *zt, _z_config_t *ps); + int8_t _z_raweth_transport_create(_z_transport_t *zt, _z_link_t *zl, _z_transport_multicast_establish_param_t *param); int8_t _z_raweth_open_peer(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, const _z_id_t *local_zid); diff --git a/src/transport/raweth/transport.c b/src/transport/raweth/transport.c index f920deeed..4774514ea 100644 --- a/src/transport/raweth/transport.c +++ b/src/transport/raweth/transport.c @@ -11,6 +11,8 @@ // Contributors: // ZettaScale Zenoh Team, +#include "zenoh-pico/transport/raweth/transport.h" + #include #include #include @@ -28,6 +30,28 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 +void _zp_raweth_fetch_zid(const _z_transport_t *zt, z_owned_closure_zid_t *callback) { + void *ctx = callback->context; + _z_transport_peer_entry_list_t *l = zt->_transport._multicast._peers; + for (; l != NULL; l = _z_transport_peer_entry_list_tail(l)) { + _z_transport_peer_entry_t *val = _z_transport_peer_entry_list_head(l); + z_id_t id = val->_remote_zid; + + callback->call(&id, ctx); + } +} + +void _zp_raweth_info_session(const _z_transport_t *zt, _z_config_t *ps) { + _z_transport_peer_entry_list_t *xs = zt->_transport._multicast._peers; + while (xs != NULL) { + _z_transport_peer_entry_t *peer = _z_transport_peer_entry_list_head(xs); + _z_bytes_t remote_zid = _z_bytes_wrap(peer->_remote_zid.id, _z_id_len(peer->_remote_zid)); + _zp_config_insert(ps, Z_INFO_PEER_PID_KEY, _z_string_from_bytes(&remote_zid)); + + xs = _z_transport_peer_entry_list_tail(xs); + } +} + int8_t _z_raweth_transport_create(_z_transport_t *zt, _z_link_t *zl, _z_transport_multicast_establish_param_t *param) { int8_t ret = _Z_RES_OK; From 79ecd1d8b3b76fc8ef39a9e3fe1c0d3263c6630c Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Tue, 28 Nov 2023 15:53:59 +0100 Subject: [PATCH 25/38] feat: mutualize code with multicast --- .../zenoh-pico/transport/raweth/transport.h | 3 - src/transport/raweth/read.c | 87 ++---- src/transport/raweth/rx.c | 293 +----------------- src/transport/raweth/transport.c | 22 -- 4 files changed, 36 insertions(+), 369 deletions(-) diff --git a/include/zenoh-pico/transport/raweth/transport.h b/include/zenoh-pico/transport/raweth/transport.h index 721a2a68d..a91eba18a 100644 --- a/include/zenoh-pico/transport/raweth/transport.h +++ b/include/zenoh-pico/transport/raweth/transport.h @@ -17,9 +17,6 @@ #include "zenoh-pico/api/types.h" -void _zp_raweth_fetch_zid(const _z_transport_t *zt, z_owned_closure_zid_t *callback); -void _zp_raweth_info_session(const _z_transport_t *zt, _z_config_t *ps); - int8_t _z_raweth_transport_create(_z_transport_t *zt, _z_link_t *zl, _z_transport_multicast_establish_param_t *param); int8_t _z_raweth_open_peer(_z_transport_multicast_establish_param_t *param, const _z_link_t *zl, const _z_id_t *local_zid); diff --git a/src/transport/raweth/read.c b/src/transport/raweth/read.c index 5bc727a03..cf4761f56 100644 --- a/src/transport/raweth/read.c +++ b/src/transport/raweth/read.c @@ -19,6 +19,7 @@ #include "zenoh-pico/config.h" #include "zenoh-pico/protocol/codec/transport.h" #include "zenoh-pico/protocol/iobuf.h" +#include "zenoh-pico/transport/multicast/rx.h" #include "zenoh-pico/transport/raweth/rx.h" #include "zenoh-pico/transport/unicast/rx.h" #include "zenoh-pico/utils/logging.h" @@ -32,10 +33,9 @@ int8_t _zp_raweth_read(_z_transport_multicast_t *ztm) { _z_transport_message_t t_msg; ret = _z_raweth_recv_t_msg(ztm, &t_msg, &addr); if (ret == _Z_RES_OK) { - ret = _z_raweth_handle_transport_message(ztm, &t_msg, &addr); + ret = _z_multicast_handle_transport_message(ztm, &t_msg, &addr); _z_t_msg_clear(&t_msg); } - return ret; } @@ -61,80 +61,31 @@ int8_t _zp_raweth_stop_read_task(_z_transport_t *zt) { void *_zp_raweth_read_task(void *ztm_arg) { #if Z_FEATURE_MULTI_THREAD == 1 _z_transport_multicast_t *ztm = (_z_transport_multicast_t *)ztm_arg; + _z_transport_message_t t_msg; + _z_bytes_t addr = _z_bytes_wrap(NULL, 0); // Acquire and keep the lock _z_mutex_lock(&ztm->_mutex_rx); - // Prepare the buffer _z_zbuf_reset(&ztm->_zbuf); - - _z_bytes_t addr = _z_bytes_wrap(NULL, 0); + // Task loop while (ztm->_read_task_running == true) { - // Read bytes from socket to the main buffer - size_t to_read = 0; - - switch (ztm->_link._cap._flow) { - case Z_LINK_CAP_FLOW_STREAM: - if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, &addr); - if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_bytes_clear(&addr); - _z_zbuf_compact(&ztm->_zbuf); - continue; - } - } - - for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) { - to_read |= _z_zbuf_read(&ztm->_zbuf) << (i * (uint8_t)8); - } - - if (_z_zbuf_len(&ztm->_zbuf) < to_read) { - _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, NULL); - if (_z_zbuf_len(&ztm->_zbuf) < to_read) { - _z_zbuf_set_rpos(&ztm->_zbuf, _z_zbuf_get_rpos(&ztm->_zbuf) - _Z_MSG_LEN_ENC_SIZE); - _z_zbuf_compact(&ztm->_zbuf); - continue; - } - } - break; - case Z_LINK_CAP_FLOW_DATAGRAM: - _z_zbuf_compact(&ztm->_zbuf); - to_read = _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, &addr); - if (to_read == SIZE_MAX) { - continue; - } - break; - default: - break; + // Read message from link + int8_t ret = _z_raweth_recv_t_msg(ztm, &t_msg, &addr); + if (ret == _Z_ERR_TRANSPORT_RX_FAILED) { + continue; + } else { + _Z_ERROR("Connection closed due to malformed message\n"); + ztm->_read_task_running = false; + continue; } - // Wrap the main buffer for to_read bytes - _z_zbuf_t zbuf = _z_zbuf_view(&ztm->_zbuf, to_read); - - while (_z_zbuf_len(&zbuf) > 0) { - int8_t ret = _Z_RES_OK; - - // Decode one session message - _z_transport_message_t t_msg; - ret = _z_transport_message_decode(&t_msg, &zbuf); - if (ret == _Z_RES_OK) { - ret = _z_raweth_handle_transport_message(ztm, &t_msg, &addr); - - if (ret == _Z_RES_OK) { - _z_t_msg_clear(&t_msg); - _z_bytes_clear(&addr); - } else { - ztm->_read_task_running = false; - continue; - } - } else { - _Z_ERROR("Connection closed due to malformed message\n"); - ztm->_read_task_running = false; - continue; - } + // Process message + if (_z_multicast_handle_transport_message(ztm, &t_msg, &addr) != _Z_RES_OK) { + ztm->_read_task_running = false; + continue; } - - // Move the read position of the read buffer - _z_zbuf_set_rpos(&ztm->_zbuf, _z_zbuf_get_rpos(&ztm->_zbuf) + to_read); + _z_t_msg_clear(&t_msg); + _z_bytes_clear(&addr); } _z_mutex_unlock(&ztm->_mutex_rx); diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c index 64ddf2ff9..ef189b4f4 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/rx.c @@ -32,7 +32,7 @@ static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z size_t rb = _z_receive_raweth(&link->_socket._raweth._sock, buff, _z_zbuf_space_left(zbf), addr); // Check validity if ((rb == SIZE_MAX) || (rb < sizeof(_zp_eth_header_t))) { - return rb; + return SIZE_MAX; } // Check if header has vlan _Bool has_vlan = false; @@ -42,7 +42,7 @@ static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z } // Check validity if (has_vlan && (rb < sizeof(_zp_eth_vlan_header_t))) { - return rb; + return SIZE_MAX; } // Update buffer but skip eth header _z_zbuf_set_wpos(zbf, _z_zbuf_get_wpos(zbf) + rb); @@ -54,23 +54,6 @@ static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z return rb; } -static _z_transport_peer_entry_t *_z_find_peer_entry(_z_transport_peer_entry_list_t *l, _z_bytes_t *remote_addr) { - _z_transport_peer_entry_t *ret = NULL; - - _z_transport_peer_entry_list_t *xs = l; - for (; xs != NULL; xs = _z_transport_peer_entry_list_tail(xs)) { - _z_transport_peer_entry_t *val = _z_transport_peer_entry_list_head(xs); - if (val->_remote_addr.len != remote_addr->len) { - continue; - } - - if (memcmp(val->_remote_addr.start, remote_addr->start, remote_addr->len) == 0) { - ret = val; - } - } - return ret; -} - /*------------------ Reception helper ------------------*/ int8_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr) { _Z_DEBUG(">> recv session msg\n"); @@ -81,44 +64,22 @@ int8_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_messa _z_mutex_lock(&ztm->_mutex_rx); #endif // Z_FEATURE_MULTI_THREAD == 1 - size_t to_read = 0; - do { - switch (ztm->_link._cap._flow) { - case Z_LINK_CAP_FLOW_STREAM: - if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_raweth_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); - if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_zbuf_compact(&ztm->_zbuf); - ret = _Z_ERR_TRANSPORT_NOT_ENOUGH_BYTES; - break; - } - } - for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) { - to_read |= _z_zbuf_read(&ztm->_zbuf) << (i * (uint8_t)8); - } - if (_z_zbuf_len(&ztm->_zbuf) < to_read) { - _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); - if (_z_zbuf_len(&ztm->_zbuf) < to_read) { - _z_zbuf_set_rpos(&ztm->_zbuf, _z_zbuf_get_rpos(&ztm->_zbuf) - _Z_MSG_LEN_ENC_SIZE); - _z_zbuf_compact(&ztm->_zbuf); - ret = _Z_ERR_TRANSPORT_NOT_ENOUGH_BYTES; - break; - } - } - break; - // Datagram capable links - case Z_LINK_CAP_FLOW_DATAGRAM: - _z_zbuf_compact(&ztm->_zbuf); - to_read = _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); - if (to_read == SIZE_MAX) { - ret = _Z_ERR_TRANSPORT_RX_FAILED; - } - break; - default: - break; + switch (ztm->_link._cap._flow) { + // Datagram capable links + case Z_LINK_CAP_FLOW_DATAGRAM: { + _z_zbuf_compact(&ztm->_zbuf); + // Read from link + size_t to_read = _z_raweth_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); + if (to_read == SIZE_MAX) { + ret = _Z_ERR_TRANSPORT_RX_FAILED; + } + break; } - } while (false); // The 1-iteration loop to use continue to break the entire loop on error - + default: + ret = _Z_ERR_GENERIC; + break; + } + // Decode message if (ret == _Z_RES_OK) { _Z_DEBUG(">> \t transport_message_decode: %ju\n", (uintmax_t)_z_zbuf_len(&ztm->_zbuf)); ret = _z_transport_message_decode(t_msg, &ztm->_zbuf); @@ -135,218 +96,6 @@ int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_ return _z_raweth_recv_t_msg_na(ztm, t_msg, addr); } -int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, - _z_bytes_t *addr) { - int8_t ret = _Z_RES_OK; -#if Z_FEATURE_MULTI_THREAD == 1 - // Acquire and keep the lock - _z_mutex_lock(&ztm->_mutex_peer); -#endif // Z_FEATURE_MULTI_THREAD == 1 - - // Mark the session that we have received data from this peer - _z_transport_peer_entry_t *entry = _z_find_peer_entry(ztm->_peers, addr); - switch (_Z_MID(t_msg->_header)) { - case _Z_MID_T_FRAME: { - _Z_INFO("Received _Z_FRAME message\n"); - if (entry == NULL) { - break; - } - entry->_received = true; - - // Check if the SN is correct - if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAME_R) == true) { - // @TODO: amend once reliability is in place. For the time being only - // monotonic SNs are ensured - if (_z_sn_precedes(entry->_sn_res, entry->_sn_rx_sns._val._plain._reliable, t_msg->_body._frame._sn) == - true) { - entry->_sn_rx_sns._val._plain._reliable = t_msg->_body._frame._sn; - } else { - _z_wbuf_clear(&entry->_dbuf_reliable); - _Z_INFO("Reliable message dropped because it is out of order\n"); - break; - } - } else { - if (_z_sn_precedes(entry->_sn_res, entry->_sn_rx_sns._val._plain._best_effort, - t_msg->_body._frame._sn) == true) { - entry->_sn_rx_sns._val._plain._best_effort = t_msg->_body._frame._sn; - } else { - _z_wbuf_clear(&entry->_dbuf_best_effort); - _Z_INFO("Best effort message dropped because it is out of order\n"); - break; - } - } - - // Handle all the zenoh message, one by one - uint16_t mapping = entry->_peer_id; - size_t len = _z_vec_len(&t_msg->_body._frame._messages); - for (size_t i = 0; i < len; i++) { - _z_network_message_t *zm = _z_network_message_vec_get(&t_msg->_body._frame._messages, i); - _z_msg_fix_mapping(zm, mapping); - _z_handle_network_message(ztm->_session, zm, mapping); - } - - break; - } - - case _Z_MID_T_FRAGMENT: { - _Z_INFO("Received Z_FRAGMENT message\n"); - if (entry == NULL) { - break; - } - entry->_received = true; - - _z_wbuf_t *dbuf = _Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAGMENT_R) - ? &entry->_dbuf_reliable - : &entry->_dbuf_best_effort; // Select the right defragmentation buffer - - _Bool drop = false; - if ((_z_wbuf_len(dbuf) + t_msg->_body._fragment._payload.len) > Z_FRAG_MAX_SIZE) { - // Filling the wbuf capacity as a way to signaling the last fragment to reset the dbuf - // Otherwise, last (smaller) fragments can be understood as a complete message - _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, _z_wbuf_space_left(dbuf)); - drop = true; - } else { - _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, - t_msg->_body._fragment._payload.len); - } - - if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAGMENT_M) == false) { - if (drop == true) { // Drop message if it exceeds the fragmentation size - _z_wbuf_reset(dbuf); - break; - } - - _z_zbuf_t zbf = _z_wbuf_to_zbuf(dbuf); // Convert the defragmentation buffer into a decoding buffer - - _z_zenoh_message_t zm; - ret = _z_network_message_decode(&zm, &zbf); - if (ret == _Z_RES_OK) { - uint16_t mapping = entry->_peer_id; - _z_msg_fix_mapping(&zm, mapping); - _z_handle_network_message(ztm->_session, &zm, mapping); - _z_msg_clear(&zm); // Clear must be explicitly called for fragmented zenoh messages. Non-fragmented - // zenoh messages are released when their transport message is released. - } - - // Free the decoding buffer - _z_zbuf_clear(&zbf); - // Reset the defragmentation buffer - _z_wbuf_reset(dbuf); - } - break; - } - - case _Z_MID_T_KEEP_ALIVE: { - _Z_INFO("Received _Z_KEEP_ALIVE message\n"); - if (entry == NULL) { - break; - } - entry->_received = true; - - break; - } - - case _Z_MID_T_INIT: { - // Do nothing, multicast transports are not expected to handle INIT messages - break; - } - - case _Z_MID_T_OPEN: { - // Do nothing, multicast transports are not expected to handle OPEN messages - break; - } - - case _Z_MID_T_JOIN: { - _Z_INFO("Received _Z_JOIN message\n"); - if (t_msg->_body._join._version != Z_PROTO_VERSION) { - break; - } - - if (entry == NULL) // New peer - { - entry = (_z_transport_peer_entry_t *)z_malloc(sizeof(_z_transport_peer_entry_t)); - if (entry != NULL) { - entry->_sn_res = _z_sn_max(t_msg->_body._join._seq_num_res); - - // If the new node has less representing capabilities then it is incompatible to communication - if ((t_msg->_body._join._seq_num_res != Z_SN_RESOLUTION) || - (t_msg->_body._join._req_id_res != Z_REQ_RESOLUTION) || - (t_msg->_body._join._batch_size != Z_BATCH_MULTICAST_SIZE)) { - ret = _Z_ERR_TRANSPORT_OPEN_SN_RESOLUTION; - } - - if (ret == _Z_RES_OK) { - entry->_remote_addr = _z_bytes_duplicate(addr); - entry->_remote_zid = t_msg->_body._join._zid; - - _z_conduit_sn_list_copy(&entry->_sn_rx_sns, &t_msg->_body._join._next_sn); - _z_conduit_sn_list_decrement(entry->_sn_res, &entry->_sn_rx_sns); - -#if Z_FEATURE_DYNAMIC_MEMORY_ALLOCATION == 1 - entry->_dbuf_reliable = _z_wbuf_make(0, true); - entry->_dbuf_best_effort = _z_wbuf_make(0, true); -#else - entry->_dbuf_reliable = _z_wbuf_make(Z_FRAG_MAX_SIZE, false); - entry->_dbuf_best_effort = _z_wbuf_make(Z_FRAG_MAX_SIZE, false); -#endif - - // Update lease time (set as ms during) - entry->_lease = t_msg->_body._join._lease; - entry->_next_lease = entry->_lease; - entry->_received = true; - - ztm->_peers = _z_transport_peer_entry_list_insert(ztm->_peers, entry); - } else { - z_free(entry); - } - } else { - ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; - } - } else { // Existing peer - entry->_received = true; - - // Check if the representing capabilities are still the same - if ((t_msg->_body._join._seq_num_res != Z_SN_RESOLUTION) || - (t_msg->_body._join._req_id_res != Z_REQ_RESOLUTION) || - (t_msg->_body._join._batch_size != Z_BATCH_MULTICAST_SIZE)) { - _z_transport_peer_entry_list_drop_filter(ztm->_peers, _z_transport_peer_entry_eq, entry); - // TODO: cleanup here should also be done on mappings/subs/etc... - break; - } - - // Update SNs - _z_conduit_sn_list_copy(&entry->_sn_rx_sns, &t_msg->_body._join._next_sn); - _z_conduit_sn_list_decrement(entry->_sn_res, &entry->_sn_rx_sns); - - // Update lease time (set as ms during) - entry->_lease = t_msg->_body._join._lease; - } - break; - } - - case _Z_MID_T_CLOSE: { - _Z_INFO("Closing session as requested by the remote peer\n"); - - if (entry == NULL) { - break; - } - ztm->_peers = _z_transport_peer_entry_list_drop_filter(ztm->_peers, _z_transport_peer_entry_eq, entry); - - break; - } - - default: { - _Z_ERROR("Unknown session message ID\n"); - break; - } - } - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztm->_mutex_peer); -#endif // Z_FEATURE_MULTI_THREAD == 1 - - return ret; -} #else int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr) { _ZP_UNUSED(ztm); @@ -354,12 +103,4 @@ int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_ _ZP_UNUSED(addr); return _Z_ERR_TRANSPORT_NOT_AVAILABLE; } - -int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, - _z_bytes_t *addr) { - _ZP_UNUSED(ztm); - _ZP_UNUSED(t_msg); - _ZP_UNUSED(addr); - return _Z_ERR_TRANSPORT_NOT_AVAILABLE; -} #endif // Z_FEATURE_RAWETH_TRANSPORT == 1 diff --git a/src/transport/raweth/transport.c b/src/transport/raweth/transport.c index 4774514ea..21735dc7d 100644 --- a/src/transport/raweth/transport.c +++ b/src/transport/raweth/transport.c @@ -30,28 +30,6 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 -void _zp_raweth_fetch_zid(const _z_transport_t *zt, z_owned_closure_zid_t *callback) { - void *ctx = callback->context; - _z_transport_peer_entry_list_t *l = zt->_transport._multicast._peers; - for (; l != NULL; l = _z_transport_peer_entry_list_tail(l)) { - _z_transport_peer_entry_t *val = _z_transport_peer_entry_list_head(l); - z_id_t id = val->_remote_zid; - - callback->call(&id, ctx); - } -} - -void _zp_raweth_info_session(const _z_transport_t *zt, _z_config_t *ps) { - _z_transport_peer_entry_list_t *xs = zt->_transport._multicast._peers; - while (xs != NULL) { - _z_transport_peer_entry_t *peer = _z_transport_peer_entry_list_head(xs); - _z_bytes_t remote_zid = _z_bytes_wrap(peer->_remote_zid.id, _z_id_len(peer->_remote_zid)); - _zp_config_insert(ps, Z_INFO_PEER_PID_KEY, _z_string_from_bytes(&remote_zid)); - - xs = _z_transport_peer_entry_list_tail(xs); - } -} - int8_t _z_raweth_transport_create(_z_transport_t *zt, _z_link_t *zl, _z_transport_multicast_establish_param_t *param) { int8_t ret = _Z_RES_OK; From b7a009edf0f567da64af02810d6eeeeafa7ed50c Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Tue, 28 Nov 2023 16:05:37 +0100 Subject: [PATCH 26/38] fix: use raweth link send for join message --- include/zenoh-pico/transport/raweth/rx.h | 2 -- src/transport/raweth/transport.c | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/include/zenoh-pico/transport/raweth/rx.h b/include/zenoh-pico/transport/raweth/rx.h index c1412196a..a8f281369 100644 --- a/include/zenoh-pico/transport/raweth/rx.h +++ b/include/zenoh-pico/transport/raweth/rx.h @@ -19,7 +19,5 @@ int8_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr); int8_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_bytes_t *addr); -int8_t _z_raweth_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, - _z_bytes_t *addr); #endif /* ZENOH_PICO_RAWETH_RX_H */ diff --git a/src/transport/raweth/transport.c b/src/transport/raweth/transport.c index 21735dc7d..b8c4bc876 100644 --- a/src/transport/raweth/transport.c +++ b/src/transport/raweth/transport.c @@ -123,7 +123,7 @@ int8_t _z_raweth_open_peer(_z_transport_multicast_establish_param_t *param, cons // Encode and send the message _Z_INFO("Sending Z_JOIN message\n"); - ret = _z_link_send_t_msg(zl, &jsm); + ret = _z_raweth_send_t_msg(zl, &jsm); _z_t_msg_clear(&jsm); if (ret == _Z_RES_OK) { From a98725e8119e996062224e267461e50163cfc328 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Tue, 28 Nov 2023 16:19:56 +0100 Subject: [PATCH 27/38] fix: add missing send function --- include/zenoh-pico/transport/raweth/tx.h | 1 + src/transport/raweth/transport.c | 2 +- src/transport/raweth/tx.c | 58 ++++++++++++++++-------- 3 files changed, 40 insertions(+), 21 deletions(-) diff --git a/include/zenoh-pico/transport/raweth/tx.h b/include/zenoh-pico/transport/raweth/tx.h index 2595e78eb..8146e38e4 100644 --- a/include/zenoh-pico/transport/raweth/tx.h +++ b/include/zenoh-pico/transport/raweth/tx.h @@ -18,6 +18,7 @@ #include "zenoh-pico/net/session.h" #include "zenoh-pico/transport/transport.h" +int8_t _z_raweth_link_send_t_msg(const _z_link_t *zl, const _z_transport_message_t *t_msg); int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, z_congestion_control_t cong_ctrl); int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg); diff --git a/src/transport/raweth/transport.c b/src/transport/raweth/transport.c index b8c4bc876..7dced540d 100644 --- a/src/transport/raweth/transport.c +++ b/src/transport/raweth/transport.c @@ -123,7 +123,7 @@ int8_t _z_raweth_open_peer(_z_transport_multicast_establish_param_t *param, cons // Encode and send the message _Z_INFO("Sending Z_JOIN message\n"); - ret = _z_raweth_send_t_msg(zl, &jsm); + ret = _z_raweth_link_send_t_msg(zl, &jsm); _z_t_msg_clear(&jsm); if (ret == _Z_RES_OK) { diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index 2834f5ff9..a65e27e53 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -112,15 +112,15 @@ static int8_t __unsafe_z_raweth_write_header(_z_transport_multicast_t *ztm) { return _Z_RES_OK; } -static int8_t _z_raweth_link_send_wbuf(const _z_transport_multicast_t *ztm) { +static int8_t _z_raweth_link_send_wbuf(const _z_link_t *zl, const _z_wbuf_t *wbf) { int8_t ret = _Z_RES_OK; - for (size_t i = 0; (i < _z_wbuf_len_iosli(&ztm->_wbuf)) && (ret == _Z_RES_OK); i++) { - _z_bytes_t bs = _z_iosli_to_bytes(_z_wbuf_get_iosli(&ztm->_wbuf, i)); + for (size_t i = 0; (i < _z_wbuf_len_iosli(wbf)) && (ret == _Z_RES_OK); i++) { + _z_bytes_t bs = _z_iosli_to_bytes(_z_wbuf_get_iosli(wbf, i)); size_t n = bs.len; do { // Retrieve addr from config + vlan tag above (locator) - size_t wb = _z_send_raweth(&ztm->_link._socket._raweth._sock, bs.start, n); // Unix + size_t wb = _z_send_raweth(&zl->_socket._raweth._sock, bs.start, n); // Unix if (wb == SIZE_MAX) { return _Z_ERR_TRANSPORT_TX_FAILED; } @@ -131,19 +131,37 @@ static int8_t _z_raweth_link_send_wbuf(const _z_transport_multicast_t *ztm) { return ret; } -// static void _z_raweth_check_config(_z_transport_multicast_t *ztm, const _z_keyexpr_t *key) { -// // Check config -// if (ztm->_link._socket._raweth._config != NULL) { -// // Check key -// if ((key != NULL)) { -// // Get send info from config keyexpr mapping -// } else { -// // Get send info from config default values -// } -// } else { -// // Nothing to do -// } -// } +int8_t _z_raweth_link_send_t_msg(const _z_link_t *zl, const _z_transport_message_t *t_msg) { + int8_t ret = _Z_RES_OK; + + // Create and prepare the buffer to serialize the message on + uint16_t mtu = (zl->_mtu < Z_BATCH_UNICAST_SIZE) ? zl->_mtu : Z_BATCH_UNICAST_SIZE; + _z_wbuf_t wbf = _z_wbuf_make(mtu, false); + + switch (zl->_cap._flow) { + case Z_LINK_CAP_FLOW_DATAGRAM: + break; + default: + ret = _Z_ERR_GENERIC; + break; + } + // Encode the session message + ret = _z_transport_message_encode(&wbf, t_msg); + if (ret == _Z_RES_OK) { + switch (zl->_cap._flow) { + case Z_LINK_CAP_FLOW_DATAGRAM: + break; + default: + ret = _Z_ERR_GENERIC; + break; + } + // Send the wbuf on the socket + ret = _z_raweth_link_send_wbuf(zl, &wbf); + } + _z_wbuf_clear(&wbf); + + return ret; +} int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg) { int8_t ret = _Z_RES_OK; @@ -159,7 +177,7 @@ int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_me // Encode the session message _Z_RETURN_IF_ERR(_z_transport_message_encode(&ztm->_wbuf, t_msg)); // Send the wbuf on the socket - _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(ztm)); + _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(&ztm->_link, &ztm->_wbuf)); // Mark the session that we have transmitted data ztm->_transmitted = true; @@ -220,7 +238,7 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, ret = _z_network_message_encode(&ztm->_wbuf, n_msg); if (ret == _Z_RES_OK) { // Send the wbuf on the socket - _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(ztm)); + _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(&ztm->_link, &ztm->_wbuf)); // Mark the session that we have transmitted data ztm->_transmitted = true; } else { // The message does not fit in the current batch, let's fragment it @@ -241,7 +259,7 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, // Serialize one fragment _Z_RETURN_IF_ERR(__unsafe_z_serialize_zenoh_fragment(&ztm->_wbuf, &fbf, reliability, sn)); // Send the wbuf on the socket - _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(ztm)); + _Z_RETURN_IF_ERR(_z_raweth_link_send_wbuf(&ztm->_link, &ztm->_wbuf)); // Mark the session that we have transmitted data ztm->_transmitted = true; } From 821cc36921b80f196d385904adf7aaebd9933652 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Tue, 28 Nov 2023 16:54:38 +0100 Subject: [PATCH 28/38] fix: missing eth header on link send function --- src/transport/raweth/tx.c | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index a65e27e53..2c6866f36 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -88,8 +88,8 @@ static _z_zint_t __unsafe_z_raweth_get_sn(_z_transport_multicast_t *ztm, z_relia * Make sure that the following mutexes are locked before calling this function: * - ztm->_mutex_inner */ -static int8_t __unsafe_z_raweth_write_header(_z_transport_multicast_t *ztm) { - _z_raweth_socket_t *resocket = &ztm->_link._socket._raweth; +static int8_t __unsafe_z_raweth_write_header(_z_link_t *zl, _z_wbuf_t *wbf) { + _z_raweth_socket_t *resocket = &zl->_socket._raweth; // Write eth header in buffer if (resocket->_has_vlan) { @@ -100,14 +100,14 @@ static int8_t __unsafe_z_raweth_write_header(_z_transport_multicast_t *ztm) { header.tag = _ZP_ETH_TYPE_VLAN; header.ethtype = _ZP_RAWETH_CFG_ETHTYPE; // Write header - _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(&ztm->_wbuf, (uint8_t *)&header, 0, sizeof(header))); + _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(wbf, (uint8_t *)&header, 0, sizeof(header))); } else { _zp_eth_header_t 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; // Write header - _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(&ztm->_wbuf, (uint8_t *)&header, 0, sizeof(header))); + _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(wbf, (uint8_t *)&header, 0, sizeof(header))); } return _Z_RES_OK; } @@ -145,6 +145,12 @@ int8_t _z_raweth_link_send_t_msg(const _z_link_t *zl, const _z_transport_message ret = _Z_ERR_GENERIC; break; } + // Discard const qualifier + _z_link_t *mzl = (_z_link_t *)zl; + // Set socket info + _zp_raweth_set_socket(NULL, &mzl->_socket._raweth); + // Write the message header + _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(mzl, &wbf)); // Encode the session message ret = _z_transport_message_encode(&wbf, t_msg); if (ret == _Z_RES_OK) { @@ -173,7 +179,7 @@ int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_me // Set socket info _zp_raweth_set_socket(NULL, &ztm->_link._socket._raweth); // Write the message header - _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); + _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_link, &ztm->_wbuf)); // Encode the session message _Z_RETURN_IF_ERR(_z_transport_message_encode(&ztm->_wbuf, t_msg)); // Send the wbuf on the socket @@ -228,7 +234,7 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, // Set socket info _zp_raweth_set_socket(keyexpr, &ztm->_link._socket._raweth); // Write the eth header - _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); + _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_link, &ztm->_wbuf)); // Set the frame header _z_zint_t sn = __unsafe_z_raweth_get_sn(ztm, reliability); _z_transport_message_t t_msg = _z_t_msg_make_frame_header(sn, reliability); @@ -255,7 +261,7 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, } is_first = false; // Write the eth header - _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(ztm)); + _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_link, &ztm->_wbuf)); // Serialize one fragment _Z_RETURN_IF_ERR(__unsafe_z_serialize_zenoh_fragment(&ztm->_wbuf, &fbf, reliability, sn)); // Send the wbuf on the socket From a019b145398dd9eaf0340b2a7c2cdd5aba8cf58b Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Wed, 29 Nov 2023 10:09:01 +0100 Subject: [PATCH 29/38] feat: add rx whitelist in config --- include/zenoh-pico/transport/raweth/config.h | 10 +++++++++- src/system/unix/link/raweth.c | 20 ++++++++++++++++---- src/transport/raweth/config.c | 7 +++++++ 3 files changed, 32 insertions(+), 5 deletions(-) diff --git a/include/zenoh-pico/transport/raweth/config.h b/include/zenoh-pico/transport/raweth/config.h index 7921f8389..a465c1e57 100644 --- a/include/zenoh-pico/transport/raweth/config.h +++ b/include/zenoh-pico/transport/raweth/config.h @@ -31,17 +31,25 @@ typedef struct { _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; // Source mac address extern const uint8_t _ZP_RAWETH_CFG_SMAC[_ZP_MAC_ADDR_LENGTH]; -// Sort the keyexpr alphabetically to use binary search (if size ~100+), otherwise use simple linear search +// 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 1774f301a..e19178bea 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -32,6 +32,7 @@ #include "zenoh-pico/collections/string.h" #include "zenoh-pico/config.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" @@ -94,14 +95,25 @@ size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t } size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buff_len, _z_bytes_t *addr) { + // Read from socket size_t bytesRead = recvfrom(sock->_fd, buff, buff_len, 0, NULL, NULL); - if (bytesRead < 0) { + 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; + } + } + // Ignore packet from unknown sources + if (!is_valid) { return SIZE_MAX; } - // Soft Filtering ? - // Copy sender mac if needed - if ((addr != NULL) && (bytesRead > 2 * ETH_ALEN)) { + if (addr != NULL) { *addr = _z_bytes_make(sizeof(ETH_ALEN)); (void)memcpy((uint8_t *)addr->start, (buff + ETH_ALEN), sizeof(ETH_ALEN)); } diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c index fb5186462..1132865dd 100644 --- a/src/transport/raweth/config.c +++ b/src/transport/raweth/config.c @@ -29,7 +29,14 @@ const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[] = { {{0, {0}, "another/keyexpr"}, 0x43, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true}, // entry2 }; +// 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}}, +}; + // 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 From 35342a52c1902341799ef2bc903e00361914100b Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Wed, 29 Nov 2023 11:40:01 +0100 Subject: [PATCH 30/38] fix: add missing close socket function --- include/zenoh-pico/system/link/raweth.h | 1 + src/system/unix/link/raweth.c | 8 ++++++++ src/transport/raweth/link.c | 2 +- 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index 41c6738fa..afc1c7506 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -58,6 +58,7 @@ int8_t _z_get_smac_raweth(_z_raweth_socket_t *resock); int8_t _z_open_raweth(_z_sys_net_socket_t *sock); 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); #endif diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index e19178bea..de4d4afc3 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -85,6 +85,14 @@ int8_t _z_open_raweth(_z_sys_net_socket_t *sock) { return ret; } +int8_t _z_close_raweth(_z_sys_net_socket_t *sock) { + int8_t ret = _Z_RES_OK; + if (close(sock->_fd) != 0) { + ret = _Z_ERR_GENERIC; + } + return ret; +} + size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t buff_len) { // Send data ssize_t wb = write(sock->_fd, buff, buff_len); diff --git a/src/transport/raweth/link.c b/src/transport/raweth/link.c index 23cf31156..162563567 100644 --- a/src/transport/raweth/link.c +++ b/src/transport/raweth/link.c @@ -78,7 +78,7 @@ static int8_t _z_f_link_open_raweth(_z_link_t *self) { static int8_t _z_f_link_listen_raweth(_z_link_t *self) { return _z_f_link_open_raweth(self); } -static void _z_f_link_close_raweth(_z_link_t *self) { _ZP_UNUSED(self); } +static void _z_f_link_close_raweth(_z_link_t *self) { _z_close_raweth(&self->_socket._raweth._sock); } static void _z_f_link_free_raweth(_z_link_t *self) { _ZP_UNUSED(self); } From 9ed1002e08e814572ccea75f74d82275a698dd3e Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Thu, 30 Nov 2023 12:28:59 +0100 Subject: [PATCH 31/38] feat: add config interface for socket binding --- include/zenoh-pico/system/link/raweth.h | 3 +- include/zenoh-pico/transport/raweth/config.h | 3 + src/system/unix/link/raweth.c | 64 ++++++++------------ src/transport/raweth/config.c | 3 + src/transport/raweth/link.c | 4 +- 5 files changed, 37 insertions(+), 40 deletions(-) diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index afc1c7506..e73a647c3 100644 --- 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 index a465c1e57..f199a4797 100644 --- 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 index de4d4afc3..745412da6 100644 --- 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 -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 index 1132865dd..646865f5a 100644 --- 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 index 162563567..b669e1f02 100644 --- 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); } From 92831877b31dab175f0b84a1ee8bf08e2a969599 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Thu, 30 Nov 2023 12:36:44 +0100 Subject: [PATCH 32/38] fix: add big endian comments --- include/zenoh-pico/transport/raweth/config.h | 2 +- src/transport/raweth/config.c | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/zenoh-pico/transport/raweth/config.h b/include/zenoh-pico/transport/raweth/config.h index f199a4797..c8d644747 100644 --- a/include/zenoh-pico/transport/raweth/config.h +++ b/include/zenoh-pico/transport/raweth/config.h @@ -26,7 +26,7 @@ typedef struct { _z_keyexpr_t _keyexpr; - uint16_t _vlan; // vlan tag (pcp + dei + id) + uint16_t _vlan; // vlan tag (pcp + dei + id), big endian uint8_t _dmac[_ZP_MAC_ADDR_LENGTH]; _Bool _has_vlan; } _zp_raweth_cfg_entry; diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c index 646865f5a..9497a3d9a 100644 --- a/src/transport/raweth/config.c +++ b/src/transport/raweth/config.c @@ -16,7 +16,7 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 -// Should be generated +// Should be generated (big endian format) const uint16_t _ZP_RAWETH_CFG_ETHTYPE = 0x72e0; // Should be generated @@ -27,9 +27,9 @@ const uint8_t _ZP_RAWETH_CFG_SMAC[_ZP_MAC_ADDR_LENGTH] = {0x30, 0x03, 0xc8, 0x37 // Should be generated const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[] = { - {{0, {0}, ""}, 0x00, {0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff}, false}, // Default mac addr - {{0, {0}, "some/key/expr"}, 0x8c, {0x00, 0x11, 0x22, 0x33, 0x44, 0x55}, true}, // entry1 - {{0, {0}, "another/keyexpr"}, 0x43, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true}, // entry2 + {{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}, "another/keyexpr"}, 0x4300, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true}, // entry2 }; // Should be generated From 07f8992d18f828b71bda583432ecd4194cbf88f7 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Thu, 30 Nov 2023 16:26:14 +0100 Subject: [PATCH 33/38] fix: add missing vlan ether type field --- include/zenoh-pico/system/link/raweth.h | 5 +++-- src/transport/raweth/tx.c | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index e73a647c3..d2009144e 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -22,8 +22,8 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 -// Ethernet types -#define _ZP_ETH_TYPE_VLAN 0x8100 +// Ethernet types (big endian) +#define _ZP_ETH_TYPE_VLAN 0x0081 // Address Sizes #define _ZP_MAC_ADDR_LENGTH 6 @@ -41,6 +41,7 @@ typedef struct { typedef struct { uint8_t dmac[_ZP_MAC_ADDR_LENGTH]; // Destination mac address uint8_t smac[_ZP_MAC_ADDR_LENGTH]; // Source mac address + uint16_t vlan_type; // Vlan ethtype uint16_t tag; // Vlan tag uint16_t ethtype; // Ethertype of frame } _zp_eth_vlan_header_t; diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index 2c6866f36..772c71d8c 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -97,7 +97,8 @@ static int8_t __unsafe_z_raweth_write_header(_z_link_t *zl, _z_wbuf_t *wbf) { memset(&header, 0, sizeof(header)); memcpy(&header.dmac, &resocket->_dmac, _ZP_MAC_ADDR_LENGTH); memcpy(&header.smac, &resocket->_smac, _ZP_MAC_ADDR_LENGTH); - header.tag = _ZP_ETH_TYPE_VLAN; + header.vlan_type = _ZP_ETH_TYPE_VLAN; + header.tag = resocket->_vlan; header.ethtype = _ZP_RAWETH_CFG_ETHTYPE; // Write header _Z_RETURN_IF_ERR(_z_wbuf_write_bytes(wbf, (uint8_t *)&header, 0, sizeof(header))); From 327dd39c19785caccdaa2d16ff7fd94031b14927 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Thu, 30 Nov 2023 16:39:51 +0100 Subject: [PATCH 34/38] fix: return error when keyexpr not found --- src/transport/raweth/config.c | 7 ++++--- src/transport/raweth/tx.c | 10 ++++++---- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c index 9497a3d9a..486853b1d 100644 --- a/src/transport/raweth/config.c +++ b/src/transport/raweth/config.c @@ -27,9 +27,10 @@ const uint8_t _ZP_RAWETH_CFG_SMAC[_ZP_MAC_ADDR_LENGTH] = {0x30, 0x03, 0xc8, 0x37 // 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}, "another/keyexpr"}, 0x4300, {0x01, 0x23, 0x45, 0x67, 0x89, 0xab}, true}, // entry2 + {{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 diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index 772c71d8c..3c32f20e9 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -50,7 +50,8 @@ int8_t _zp_raweth_set_socket(const _z_keyexpr_t *keyexpr, _z_raweth_socket_t *so 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)) { + if (zp_keyexpr_intersect_null_terminated(keyexpr->_suffix, _ZP_RAWETH_CFG_ARRAY[i]._keyexpr._suffix) != + _Z_RES_OK) { continue; } // Store data into socket @@ -61,6 +62,7 @@ int8_t _zp_raweth_set_socket(const _z_keyexpr_t *keyexpr, _z_raweth_socket_t *so memcpy(&sock->_vlan, &vlan, sizeof(vlan)); } ret = _Z_RES_OK; + break; } } return ret; @@ -149,7 +151,7 @@ int8_t _z_raweth_link_send_t_msg(const _z_link_t *zl, const _z_transport_message // Discard const qualifier _z_link_t *mzl = (_z_link_t *)zl; // Set socket info - _zp_raweth_set_socket(NULL, &mzl->_socket._raweth); + _Z_RETURN_IF_ERR(_zp_raweth_set_socket(NULL, &mzl->_socket._raweth)); // Write the message header _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(mzl, &wbf)); // Encode the session message @@ -178,7 +180,7 @@ int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_me _z_mutex_lock(&ztm->_mutex_tx); #endif // Set socket info - _zp_raweth_set_socket(NULL, &ztm->_link._socket._raweth); + _Z_RETURN_IF_ERR(_zp_raweth_set_socket(NULL, &ztm->_link._socket._raweth)); // Write the message header _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_link, &ztm->_wbuf)); // Encode the session message @@ -233,7 +235,7 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, break; } // Set socket info - _zp_raweth_set_socket(keyexpr, &ztm->_link._socket._raweth); + _Z_RETURN_IF_ERR(_zp_raweth_set_socket(keyexpr, &ztm->_link._socket._raweth)); // Write the eth header _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_link, &ztm->_wbuf)); // Set the frame header From 55cc05e634c3ea6142b475db595e54e11b322662 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 1 Dec 2023 12:22:22 +0100 Subject: [PATCH 35/38] fix: clear buffer before sending --- src/transport/raweth/config.c | 1 + src/transport/raweth/tx.c | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/src/transport/raweth/config.c b/src/transport/raweth/config.c index 486853b1d..08e4684e7 100644 --- a/src/transport/raweth/config.c +++ b/src/transport/raweth/config.c @@ -37,6 +37,7 @@ const _zp_raweth_cfg_entry _ZP_RAWETH_CFG_ARRAY[] = { 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 diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index 3c32f20e9..bba3153aa 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -179,6 +179,8 @@ int8_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_me #if Z_FEATURE_MULTI_THREAD == 1 _z_mutex_lock(&ztm->_mutex_tx); #endif + // Reset wbuf + _z_wbuf_reset(&ztm->_wbuf); // Set socket info _Z_RETURN_IF_ERR(_zp_raweth_set_socket(NULL, &ztm->_link._socket._raweth)); // Write the message header @@ -234,6 +236,8 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, default: break; } + // Reset wbuf + _z_wbuf_reset(&ztm->_wbuf); // Set socket info _Z_RETURN_IF_ERR(_zp_raweth_set_socket(keyexpr, &ztm->_link._socket._raweth)); // Write the eth header @@ -263,6 +267,8 @@ int8_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, sn = __unsafe_z_raweth_get_sn(ztm, reliability); } is_first = false; + // Reset wbuf + _z_wbuf_reset(&ztm->_wbuf); // Write the eth header _Z_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_link, &ztm->_wbuf)); // Serialize one fragment From 66ca132dc39659365936ba10c327eebca3acbfa2 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 1 Dec 2023 14:56:00 +0100 Subject: [PATCH 36/38] fix: remove read mutex double lock --- src/transport/raweth/lease.c | 10 +++++----- src/transport/raweth/read.c | 16 +++++----------- src/transport/raweth/rx.c | 3 +++ 3 files changed, 13 insertions(+), 16 deletions(-) diff --git a/src/transport/raweth/lease.c b/src/transport/raweth/lease.c index 672c04def..10fa08a8f 100644 --- a/src/transport/raweth/lease.c +++ b/src/transport/raweth/lease.c @@ -73,18 +73,18 @@ int8_t _zp_raweth_start_lease_task(_z_transport_t *zt, _z_task_attr_t *attr, _z_ // Init memory (void)memset(task, 0, sizeof(_z_task_t)); // Attach task - zt->_transport._multicast._lease_task = task; - zt->_transport._multicast._lease_task_running = true; + zt->_transport._raweth._lease_task = task; + zt->_transport._raweth._lease_task_running = true; // Init task - if (_z_task_init(task, attr, _zp_raweth_lease_task, &zt->_transport._multicast) != _Z_RES_OK) { - zt->_transport._multicast._lease_task_running = false; + if (_z_task_init(task, attr, _zp_raweth_lease_task, &zt->_transport._raweth) != _Z_RES_OK) { + zt->_transport._raweth._lease_task_running = false; return _Z_ERR_SYSTEM_TASK_FAILED; } return _Z_RES_OK; } int8_t _zp_raweth_stop_lease_task(_z_transport_t *zt) { - zt->_transport._multicast._lease_task_running = false; + zt->_transport._raweth._lease_task_running = false; return _Z_RES_OK; } diff --git a/src/transport/raweth/read.c b/src/transport/raweth/read.c index cf4761f56..c8c6d2ba4 100644 --- a/src/transport/raweth/read.c +++ b/src/transport/raweth/read.c @@ -43,18 +43,18 @@ int8_t _zp_raweth_start_read_task(_z_transport_t *zt, _z_task_attr_t *attr, _z_t // Init memory (void)memset(task, 0, sizeof(_z_task_t)); // Attach task - zt->_transport._multicast._read_task = task; - zt->_transport._multicast._read_task_running = true; + zt->_transport._raweth._read_task = task; + zt->_transport._raweth._read_task_running = true; // Init task - if (_z_task_init(task, attr, _zp_raweth_read_task, &zt->_transport._multicast) != _Z_RES_OK) { - zt->_transport._multicast._read_task_running = false; + if (_z_task_init(task, attr, _zp_raweth_read_task, &zt->_transport._raweth) != _Z_RES_OK) { + zt->_transport._raweth._read_task_running = false; return _Z_ERR_SYSTEM_TASK_FAILED; } return _Z_RES_OK; } int8_t _zp_raweth_stop_read_task(_z_transport_t *zt) { - zt->_transport._multicast._read_task_running = false; + zt->_transport._raweth._read_task_running = false; return _Z_RES_OK; } @@ -64,10 +64,6 @@ void *_zp_raweth_read_task(void *ztm_arg) { _z_transport_message_t t_msg; _z_bytes_t addr = _z_bytes_wrap(NULL, 0); - // Acquire and keep the lock - _z_mutex_lock(&ztm->_mutex_rx); - // Prepare the buffer - _z_zbuf_reset(&ztm->_zbuf); // Task loop while (ztm->_read_task_running == true) { // Read message from link @@ -87,8 +83,6 @@ void *_zp_raweth_read_task(void *ztm_arg) { _z_t_msg_clear(&t_msg); _z_bytes_clear(&addr); } - - _z_mutex_unlock(&ztm->_mutex_rx); #endif // Z_FEATURE_MULTI_THREAD == 1 return NULL; diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c index ef189b4f4..180106c58 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/rx.c @@ -64,6 +64,9 @@ int8_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_messa _z_mutex_lock(&ztm->_mutex_rx); #endif // Z_FEATURE_MULTI_THREAD == 1 + // Prepare the buffer + _z_zbuf_reset(&ztm->_zbuf); + switch (ztm->_link._cap._flow) { // Datagram capable links case Z_LINK_CAP_FLOW_DATAGRAM: { From 24b1d1f135e23953a97f8dc1d0de507af0bde310 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 1 Dec 2023 15:00:50 +0100 Subject: [PATCH 37/38] fix: run clang --- src/transport/raweth/rx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c index 180106c58..5e1710c3f 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/rx.c @@ -66,7 +66,7 @@ int8_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_messa // Prepare the buffer _z_zbuf_reset(&ztm->_zbuf); - + switch (ztm->_link._cap._flow) { // Datagram capable links case Z_LINK_CAP_FLOW_DATAGRAM: { From 1fb4344b50388358f2eb2c16198c069ffc6fc992 Mon Sep 17 00:00:00 2001 From: Jean-Roland Date: Fri, 1 Dec 2023 15:46:18 +0100 Subject: [PATCH 38/38] fix: add valid path to read task --- src/system/unix/link/raweth.c | 3 ++- src/transport/raweth/read.c | 20 ++++++++++++++------ src/transport/raweth/rx.c | 8 ++++++-- 3 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index 745412da6..c64376594 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -91,7 +91,7 @@ size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buff_len, _z_bytes_t *addr) { // Read from socket - size_t bytesRead = recvfrom(sock->_fd, buff, buff_len, 0, NULL, NULL); + ssize_t bytesRead = recvfrom(sock->_fd, buff, buff_len, 0, NULL, NULL); if ((bytesRead < 0) || (bytesRead < sizeof(_zp_eth_header_t))) { return SIZE_MAX; } @@ -101,6 +101,7 @@ size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buf 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; } } // Ignore packet from unknown sources diff --git a/src/transport/raweth/read.c b/src/transport/raweth/read.c index c8c6d2ba4..668b650f5 100644 --- a/src/transport/raweth/read.c +++ b/src/transport/raweth/read.c @@ -68,12 +68,20 @@ void *_zp_raweth_read_task(void *ztm_arg) { while (ztm->_read_task_running == true) { // Read message from link int8_t ret = _z_raweth_recv_t_msg(ztm, &t_msg, &addr); - if (ret == _Z_ERR_TRANSPORT_RX_FAILED) { - continue; - } else { - _Z_ERROR("Connection closed due to malformed message\n"); - ztm->_read_task_running = false; - continue; + switch (ret) { + case _Z_RES_OK: + // Process message + break; + case _Z_ERR_TRANSPORT_RX_FAILED: + // Drop message + continue; + break; + default: + // Drop message & stop task + _Z_ERROR("Connection closed due to malformed message\n"); + ztm->_read_task_running = false; + continue; + break; } // Process message if (_z_multicast_handle_transport_message(ztm, &t_msg, &addr) != _Z_RES_OK) { diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c index 5e1710c3f..1709ed67f 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/rx.c @@ -27,6 +27,10 @@ #if Z_FEATURE_RAWETH_TRANSPORT == 1 +void print_buf(_z_zbuf_t *buf) { + printf("Buff info: %ld, %ld, %ld\n", buf->_ios._r_pos, buf->_ios._w_pos, buf->_ios._capacity); +} + 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); @@ -47,9 +51,9 @@ static size_t _z_raweth_link_recv_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, _z // Update buffer but skip eth header _z_zbuf_set_wpos(zbf, _z_zbuf_get_wpos(zbf) + rb); if (has_vlan) { - _z_zbuf_set_rpos(zbf, _z_zbuf_get_wpos(zbf) - (rb - sizeof(_zp_eth_vlan_header_t))); + _z_zbuf_set_rpos(zbf, _z_zbuf_get_rpos(zbf) + sizeof(_zp_eth_vlan_header_t)); } else { - _z_zbuf_set_rpos(zbf, _z_zbuf_get_wpos(zbf) - (rb - sizeof(_zp_eth_header_t))); + _z_zbuf_set_rpos(zbf, _z_zbuf_get_rpos(zbf) + sizeof(_zp_eth_header_t)); } return rb; }