From f98ca92c0f9b2a534409f45f49ffe2df1710a13a Mon Sep 17 00:00:00 2001 From: Jean-Roland Gosse Date: Tue, 10 Dec 2024 16:51:09 +0100 Subject: [PATCH 1/4] fix: svec copy init (#826) --- src/collections/vec.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/collections/vec.c b/src/collections/vec.c index c03f18f09..733267e5b 100644 --- a/src/collections/vec.c +++ b/src/collections/vec.c @@ -171,8 +171,7 @@ void _z_svec_move(_z_svec_t *dst, _z_svec_t *src) { z_result_t _z_svec_copy(_z_svec_t *dst, const _z_svec_t *src, z_element_copy_f copy, size_t element_size, bool use_elem_f) { - dst->_capacity = 0; - dst->_len = 0; + *dst = _z_svec_null(); dst->_val = z_malloc(element_size * src->_capacity); if (dst->_val == NULL) { return _Z_ERR_SYSTEM_OUT_OF_MEMORY; From 916ec33c8e89bfe8f555609062245bce1db14e52 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Tue, 10 Dec 2024 17:16:59 +0100 Subject: [PATCH 2/4] FreeRTOS-Plus-TCP port fixes (#822) * Fix mutex drop and support static mutex allocation # Conflicts: # include/zenoh-pico/system/platform/freertos_plus_tcp.h * Mark realloc args as unused * Use _z_task_t as a task wrapper argument * Fix task cancelling * Fix task free * Use _Z_RES_OK instead of 0 * Use gettimeofday for z_time functions * Replace z_result returns with _Z_RES and _Z_ERR * Fix potential race condition in z_task_cancel * Fix race condition on task deletion --- .../system/platform/freertos_plus_tcp.h | 16 +- src/system/freertos_plus_tcp/system.c | 164 ++++++++++++------ 2 files changed, 127 insertions(+), 53 deletions(-) diff --git a/include/zenoh-pico/system/platform/freertos_plus_tcp.h b/include/zenoh-pico/system/platform/freertos_plus_tcp.h index 97d6c1b10..a0a76e906 100644 --- a/include/zenoh-pico/system/platform/freertos_plus_tcp.h +++ b/include/zenoh-pico/system/platform/freertos_plus_tcp.h @@ -14,6 +14,8 @@ #ifndef ZENOH_PICO_SYSTEM_FREERTOS_PLUS_TCP_TYPES_H #define ZENOH_PICO_SYSTEM_FREERTOS_PLUS_TCP_TYPES_H +#include + #include "FreeRTOS.h" #include "FreeRTOS_IP.h" #include "semphr.h" @@ -37,9 +39,19 @@ typedef struct { typedef struct { TaskHandle_t handle; EventGroupHandle_t join_event; + void *(*fun)(void *); + void *arg; +#if (configSUPPORT_STATIC_ALLOCATION == 1) + StaticEventGroup_t join_event_buffer; +#endif /* SUPPORT_STATIC_ALLOCATION */ } _z_task_t; -typedef SemaphoreHandle_t _z_mutex_t; +typedef struct { + SemaphoreHandle_t handle; +#if (configSUPPORT_STATIC_ALLOCATION == 1) + StaticSemaphore_t buffer; +#endif /* SUPPORT_STATIC_ALLOCATION */ +} _z_mutex_t; typedef struct { SemaphoreHandle_t mutex; SemaphoreHandle_t sem; @@ -52,7 +64,7 @@ typedef struct { #endif // Z_MULTI_THREAD == 1 typedef TickType_t z_clock_t; -typedef TickType_t z_time_t; +typedef struct timeval z_time_t; typedef struct { union { diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 395e6bc59..632a9b9c5 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -17,6 +17,7 @@ #include #include #include +#include #include "FreeRTOS_IP.h" #include "zenoh-pico/config.h" @@ -51,6 +52,8 @@ void z_random_fill(void *buf, size_t len) { void *z_malloc(size_t size) { return pvPortMalloc(size); } void *z_realloc(void *ptr, size_t size) { + _ZP_UNUSED(ptr); + _ZP_UNUSED(size); // realloc not implemented in FreeRTOS return NULL; } @@ -58,19 +61,21 @@ void *z_realloc(void *ptr, size_t size) { void z_free(void *ptr) { vPortFree(ptr); } #if Z_FEATURE_MULTI_THREAD == 1 -// In FreeRTOS, tasks created using xTaskCreate must end with vTaskDelete. -// A task function should __not__ simply return. -typedef struct { - void *(*fun)(void *); - void *arg; - EventGroupHandle_t join_event; -} z_task_arg; - +/*------------------ Thread ------------------*/ static void z_task_wrapper(void *arg) { - z_task_arg *targ = (z_task_arg *)arg; - targ->fun(targ->arg); - xEventGroupSetBits(targ->join_event, 1); - vTaskDelete(NULL); + _z_task_t *task = (_z_task_t *)arg; + + // Run the task function + task->fun(task->arg); + + // Notify the joiner that the task has finished + xEventGroupSetBits(task->join_event, 1); + + // In FreeRTOS, when a task deletes itself, it adds itself to a list of tasks awaiting to be terminated by the idle + // task. There is no guarantee when exactly that will happen, which could lead to race conditions on freeing the + // task resources. To avoid this, we suspend the task indefinitely and delete this task from another task running + // z_task_join or z_task_detach. + vTaskSuspend(NULL); } static z_task_attr_t z_default_task_attr = { @@ -86,14 +91,14 @@ static z_task_attr_t z_default_task_attr = { /*------------------ Thread ------------------*/ z_result_t _z_task_init(_z_task_t *task, z_task_attr_t *attr, void *(*fun)(void *), void *arg) { - z_task_arg *z_arg = (z_task_arg *)z_malloc(sizeof(z_task_arg)); - if (z_arg == NULL) { - return -1; - } + task->fun = fun; + task->arg = arg; - z_arg->fun = fun; - z_arg->arg = arg; - z_arg->join_event = task->join_event = xEventGroupCreate(); +#if (configSUPPORT_STATIC_ALLOCATION == 1) + task->join_event = xEventGroupCreateStatic(&task->join_event_buffer); +#else + task->join_event = xEventGroupCreate(); +#endif /* SUPPORT_STATIC_ALLOCATION */ if (attr == NULL) { attr = &z_default_task_attr; @@ -101,60 +106,86 @@ z_result_t _z_task_init(_z_task_t *task, z_task_attr_t *attr, void *(*fun)(void #if (configSUPPORT_STATIC_ALLOCATION == 1) if (attr->static_allocation) { - task->handle = xTaskCreateStatic(z_task_wrapper, attr->name, attr->stack_depth, z_arg, attr->priority, + task->handle = xTaskCreateStatic(z_task_wrapper, attr->name, attr->stack_depth, task, attr->priority, attr->stack_buffer, attr->task_buffer); if (task->handle == NULL) { - return -1; + return _Z_ERR_GENERIC; } } else { #endif /* SUPPORT_STATIC_ALLOCATION */ - if (xTaskCreate(z_task_wrapper, attr->name, attr->stack_depth, z_arg, attr->priority, &task->handle) != - pdPASS) { - return -1; + if (xTaskCreate(z_task_wrapper, attr->name, attr->stack_depth, task, attr->priority, &task->handle) != pdPASS) { + return _Z_ERR_GENERIC; } #if (configSUPPORT_STATIC_ALLOCATION == 1) } #endif /* SUPPORT_STATIC_ALLOCATION */ - return 0; + return _Z_RES_OK; } z_result_t _z_task_join(_z_task_t *task) { xEventGroupWaitBits(task->join_event, 1, pdFALSE, pdFALSE, portMAX_DELAY); - return 0; + + taskENTER_CRITICAL(); + if (task->handle != NULL) { + vTaskDelete(task->handle); + task->handle = NULL; + } + taskEXIT_CRITICAL(); + + return _Z_RES_OK; } z_result_t _z_task_detach(_z_task_t *task) { - // Not implemented - return _Z_ERR_GENERIC; + // Note: task/thread detach not supported on FreeRTOS API, so we force its deletion instead. + return _z_task_cancel(task); } z_result_t _z_task_cancel(_z_task_t *task) { - vTaskDelete(task->handle); - return 0; + taskENTER_CRITICAL(); + if (task->handle != NULL) { + vTaskDelete(task->handle); + task->handle = NULL; + } + taskEXIT_CRITICAL(); + + xEventGroupSetBits(task->join_event, 1); + + return _Z_RES_OK; } void _z_task_free(_z_task_t **task) { - z_free((*task)->join_event); - z_free(*task); + _z_task_t *ptr = *task; + vEventGroupDelete(ptr->join_event); + *task = NULL; } /*------------------ Mutex ------------------*/ z_result_t _z_mutex_init(_z_mutex_t *m) { - *m = xSemaphoreCreateRecursiveMutex(); - return *m == NULL ? -1 : 0; +#if (configSUPPORT_STATIC_ALLOCATION == 1) + m->handle = xSemaphoreCreateRecursiveMutexStatic(&m->buffer); +#else + m->handle = xSemaphoreCreateRecursiveMutex(); +#endif /* SUPPORT_STATIC_ALLOCATION */ + return m->handle == NULL ? _Z_ERR_GENERIC : _Z_RES_OK; } z_result_t _z_mutex_drop(_z_mutex_t *m) { - z_free(*m); - return 0; + vSemaphoreDelete(m->handle); + return _Z_RES_OK; } -z_result_t _z_mutex_lock(_z_mutex_t *m) { return xSemaphoreTakeRecursive(*m, portMAX_DELAY) == pdTRUE ? 0 : -1; } +z_result_t _z_mutex_lock(_z_mutex_t *m) { + return xSemaphoreTakeRecursive(m->handle, portMAX_DELAY) == pdTRUE ? _Z_RES_OK : _Z_ERR_GENERIC; +} -z_result_t _z_mutex_try_lock(_z_mutex_t *m) { return xSemaphoreTakeRecursive(*m, 0) == pdTRUE ? 0 : -1; } +z_result_t _z_mutex_try_lock(_z_mutex_t *m) { + return xSemaphoreTakeRecursive(m->handle, 0) == pdTRUE ? _Z_RES_OK : _Z_ERR_GENERIC; +} -z_result_t _z_mutex_unlock(_z_mutex_t *m) { return xSemaphoreGiveRecursive(*m) == pdTRUE ? 0 : -1; } +z_result_t _z_mutex_unlock(_z_mutex_t *m) { + return xSemaphoreGiveRecursive(m->handle) == pdTRUE ? _Z_RES_OK : _Z_ERR_GENERIC; +} /*------------------ CondVar ------------------*/ z_result_t _z_condvar_init(_z_condvar_t *cv) { @@ -238,45 +269,76 @@ z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { /*------------------ Sleep ------------------*/ z_result_t z_sleep_us(size_t time) { vTaskDelay(pdMS_TO_TICKS(time / 1000)); - return 0; + return _Z_RES_OK; } z_result_t z_sleep_ms(size_t time) { vTaskDelay(pdMS_TO_TICKS(time)); - return 0; + return _Z_RES_OK; } z_result_t z_sleep_s(size_t time) { vTaskDelay(pdMS_TO_TICKS(time * 1000)); - return 0; + return _Z_RES_OK; } /*------------------ Clock ------------------*/ -z_clock_t z_clock_now(void) { return z_time_now(); } +z_clock_t z_clock_now(void) { return xTaskGetTickCount(); } unsigned long z_clock_elapsed_us(z_clock_t *instant) { return z_clock_elapsed_ms(instant) * 1000; } -unsigned long z_clock_elapsed_ms(z_clock_t *instant) { return z_time_elapsed_ms(instant); } +unsigned long z_clock_elapsed_ms(z_clock_t *instant) { + z_clock_t now = z_clock_now(); + + unsigned long elapsed = (now - *instant) * portTICK_PERIOD_MS; + return elapsed; +} unsigned long z_clock_elapsed_s(z_clock_t *instant) { return z_clock_elapsed_ms(instant) / 1000; } /*------------------ Time ------------------*/ -z_time_t z_time_now(void) { return xTaskGetTickCount(); } +z_time_t z_time_now(void) { + z_time_t now; + gettimeofday(&now, NULL); + return now; +} const char *z_time_now_as_str(char *const buf, unsigned long buflen) { - snprintf(buf, buflen, "%u", z_time_now()); + z_time_t tv = z_time_now(); + struct tm ts; + ts = *localtime(&tv.tv_sec); + strftime(buf, buflen, "%Y-%m-%dT%H:%M:%SZ", &ts); return buf; } -unsigned long z_time_elapsed_us(z_time_t *time) { return z_time_elapsed_ms(time) * 1000; } +unsigned long z_time_elapsed_us(z_time_t *time) { + z_time_t now; + gettimeofday(&now, NULL); + + unsigned long elapsed = (unsigned long)(1000000 * (now.tv_sec - time->tv_sec) + (now.tv_usec - time->tv_usec)); + return elapsed; +} unsigned long z_time_elapsed_ms(z_time_t *time) { - z_time_t now = z_time_now(); + z_time_t now; + gettimeofday(&now, NULL); - unsigned long elapsed = (now - *time) * portTICK_PERIOD_MS; + unsigned long elapsed = (unsigned long)(1000 * (now.tv_sec - time->tv_sec) + (now.tv_usec - time->tv_usec) / 1000); return elapsed; } -unsigned long z_time_elapsed_s(z_time_t *time) { return z_time_elapsed_ms(time) / 1000; } +unsigned long z_time_elapsed_s(z_time_t *time) { + z_time_t now; + gettimeofday(&now, NULL); -z_result_t _z_get_time_since_epoch(_z_time_since_epoch *t) { return -1; } + unsigned long elapsed = (unsigned long)(now.tv_sec - time->tv_sec); + return elapsed; +} + +z_result_t _z_get_time_since_epoch(_z_time_since_epoch *t) { + z_time_t now; + gettimeofday(&now, NULL); + t->secs = (uint32_t)now.tv_sec; + t->nanos = (uint32_t)now.tv_usec * 1000; + return _Z_RES_OK; +} From 64cd525dfb19bf28f6253f6eddd3d5afeb98d9d9 Mon Sep 17 00:00:00 2001 From: Alexander Bushnev Date: Wed, 11 Dec 2024 11:45:40 +0100 Subject: [PATCH 3/4] Update and improve serial protocol (#823) * Update serial protocol * Remove buffers reusage to avoid conflicts --- .github/workflows/arduino_esp32.yaml | 2 +- .github/workflows/espidf.yaml | 2 +- CMakeLists.txt | 2 +- examples/rpi_pico/main.c | 3 + include/zenoh-pico/collections/arc_slice.h | 2 +- include/zenoh-pico/protocol/codec/serial.h | 33 ++++ .../zenoh-pico/protocol/definitions/serial.h | 61 ++++++++ .../{platform_common.h => common/platform.h} | 0 include/zenoh-pico/system/common/serial.h | 36 +++++ include/zenoh-pico/system/link/serial.h | 6 +- include/zenoh-pico/system/platform.h | 2 +- include/zenoh-pico/system/platform/espidf.h | 6 +- include/zenoh-pico/utils/logging.h | 2 +- src/api/api.c | 2 +- src/link/unicast/serial.c | 1 + src/protocol/codec/serial.c | 117 ++++++++++++++ src/system/arduino/esp32/network.cpp | 144 +++++------------- src/system/arduino/esp32/system.c | 1 - .../{platform_common.c => common/platform.c} | 2 +- src/system/common/serial.c | 83 ++++++++++ src/system/espidf/network.c | 127 ++++----------- src/system/flipper/network.c | 102 +++---------- src/system/mbed/network.cpp | 110 +++---------- src/system/rpi_pico/network.c | 121 ++++----------- src/system/unix/system.c | 1 - src/system/zephyr/network.c | 111 +++----------- tests/z_msgcodec_test.c | 69 +++++++++ tests/z_refcount_test.c | 2 +- zephyr/CMakeLists.txt | 2 +- 29 files changed, 582 insertions(+), 570 deletions(-) create mode 100644 include/zenoh-pico/protocol/codec/serial.h create mode 100644 include/zenoh-pico/protocol/definitions/serial.h rename include/zenoh-pico/system/{platform_common.h => common/platform.h} (100%) create mode 100644 include/zenoh-pico/system/common/serial.h create mode 100644 src/protocol/codec/serial.c rename src/system/{platform_common.c => common/platform.c} (98%) create mode 100644 src/system/common/serial.c diff --git a/.github/workflows/arduino_esp32.yaml b/.github/workflows/arduino_esp32.yaml index a1c386616..41e781d28 100644 --- a/.github/workflows/arduino_esp32.yaml +++ b/.github/workflows/arduino_esp32.yaml @@ -51,7 +51,7 @@ jobs: mkdir -p $ARDUINO_BASE cd $ARDUINO_BASE - platformio init -b esp32thing_plus -O "board_build.cmake_extra_args=-DZ_FEATURE_LINK_BLUETOOTH=1" -O "build_flags=-DZENOH_DEBUG=3 -DZENOH_COMPILER_GCC" -O "lib_ldf_mode=deep+" + platformio init -b esp32thing_plus -O "board_build.cmake_extra_args=-DZ_FEATURE_LINK_BLUETOOTH=1 -DZ_FEATURE_LINK_SERIAL=1" -O "build_flags=-DZENOH_DEBUG=3 -DZENOH_COMPILER_GCC" -O "lib_ldf_mode=deep+" cd $ARDUINO_BASE/lib ln -s $ZENOH_PICO_BASE diff --git a/.github/workflows/espidf.yaml b/.github/workflows/espidf.yaml index e0685732d..07097ec93 100644 --- a/.github/workflows/espidf.yaml +++ b/.github/workflows/espidf.yaml @@ -51,7 +51,7 @@ jobs: mkdir -p $ESPIDF_BASE cd $ESPIDF_BASE - platformio init -b az-delivery-devkit-v4 --project-option="framework=espidf" --project-option="build_flags=-DZENOH_ESPIDF -DZENOH_DEBUG=3" + platformio init -b az-delivery-devkit-v4 -O "board_build.cmake_extra_args=-DZ_FEATURE_LINK_SERIAL=1" --project-option="framework=espidf" --project-option="build_flags=-DZENOH_ESPIDF -DZENOH_DEBUG=3" cd $ESPIDF_BASE/lib ln -s $ZENOH_PICO_BASE diff --git a/CMakeLists.txt b/CMakeLists.txt index a4320a36a..4c32bce5d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -343,7 +343,7 @@ file(GLOB_RECURSE Sources "src/session/*.c" "src/transport/*.c" "src/utils/*.c" - "src/system/platform_common.c" + "src/system/common/*.c" ) if(WITH_ZEPHYR) diff --git a/examples/rpi_pico/main.c b/examples/rpi_pico/main.c index 5184f6b91..499e56f72 100644 --- a/examples/rpi_pico/main.c +++ b/examples/rpi_pico/main.c @@ -44,7 +44,10 @@ void print_ip_address() { void main_task(void *params) { (void)params; +#ifndef NDEBUG vTaskDelay(pdMS_TO_TICKS(3000)); +#endif + #if WIFI_SUPPORT_ENABLED if (cyw43_arch_init()) { printf("Failed to initialise\n"); diff --git a/include/zenoh-pico/collections/arc_slice.h b/include/zenoh-pico/collections/arc_slice.h index 782373141..cf075980e 100644 --- a/include/zenoh-pico/collections/arc_slice.h +++ b/include/zenoh-pico/collections/arc_slice.h @@ -22,7 +22,7 @@ #include "refcount.h" #include "slice.h" -#include "zenoh-pico/system/platform_common.h" +#include "zenoh-pico/system/common/platform.h" #ifdef __cplusplus extern "C" { diff --git a/include/zenoh-pico/protocol/codec/serial.h b/include/zenoh-pico/protocol/codec/serial.h new file mode 100644 index 000000000..d3a961a4e --- /dev/null +++ b/include/zenoh-pico/protocol/codec/serial.h @@ -0,0 +1,33 @@ +// +// Copyright (c) 2024 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 INCLUDE_ZENOH_PICO_PROTOCOL_CODEC_SERIAL_H +#define INCLUDE_ZENOH_PICO_PROTOCOL_CODEC_SERIAL_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +size_t _z_serial_msg_serialize(uint8_t *dest, size_t dest_len, const uint8_t *src, size_t src_len, uint8_t header, + uint8_t *tmp_buf, size_t tmp_buf_len); +size_t _z_serial_msg_deserialize(const uint8_t *src, size_t src_len, uint8_t *dst, size_t dst_len, uint8_t *header, + uint8_t *tmp_buf, size_t tmp_buf_len); + +#ifdef __cplusplus +} +#endif + +#endif /* INCLUDE_ZENOH_PICO_PROTOCOL_CODEC_SERIAL_H */ diff --git a/include/zenoh-pico/protocol/definitions/serial.h b/include/zenoh-pico/protocol/definitions/serial.h new file mode 100644 index 000000000..c00a46409 --- /dev/null +++ b/include/zenoh-pico/protocol/definitions/serial.h @@ -0,0 +1,61 @@ +// +// 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 INCLUDE_ZENOH_PICO_PROTOCOL_DEFINITIONS_SERIAL_H +#define INCLUDE_ZENOH_PICO_PROTOCOL_DEFINITIONS_SERIAL_H + +#include + +#include "zenoh-pico/link/endpoint.h" +#include "zenoh-pico/protocol/definitions/network.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/// ZSerial Frame Format +/// +/// Using COBS +/// +/// +-+-+----+------------+--------+-+ +/// |O|H|XXXX|ZZZZ....ZZZZ|CCCCCCCC|0| +/// +-+----+------------+--------+-+ +/// |O| |Len | Data | CRC32 |C| +/// +-+-+-2--+----N-------+---4----+-+ +/// +/// Header: 1byte +/// +---------------+ +/// |7|6|5|4|3|2|1|0| +/// +---------------+ +/// |x|x|x|x|x|R|A|I| +/// +---------------+ +/// +/// Flags: +/// I - Init +/// A - Ack +/// R - Reset +/// +/// Max Frame Size: 1510 +/// Max MTU: 1500 +/// Max On-the-wire length: 1516 (MFS + Overhead Byte (OHB) + Kind Byte + End of packet (EOP)) + +#define _Z_FLAG_SERIAL_INIT 0x01 +#define _Z_FLAG_SERIAL_ACK 0x02 +#define _Z_FLAG_SERIAL_RESET 0x04 + +#ifdef __cplusplus +} +#endif + +#endif /* INCLUDE_ZENOH_PICO_PROTOCOL_DEFINITIONS_SERIAL_H*/ diff --git a/include/zenoh-pico/system/platform_common.h b/include/zenoh-pico/system/common/platform.h similarity index 100% rename from include/zenoh-pico/system/platform_common.h rename to include/zenoh-pico/system/common/platform.h diff --git a/include/zenoh-pico/system/common/serial.h b/include/zenoh-pico/system/common/serial.h new file mode 100644 index 000000000..4802c7406 --- /dev/null +++ b/include/zenoh-pico/system/common/serial.h @@ -0,0 +1,36 @@ +// +// Copyright (c) 2024 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_COMMON_SERIAL_H +#define ZENOH_PICO_SYSTEM_COMMON_SERIAL_H + +#include + +#include "zenoh-pico/system/common/platform.h" +#include "zenoh-pico/utils/result.h" + +#ifdef __cplusplus +extern "C" { +#endif + +z_result_t _z_connect_serial(const _z_sys_net_socket_t sock); +size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len); +size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len); +size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len); + +#ifdef __cplusplus +} +#endif + +#endif /* ZENOH_PICO_SYSTEM_COMMON_SERIAL_H */ diff --git a/include/zenoh-pico/system/link/serial.h b/include/zenoh-pico/system/link/serial.h index 21679d996..951f1fad8 100644 --- a/include/zenoh-pico/system/link/serial.h +++ b/include/zenoh-pico/system/link/serial.h @@ -28,7 +28,7 @@ extern "C" { #if Z_FEATURE_LINK_SERIAL == 1 #define _Z_SERIAL_MTU_SIZE 1500 -#define _Z_SERIAL_MFS_SIZE _Z_SERIAL_MTU_SIZE + 2 + 4 // MTU + Serial Len + Serial CRC32 +#define _Z_SERIAL_MFS_SIZE _Z_SERIAL_MTU_SIZE + 1 + 2 + 4 // MTU + Header + Serial Len + Serial CRC32 #define _Z_SERIAL_MAX_COBS_BUF_SIZE \ 1516 // Max On-the-wire length for an MFS/MTU of 1510/1500 (MFS + Overhead Byte (OHB) + End of packet (EOP)) @@ -42,8 +42,8 @@ z_result_t _z_listen_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, z_result_t _z_listen_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t baudrate); void _z_close_serial(_z_sys_net_socket_t *sock); size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len); -size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len); -size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len); +size_t _z_read_serial_internal(const _z_sys_net_socket_t sock, uint8_t *header, uint8_t *ptr, size_t len); +size_t _z_send_serial_internal(const _z_sys_net_socket_t sock, uint8_t header, const uint8_t *ptr, size_t len); #endif diff --git a/include/zenoh-pico/system/platform.h b/include/zenoh-pico/system/platform.h index 15e49548e..98f023bcc 100644 --- a/include/zenoh-pico/system/platform.h +++ b/include/zenoh-pico/system/platform.h @@ -18,6 +18,6 @@ #include #include "zenoh-pico/config.h" -#include "zenoh-pico/system/platform_common.h" +#include "zenoh-pico/system/common/platform.h" #endif /* ZENOH_PICO_SYSTEM_PLATFORM_H */ diff --git a/include/zenoh-pico/system/platform/espidf.h b/include/zenoh-pico/system/platform/espidf.h index aadeb782f..eaca83742 100644 --- a/include/zenoh-pico/system/platform/espidf.h +++ b/include/zenoh-pico/system/platform/espidf.h @@ -56,11 +56,7 @@ typedef struct { int _fd; #endif #if Z_FEATURE_LINK_SERIAL == 1 - struct { - uart_port_t _serial; - uint8_t *before_cobs; - uint8_t *after_cobs; - }; + uart_port_t _serial; #endif }; } _z_sys_net_socket_t; diff --git a/include/zenoh-pico/utils/logging.h b/include/zenoh-pico/utils/logging.h index b50110bdf..a5ea4848b 100644 --- a/include/zenoh-pico/utils/logging.h +++ b/include/zenoh-pico/utils/logging.h @@ -17,7 +17,7 @@ #include -#include "zenoh-pico/system/platform_common.h" +#include "zenoh-pico/system/common/platform.h" #ifdef __cplusplus extern "C" { diff --git a/src/api/api.c b/src/api/api.c index f272b6943..4def12579 100644 --- a/src/api/api.c +++ b/src/api/api.c @@ -37,8 +37,8 @@ #include "zenoh-pico/session/resource.h" #include "zenoh-pico/session/subscription.h" #include "zenoh-pico/session/utils.h" +#include "zenoh-pico/system/common/platform.h" #include "zenoh-pico/system/platform.h" -#include "zenoh-pico/system/platform_common.h" #include "zenoh-pico/transport/common/tx.h" #include "zenoh-pico/transport/multicast.h" #include "zenoh-pico/transport/unicast.h" diff --git a/src/link/unicast/serial.c b/src/link/unicast/serial.c index 7cc16be15..17d59a5c0 100644 --- a/src/link/unicast/serial.c +++ b/src/link/unicast/serial.c @@ -20,6 +20,7 @@ #include "zenoh-pico/config.h" #include "zenoh-pico/link/manager.h" +#include "zenoh-pico/system/common/serial.h" #include "zenoh-pico/system/link/serial.h" #include "zenoh-pico/utils/pointers.h" diff --git a/src/protocol/codec/serial.c b/src/protocol/codec/serial.c new file mode 100644 index 000000000..76356ecdd --- /dev/null +++ b/src/protocol/codec/serial.c @@ -0,0 +1,117 @@ +// +// Copyright (c) 2024 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 "zenoh-pico/utils/checksum.h" +#include "zenoh-pico/utils/encoding.h" +#include "zenoh-pico/utils/logging.h" +#include "zenoh-pico/utils/pointers.h" + +#define KIND_FIELD_LEN 1u +#define LEN_FIELD_LEN 2u +#define CRC32_LEN 4u + +size_t _z_serial_msg_serialize(uint8_t *dest, size_t dest_len, const uint8_t *src, size_t src_len, uint8_t header, + uint8_t *tmp_buf, size_t tmp_buf_len) { + size_t expected_size = src_len + KIND_FIELD_LEN + LEN_FIELD_LEN + CRC32_LEN; + if (tmp_buf_len < expected_size) { + _Z_DEBUG("tmp buffer too small: %zu < %zu", tmp_buf_len, expected_size); + return SIZE_MAX; + } + + uint32_t crc32 = _z_crc32(src, src_len); + uint8_t crc_bytes[CRC32_LEN] = {(uint8_t)(crc32 & 0xFF), (uint8_t)((crc32 >> 8) & 0xFF), + (uint8_t)((crc32 >> 16) & 0xFF), (uint8_t)((crc32 >> 24) & 0xFF)}; + + uint16_t wire_size = (uint16_t)src_len; + uint8_t size_bytes[LEN_FIELD_LEN] = {(uint8_t)(wire_size & 0xFF), (uint8_t)((wire_size >> 8) & 0xFF)}; + + uint8_t *tmp_buf_ptr = tmp_buf; + + tmp_buf_ptr[0] = header; + tmp_buf_ptr += sizeof(header); + + memcpy(tmp_buf_ptr, size_bytes, sizeof(size_bytes)); + tmp_buf_ptr += sizeof(size_bytes); + + memcpy(tmp_buf_ptr, src, src_len); + tmp_buf_ptr += src_len; + + memcpy(tmp_buf_ptr, crc_bytes, sizeof(crc_bytes)); + tmp_buf_ptr += sizeof(crc_bytes); + + size_t total_len = _z_ptr_u8_diff(tmp_buf_ptr, tmp_buf); + + size_t ret = _z_cobs_encode(tmp_buf, total_len, dest); + if (ret + 1 > dest_len) { + _Z_DEBUG("destination buffer too small"); + return SIZE_MAX; + } + + dest[ret] = 0x00; + + return ret + 1u; +} + +size_t _z_serial_msg_deserialize(const uint8_t *src, size_t src_len, uint8_t *dst, size_t dst_len, uint8_t *header, + uint8_t *tmp_buf, size_t tmp_buf_len) { + if (tmp_buf_len < src_len) { + _Z_DEBUG("tmp_buf too small"); + return SIZE_MAX; + } + + size_t decoded_size = _z_cobs_decode(src, src_len, tmp_buf); + + if (decoded_size < KIND_FIELD_LEN + LEN_FIELD_LEN + CRC32_LEN) { + _Z_DEBUG("decoded frame too small"); + return SIZE_MAX; + } + + uint8_t *tmp_buf_ptr = tmp_buf; + + *header = tmp_buf_ptr[0]; + tmp_buf_ptr += sizeof(uint8_t); + + uint16_t wire_size = tmp_buf_ptr[0] | (tmp_buf_ptr[1] << 8); + tmp_buf_ptr += sizeof(uint16_t); + + size_t expected_size = wire_size + KIND_FIELD_LEN + LEN_FIELD_LEN + CRC32_LEN; + if (expected_size != decoded_size) { + _Z_DEBUG("wire size mismatch: %zu != %zu", expected_size, decoded_size); + return SIZE_MAX; + } + + if (dst_len < wire_size) { + _Z_DEBUG("destination buffer too small: %zu < %u", dst_len, wire_size); + return SIZE_MAX; + } + + if (wire_size != 0) { + memcpy(dst, tmp_buf_ptr, wire_size); + tmp_buf_ptr += wire_size; + } + + uint32_t received_crc = tmp_buf_ptr[0] | (tmp_buf_ptr[1] << 8) | (tmp_buf_ptr[2] << 16) | (tmp_buf_ptr[3] << 24); + + uint32_t computed_crc = _z_crc32(dst, wire_size); + if (received_crc != computed_crc) { + _Z_DEBUG("CRC mismatch. Received: 0x%08" PRIu32 ", Computed: 0x%08" PRIu32, received_crc, computed_crc); + return SIZE_MAX; + } + + return wire_size; +} diff --git a/src/system/arduino/esp32/network.cpp b/src/system/arduino/esp32/network.cpp index c0a82927d..9cb4c5bd1 100644 --- a/src/system/arduino/esp32/network.cpp +++ b/src/system/arduino/esp32/network.cpp @@ -24,6 +24,8 @@ extern "C" { #include "zenoh-pico/collections/slice.h" #include "zenoh-pico/collections/string.h" #include "zenoh-pico/config.h" +#include "zenoh-pico/protocol/codec/serial.h" +#include "zenoh-pico/system/common/serial.h" #include "zenoh-pico/system/link/bt.h" #include "zenoh-pico/system/link/serial.h" #include "zenoh-pico/system/platform.h" @@ -687,8 +689,6 @@ z_result_t _z_open_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, u } z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t baudrate) { - z_result_t ret = _Z_RES_OK; - uint8_t uart = 255; uint32_t rxpin = 0; uint32_t txpin = 0; @@ -705,28 +705,26 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_ rxpin = 16; txpin = 17; } else { - ret = _Z_ERR_GENERIC; + return _Z_ERR_GENERIC; } - if (ret == _Z_RES_OK) { - // WARNING: Glitch on the very 1st byte is common when the serial input line is allowed to float. - // To minimize this issue the RxPin is set to INPUT_PULLUP, and set the TxPin is driven to HIGH. This - // will keep the pins high and upon initialization of the serial port using serial.begin() the line - // does not float (drops to low). - pinMode(rxpin, INPUT_PULLUP); - pinMode(txpin, OUTPUT); - digitalWrite(txpin, HIGH); - - sock->_serial = new HardwareSerial(uart); - if (sock->_serial != NULL) { - sock->_serial->begin(baudrate); - sock->_serial->flush(); - } else { - ret = _Z_ERR_GENERIC; - } + // WARNING: Glitch on the very 1st byte is common when the serial input line is allowed to float. + // To minimize this issue the RxPin is set to INPUT_PULLUP, and set the TxPin is driven to HIGH. This + // will keep the pins high and upon initialization of the serial port using serial.begin() the line + // does not float (drops to low). + pinMode(rxpin, INPUT_PULLUP); + pinMode(txpin, OUTPUT); + digitalWrite(txpin, HIGH); + + sock->_serial = new HardwareSerial(uart); + if (sock->_serial != NULL) { + sock->_serial->begin(baudrate); + sock->_serial->flush(); + } else { + return _Z_ERR_GENERIC; } - return ret; + return _z_connect_serial(*sock); } z_result_t _z_listen_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, uint32_t rxpin, uint32_t baudrate) { @@ -759,110 +757,48 @@ void _z_close_serial(_z_sys_net_socket_t *sock) { delete sock->_serial; } -size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t *before_cobs = new uint8_t[_Z_SERIAL_MAX_COBS_BUF_SIZE](); +size_t _z_read_serial_internal(const _z_sys_net_socket_t sock, uint8_t *header, uint8_t *ptr, size_t len) { + uint8_t *raw_buf = z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); size_t rb = 0; for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) { while (sock._serial->available() < 1) { z_sleep_ms(1); // FIXME: Yield by sleeping. } - before_cobs[i] = sock._serial->read(); + raw_buf[i] = sock._serial->read(); rb = rb + (size_t)1; - if (before_cobs[i] == (uint8_t)0x00) { + if (raw_buf[i] == (uint8_t)0x00) { break; } } - uint8_t *after_cobs = new uint8_t[_Z_SERIAL_MFS_SIZE](); - size_t trb = _z_cobs_decode(before_cobs, rb, after_cobs); - - size_t i = 0; - uint16_t payload_len = 0; - for (i = 0; i < sizeof(payload_len); i++) { - payload_len |= (after_cobs[i] << ((uint8_t)i * (uint8_t)8)); - } + uint8_t *tmp_buf = z_malloc(_Z_SERIAL_MFS_SIZE); + size_t ret = _z_serial_msg_deserialize(raw_buf, rb, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - if (trb == (size_t)(payload_len + (uint16_t)6)) { - (void)memcpy(ptr, &after_cobs[i], payload_len); - i = i + (size_t)payload_len; + z_free(raw_buf); + z_free(tmp_buf); - uint32_t crc = 0; - for (uint8_t j = 0; j < sizeof(crc); j++) { - crc |= (uint32_t)(after_cobs[i] << (j * (uint8_t)8)); - i = i + (size_t)1; - } - - uint32_t c_crc = _z_crc32(ptr, payload_len); - if (c_crc != crc) { - ret = _Z_ERR_GENERIC; - } - } else { - ret = _Z_ERR_GENERIC; - } - - delete before_cobs; - delete after_cobs; - - rb = payload_len; - if (ret != _Z_RES_OK) { - rb = SIZE_MAX; - } - - return rb; -} - -size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - size_t n = len; - size_t rb = 0; - - do { - rb = _z_read_serial(sock, ptr, n); - if (rb == SIZE_MAX) return rb; - - n -= rb; - ptr = ptr + (len - n); - } while (n > 0); - - return len; + return ret; } -size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t *before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); - size_t i = 0; - for (i = 0; i < sizeof(uint16_t); ++i) { - before_cobs[i] = (len >> (i * (size_t)8)) & (size_t)0XFF; - } +size_t _z_send_serial_internal(const _z_sys_net_socket_t sock, uint8_t header, const uint8_t *ptr, size_t len) { + uint8_t *tmp_buf = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); + uint8_t *raw_buf = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); + size_t ret = + _z_serial_msg_serialize(raw_buf, _Z_SERIAL_MAX_COBS_BUF_SIZE, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - (void)memcpy(&before_cobs[i], ptr, len); - i = i + len; - - uint32_t crc = _z_crc32(ptr, len); - for (uint8_t j = 0; j < sizeof(crc); j++) { - before_cobs[i] = (crc >> (j * (uint8_t)8)) & (uint32_t)0XFF; - i = i + (size_t)1; + if (ret == SIZE_MAX) { + return ret; } - uint8_t *after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); - ssize_t twb = _z_cobs_encode(before_cobs, i, after_cobs); - after_cobs[twb] = 0x00; // Manually add the COBS delimiter - ssize_t wb = sock._serial->write(after_cobs, twb + (ssize_t)1); - if (wb != (twb + (ssize_t)1)) { - ret = _Z_ERR_GENERIC; + ssize_t wb = sock._serial->write(raw_buf, ret); + if (wb != (ssize_t)ret) { + ret = SIZE_MAX; } - z_free(before_cobs); - z_free(after_cobs); + z_free(raw_buf); + z_free(tmp_buf); - size_t sb = len; - if (ret != _Z_RES_OK) { - sb = SIZE_MAX; - } - - return sb; + return len; } #endif diff --git a/src/system/arduino/esp32/system.c b/src/system/arduino/esp32/system.c index 2a6a257e6..77f6b3459 100644 --- a/src/system/arduino/esp32/system.c +++ b/src/system/arduino/esp32/system.c @@ -20,7 +20,6 @@ #include "zenoh-pico/config.h" #include "zenoh-pico/system/platform.h" -#include "zenoh-pico/system/platform_common.h" #include "zenoh-pico/utils/result.h" /*------------------ Random ------------------*/ diff --git a/src/system/platform_common.c b/src/system/common/platform.c similarity index 98% rename from src/system/platform_common.c rename to src/system/common/platform.c index 2317d0cc6..7697b9ed1 100644 --- a/src/system/platform_common.c +++ b/src/system/common/platform.c @@ -11,7 +11,7 @@ // Contributors: // ZettaScale Zenoh Team, -#include "zenoh-pico/system/platform_common.h" +#include "zenoh-pico/system/common/platform.h" #include "zenoh-pico/api/olv_macros.h" #include "zenoh-pico/utils/logging.h" diff --git a/src/system/common/serial.c b/src/system/common/serial.c new file mode 100644 index 000000000..2f21c3027 --- /dev/null +++ b/src/system/common/serial.c @@ -0,0 +1,83 @@ +// +// Copyright (c) 2024 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/protocol/definitions/serial.h" + +#include + +#include "zenoh-pico/system/common/platform.h" +#include "zenoh-pico/system/common/serial.h" +#include "zenoh-pico/system/link/serial.h" +#include "zenoh-pico/utils/logging.h" +#include "zenoh-pico/utils/pointers.h" +#include "zenoh-pico/utils/result.h" + +#if Z_FEATURE_LINK_SERIAL == 1 + +#define SERIAL_CONNECT_THROTTLE_TIME_MS 250 + +z_result_t _z_connect_serial(const _z_sys_net_socket_t sock) { + while (true) { + uint8_t header = _Z_FLAG_SERIAL_INIT; + + _z_send_serial_internal(sock, header, NULL, 0); + uint8_t tmp; + size_t ret = _z_read_serial_internal(sock, &header, &tmp, sizeof(tmp)); + if (ret == SIZE_MAX) { + return _Z_ERR_TRANSPORT_RX_FAILED; + } + + if (_Z_HAS_FLAG(header, _Z_FLAG_SERIAL_ACK) && _Z_HAS_FLAG(header, _Z_FLAG_SERIAL_INIT)) { + _Z_DEBUG("connected"); + break; + } else if (_Z_HAS_FLAG(header, _Z_FLAG_SERIAL_RESET)) { + z_sleep_ms(SERIAL_CONNECT_THROTTLE_TIME_MS); + _Z_DEBUG("reset"); + continue; + } else { + _Z_ERROR("unknown header received: %02X", header); + return _Z_ERR_TRANSPORT_RX_FAILED; + } + } + + return _Z_RES_OK; +} + +size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { + uint8_t header; + return _z_read_serial_internal(sock, &header, ptr, len); +} + +size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len) { + return _z_send_serial_internal(sock, 0, ptr, len); +} + +size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { + size_t n = 0; + uint8_t *pos = &ptr[0]; + + do { + size_t rb = _z_read_serial(sock, ptr, len - n); + if (rb == SIZE_MAX) { + n = rb; + break; + } + + n += rb; + pos = _z_ptr_u8_offset(pos, n); + } while (n != len); + + return n; +} +#endif // Z_FEATURE_LINK_SERIAL == 1 diff --git a/src/system/espidf/network.c b/src/system/espidf/network.c index b76e7f04a..7aa41e49a 100644 --- a/src/system/espidf/network.c +++ b/src/system/espidf/network.c @@ -20,6 +20,8 @@ #include "zenoh-pico/collections/slice.h" #include "zenoh-pico/collections/string.h" #include "zenoh-pico/config.h" +#include "zenoh-pico/protocol/codec/serial.h" +#include "zenoh-pico/system/common/serial.h" #include "zenoh-pico/system/link/bt.h" #include "zenoh-pico/system/link/serial.h" #include "zenoh-pico/system/platform.h" @@ -564,8 +566,6 @@ z_result_t _z_open_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, u } z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t baudrate) { - z_result_t ret = _Z_RES_OK; - uint32_t rxpin = 0; uint32_t txpin = 0; if (strcmp(dev, "UART_0") == 0) { @@ -581,7 +581,7 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_ rxpin = 16; txpin = 17; } else { - ret = _Z_ERR_GENERIC; + return _Z_ERR_GENERIC; } const uart_config_t config = { @@ -592,7 +592,7 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_ .flow_ctrl = UART_MODE_UART, // Default in Zenoh Rust }; if (uart_param_config(sock->_serial, &config) != 0) { - ret = _Z_ERR_GENERIC; + return _Z_ERR_GENERIC; } uart_set_pin(sock->_serial, txpin, rxpin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); @@ -601,9 +601,8 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_ QueueHandle_t uart_queue; uart_driver_install(sock->_serial, uart_buffer_size, 0, 100, &uart_queue, 0); uart_flush_input(sock->_serial); - sock->after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); - sock->before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); - return ret; + + return _z_connect_serial(*sock); } z_result_t _z_listen_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, uint32_t rxpin, uint32_t baudrate) { @@ -635,123 +634,57 @@ void _z_close_serial(_z_sys_net_socket_t *sock) { uart_wait_tx_done(sock->_serial, 1000); uart_flush(sock->_serial); uart_driver_delete(sock->_serial); - z_free(sock->after_cobs); - z_free(sock->before_cobs); } -size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - +size_t _z_read_serial_internal(const _z_sys_net_socket_t sock, uint8_t *header, uint8_t *ptr, size_t len) { + uint8_t *raw_buf = z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); size_t rb = 0; while (rb < _Z_SERIAL_MAX_COBS_BUF_SIZE) { - int r = uart_read_bytes(sock._serial, &sock.before_cobs[rb], 1, 1000); + int r = uart_read_bytes(sock._serial, &raw_buf[rb], 1, 1000); if (r == 0) { _Z_DEBUG("Timeout reading from serial"); if (rb == 0) { - ret = _Z_ERR_GENERIC; + return SIZE_MAX; } - break; } else if (r == 1) { rb = rb + (size_t)1; - if (sock.before_cobs[rb - 1] == (uint8_t)0x00) { + if (raw_buf[rb - 1] == (uint8_t)0x00) { break; } } else { _Z_ERROR("Error reading from serial"); - ret = _Z_ERR_GENERIC; - } - } - uint16_t payload_len = 0; - - if (ret == _Z_RES_OK) { - _Z_DEBUG("Read %u bytes from serial", rb); - size_t trb = _z_cobs_decode(sock.before_cobs, rb, sock.after_cobs); - _Z_DEBUG("Decoded %u bytes from serial", trb); - size_t i = 0; - for (i = 0; i < sizeof(payload_len); i++) { - payload_len |= (sock.after_cobs[i] << ((uint8_t)i * (uint8_t)8)); - } - _Z_DEBUG("payload_len = %u <= %X %X", payload_len, sock.after_cobs[1], sock.after_cobs[0]); - - if (trb == (size_t)(payload_len + (uint16_t)6)) { - (void)memcpy(ptr, &sock.after_cobs[i], payload_len); - i = i + (size_t)payload_len; - - uint32_t crc = 0; - for (uint8_t j = 0; j < sizeof(crc); j++) { - crc |= (uint32_t)(sock.after_cobs[i] << (j * (uint8_t)8)); - i = i + (size_t)1; - } - - uint32_t c_crc = _z_crc32(ptr, payload_len); - if (c_crc != crc) { - _Z_ERROR("CRC mismatch: %ld != %ld ", c_crc, crc); - ret = _Z_ERR_GENERIC; - } - } else { - _Z_ERROR("length mismatch => %d <> %d ", trb, payload_len + (uint16_t)6); - ret = _Z_ERR_GENERIC; + return SIZE_MAX; } } - rb = payload_len; - if (ret != _Z_RES_OK) { - rb = SIZE_MAX; - } - _Z_DEBUG("return _z_read_serial() = %d ", rb); - return rb; -} - -size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - size_t n = len; - size_t rb = 0; - - do { - rb = _z_read_serial(sock, ptr, n); - if (rb == SIZE_MAX) return rb; + uint8_t *tmp_buf = z_malloc(_Z_SERIAL_MFS_SIZE); + size_t ret = _z_serial_msg_deserialize(raw_buf, rb, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - n -= rb; - ptr = ptr + (len - n); - } while (n > 0); + z_free(raw_buf); + z_free(tmp_buf); - return len; + return ret; } -size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t *before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); - size_t i = 0; - for (i = 0; i < sizeof(uint16_t); ++i) { - before_cobs[i] = (len >> (i * (size_t)8)) & (size_t)0XFF; - } - - (void)memcpy(&before_cobs[i], ptr, len); - i = i + len; +size_t _z_send_serial_internal(const _z_sys_net_socket_t sock, uint8_t header, const uint8_t *ptr, size_t len) { + uint8_t *tmp_buf = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); + uint8_t *raw_buf = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); + size_t ret = + _z_serial_msg_serialize(raw_buf, _Z_SERIAL_MAX_COBS_BUF_SIZE, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - uint32_t crc = _z_crc32(ptr, len); - for (uint8_t j = 0; j < sizeof(crc); j++) { - before_cobs[i] = (crc >> (j * (uint8_t)8)) & (uint32_t)0XFF; - i = i + (size_t)1; + if (ret == SIZE_MAX) { + return ret; } - uint8_t *after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); - ssize_t twb = _z_cobs_encode(before_cobs, i, after_cobs); - after_cobs[twb] = 0x00; // Manually add the COBS delimiter - ssize_t wb = uart_write_bytes(sock._serial, after_cobs, twb + (ssize_t)1); - if (wb != (twb + (ssize_t)1)) { - ret = _Z_ERR_GENERIC; + ssize_t wb = uart_write_bytes(sock._serial, raw_buf, ret); + if (wb != (ssize_t)ret) { + ret = SIZE_MAX; } - z_free(before_cobs); - z_free(after_cobs); + z_free(raw_buf); + z_free(tmp_buf); - size_t sb = len; - if (ret != _Z_RES_OK) { - sb = SIZE_MAX; - } - - return sb; + return len; } #endif diff --git a/src/system/flipper/network.c b/src/system/flipper/network.c index f03b3c647..cebe32851 100644 --- a/src/system/flipper/network.c +++ b/src/system/flipper/network.c @@ -77,7 +77,8 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t* sock, char* dev, uint32_ furi_hal_serial_async_rx_start(sock->_serial, _z_serial_received_byte_callback, sock->_rx_stream, false); _Z_DEBUG("Serial port opened: %s (%li)", dev, baudrate); - return _Z_RES_OK; + + return _z_connect_serial(*sock); } z_result_t _z_listen_serial_from_pins(_z_sys_net_socket_t* sock, uint32_t txpin, uint32_t rxpin, uint32_t baudrate) { @@ -116,105 +117,46 @@ void _z_close_serial(_z_sys_net_socket_t* sock) { _Z_DEBUG("Serial port closed"); } -size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t* ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t* before_cobs = (uint8_t*)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); +size_t _z_read_serial_internal(const _z_sys_net_socket_t sock, uint8_t* header, uint8_t* ptr, size_t len) { + uint8_t* raw_buf = z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); size_t rb = 0; for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) { size_t len = 0; - len = furi_stream_buffer_receive(sock._rx_stream, &before_cobs[i], 1, FLIPPER_SERIAL_TIMEOUT_MS); + len = furi_stream_buffer_receive(sock._rx_stream, &raw_buf[i], 1, FLIPPER_SERIAL_TIMEOUT_MS); if (!len) { - z_free(before_cobs); return SIZE_MAX; } - rb += 1; + rb++; - if (before_cobs[i] == (uint8_t)0x00) { + if (raw_buf[i] == (uint8_t)0x00) { break; } } - uint8_t* after_cobs = (uint8_t*)z_malloc(_Z_SERIAL_MFS_SIZE); - size_t trb = _z_cobs_decode(before_cobs, rb, after_cobs); - - size_t i = 0; - uint16_t payload_len = 0; - for (i = 0; i < sizeof(payload_len); i++) { - payload_len |= (after_cobs[i] << ((uint8_t)i * (uint8_t)8)); - } - - if (trb == (size_t)(payload_len + (uint16_t)6)) { - (void)memcpy(ptr, &after_cobs[i], payload_len); - i += (size_t)payload_len; - - uint32_t crc = 0; - for (uint8_t j = 0; j < sizeof(crc); j++) { - crc |= (uint32_t)(after_cobs[i] << (j * (uint8_t)8)); - i += 1; - } - - uint32_t c_crc = _z_crc32(ptr, payload_len); - if (c_crc != crc) { - ret = _Z_ERR_GENERIC; - } - } else { - ret = _Z_ERR_GENERIC; - } + uint8_t* tmp_buf = z_malloc(_Z_SERIAL_MFS_SIZE); + size_t ret = _z_serial_msg_deserialize(raw_buf, rb, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - z_free(before_cobs); - z_free(after_cobs); + z_free(raw_buf); + z_free(tmp_buf); - rb = payload_len; - if (ret != _Z_RES_OK) { - rb = SIZE_MAX; - } - - return rb; + return ret; } -size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t* ptr, size_t len) { - size_t n = len; - - do { - size_t rb = _z_read_serial(sock, ptr, n); - if (rb == SIZE_MAX) { - return rb; - } - n -= rb; - ptr += len - n; - } while (n > 0); - - return len; -} +size_t _z_send_serial_internal(const _z_sys_net_socket_t sock, uint8_t header, const uint8_t* ptr, size_t len) { + uint8_t* tmp_buf = (uint8_t*)z_malloc(_Z_SERIAL_MFS_SIZE); + uint8_t* raw_buf = (uint8_t*)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); + size_t ret = + _z_serial_msg_serialize(raw_buf, _Z_SERIAL_MAX_COBS_BUF_SIZE, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); -size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t* ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t* before_cobs = (uint8_t*)z_malloc(_Z_SERIAL_MFS_SIZE); - size_t i = 0; - for (i = 0; i < sizeof(uint16_t); ++i) { - before_cobs[i] = (len >> (i * (size_t)8)) & (size_t)0XFF; + if (ret == SIZE_MAX) { + return ret; } - (void)memcpy(&before_cobs[i], ptr, len); - i += len; - - uint32_t crc = _z_crc32(ptr, len); - for (uint8_t j = 0; j < sizeof(crc); j++) { - before_cobs[i] = (crc >> (j * (uint8_t)8)) & (uint32_t)0XFF; - i++; - } - - uint8_t* after_cobs = (uint8_t*)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); - ssize_t twb = _z_cobs_encode(before_cobs, i, after_cobs); - after_cobs[twb] = 0x00; // Manually add the COBS delimiter - - furi_hal_serial_tx(sock._serial, after_cobs, twb + (ssize_t)1); + furi_hal_serial_tx(sock._serial, raw_buf, ret); furi_hal_serial_tx_wait_complete(sock._serial); - z_free(before_cobs); - z_free(after_cobs); + z_free(raw_buf); + z_free(tmp_buf); return len; } diff --git a/src/system/mbed/network.cpp b/src/system/mbed/network.cpp index a9d365782..39de6204f 100644 --- a/src/system/mbed/network.cpp +++ b/src/system/mbed/network.cpp @@ -332,7 +332,7 @@ z_result_t _z_open_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, u ret = _Z_ERR_GENERIC; } - return ret; + return _z_connect_serial(*sock); } z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *device, uint32_t baudrate) { @@ -373,110 +373,44 @@ z_result_t _z_listen_serial_from_dev(_z_sys_net_socket_t *sock, char *device, ui void _z_close_serial(_z_sys_net_socket_t *sock) { delete sock->_serial; } size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t *before_cobs = new uint8_t[_Z_SERIAL_MAX_COBS_BUF_SIZE](); + uint8_t *raw_buf = z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); size_t rb = 0; for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) { - sock._serial->read(&before_cobs[i], 1); - rb = rb + (size_t)1; + sock._serial->read(&raw_buf[i], 1); + rb++; if (before_cobs[i] == (uint8_t)0x00) { break; } } - uint8_t *after_cobs = new uint8_t[_Z_SERIAL_MFS_SIZE](); - size_t trb = _z_cobs_decode(before_cobs, rb, after_cobs); - - size_t i = 0; - uint16_t payload_len = 0; - for (i = 0; i < sizeof(payload_len); i++) { - payload_len |= (after_cobs[i] << ((uint8_t)i * (uint8_t)8)); - } - - if (trb == (size_t)(payload_len + (uint16_t)6)) { - (void)memcpy(ptr, &after_cobs[i], payload_len); - i = i + (size_t)payload_len; + uint8_t *tmp_buf = z_malloc(_Z_SERIAL_MFS_SIZE); + size_t ret = _z_serial_msg_deserialize(raw_buf, rb, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - uint32_t crc = 0; - for (uint8_t j = 0; j < sizeof(crc); j++) { - crc |= (uint32_t)(after_cobs[i] << (j * (uint8_t)8)); - i = i + (size_t)1; - } - - uint32_t c_crc = _z_crc32(ptr, payload_len); - if (c_crc != crc) { - ret = _Z_ERR_GENERIC; - } - } else { - ret = _Z_ERR_GENERIC; - } - - delete before_cobs; - delete after_cobs; - - rb = payload_len; - if (ret != _Z_RES_OK) { - rb = SIZE_MAX; - } - - return rb; -} - -size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - size_t n = 0; - uint8_t *pos = &ptr[0]; + z_free(raw_buf); + z_free(tmp_buf); - do { - size_t rb = _z_read_serial(sock, ptr, len - n); - if (rb == SIZE_MAX) { - n = rb; - break; - } - - n = n + rb; - pos = _z_ptr_u8_offset(pos, n); - } while (n != len); - - return n; + return ret; } -size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; +size_t _z_send_serial_internal(const _z_sys_net_socket_t sock, uint8_t header, const uint8_t *ptr, size_t len) { + uint8_t *tmp_buf = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); + uint8_t *raw_buf = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); + size_t ret = + _z_serial_msg_serialize(raw_buf, _Z_SERIAL_MAX_COBS_BUF_SIZE, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - uint8_t *before_cobs = new uint8_t[_Z_SERIAL_MFS_SIZE](); - size_t i = 0; - for (; i < sizeof(uint16_t); ++i) { - before_cobs[i] = (len >> (i * (size_t)8)) & (size_t)0XFF; + if (ret == SIZE_MAX) { + return ret; } - (void)memcpy(&before_cobs[i], ptr, len); - i = i + len; - - uint32_t crc = _z_crc32(ptr, len); - for (uint8_t j = 0; j < sizeof(crc); j++) { - before_cobs[i] = (crc >> (j * (uint8_t)8)) & (uint32_t)0XFF; - i = i + (size_t)1; + ssize_t wb = sock._serial->write(after_cobs, raw_buf, ret); + if (wb != (ssize_t)ret) { + ret = SIZE_MAX; } - uint8_t *after_cobs = new uint8_t[_Z_SERIAL_MAX_COBS_BUF_SIZE](); - ssize_t twb = _z_cobs_encode(before_cobs, i, after_cobs); - after_cobs[twb] = 0x00; // Manually add the COBS delimiter - - ssize_t wb = sock._serial->write(after_cobs, twb + (ssize_t)1); - if (wb != (twb + (ssize_t)1)) { - ret = _Z_ERR_GENERIC; - } + z_free(raw_buf); + z_free(tmp_buf); - delete before_cobs; - delete after_cobs; - - size_t sb = len; - if (ret != _Z_RES_OK) { - sb = SIZE_MAX; - } - - return sb; + return len; } #endif diff --git a/src/system/rpi_pico/network.c b/src/system/rpi_pico/network.c index 135d05c84..322e82230 100644 --- a/src/system/rpi_pico/network.c +++ b/src/system/rpi_pico/network.c @@ -15,6 +15,8 @@ #include #include "zenoh-pico/collections/string.h" +#include "zenoh-pico/protocol/codec/serial.h" +#include "zenoh-pico/system/common/serial.h" #include "zenoh-pico/system/link/serial.h" #include "zenoh-pico/system/platform.h" #include "zenoh-pico/utils/checksum.h" @@ -528,7 +530,7 @@ z_result_t _z_open_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, u _z_open_serial_impl(sock->_serial, txpin, rxpin, baudrate); - return ret; + return _z_connect_serial(*sock); } z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t baudrate) { @@ -539,7 +541,7 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_ #if Z_FEATURE_LINK_SERIAL_USB == 1 if (strcmp("usb", dev) == 0) { _z_usb_uart_init(); - return ret; + return _z_connect_serial(*sock); } #endif @@ -562,7 +564,7 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_ _z_open_serial_impl(sock->_serial, txpin, rxpin, baudrate); - return ret; + return _z_connect_serial(*sock); } z_result_t _z_listen_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, uint32_t rxpin, uint32_t baudrate) { @@ -600,118 +602,51 @@ void _z_close_serial(_z_sys_net_socket_t *sock) { } } -size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t *before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); +size_t _z_read_serial_internal(const _z_sys_net_socket_t sock, uint8_t *header, uint8_t *ptr, size_t len) { + uint8_t *raw_buf = z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); size_t rb = 0; for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) { #if Z_FEATURE_LINK_SERIAL_USB == 1 - before_cobs[i] = (sock._serial == NULL) ? _z_usb_uart_getc() : uart_getc(sock._serial); + raw_buf[i] = (sock._serial == NULL) ? _z_usb_uart_getc() : uart_getc(sock._serial); #else - before_cobs[i] = uart_getc(sock._serial); + raw_buf[i] = uart_getc(sock._serial); #endif - - rb = rb + (size_t)1; - if (before_cobs[i] == (uint8_t)0x00) { + rb++; + if (raw_buf[i] == 0x00) { break; } } - uint8_t *after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); - size_t trb = _z_cobs_decode(before_cobs, rb, after_cobs); - - size_t i = 0; - uint16_t payload_len = 0; - for (; i < sizeof(payload_len); i++) { - payload_len |= (after_cobs[i] << ((uint8_t)i * (uint8_t)8)); - } - - if (trb == (size_t)(payload_len + (uint16_t)6)) { - (void)memcpy(ptr, &after_cobs[i], payload_len); - i = i + (size_t)payload_len; - - uint32_t crc = 0; - for (uint8_t j = 0; j < sizeof(crc); j++) { - crc |= (uint32_t)(after_cobs[i] << (j * (uint8_t)8)); - i = i + (size_t)1; - } - - uint32_t c_crc = _z_crc32(ptr, payload_len); - if (c_crc != crc) { - ret = _Z_ERR_GENERIC; - } - } else { - ret = _Z_ERR_GENERIC; - } - - z_free(before_cobs); - z_free(after_cobs); - - rb = payload_len; - if (ret != _Z_RES_OK) { - rb = SIZE_MAX; - } - - return rb; -} - -size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - size_t n = 0; - uint8_t *pos = &ptr[0]; + uint8_t *tmp_buf = z_malloc(_Z_SERIAL_MFS_SIZE); + size_t ret = _z_serial_msg_deserialize(raw_buf, rb, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - do { - size_t rb = _z_read_serial(sock, ptr, len - n); - if (rb == SIZE_MAX) { - n = rb; - break; - } - - n = n + rb; - pos = _z_ptr_u8_offset(pos, n); - } while (n != len); + z_free(raw_buf); + z_free(tmp_buf); - return n; + return ret; } -size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t *before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); - size_t i = 0; - for (; i < sizeof(uint16_t); ++i) { - before_cobs[i] = (len >> (i * (size_t)8)) & (size_t)0XFF; - } +size_t _z_send_serial_internal(const _z_sys_net_socket_t sock, uint8_t header, const uint8_t *ptr, size_t len) { + uint8_t *tmp_buf = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); + uint8_t *raw_buf = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); + size_t ret = + _z_serial_msg_serialize(raw_buf, _Z_SERIAL_MAX_COBS_BUF_SIZE, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - (void)memcpy(&before_cobs[i], ptr, len); - i = i + len; - - uint32_t crc = _z_crc32(ptr, len); - for (uint8_t j = 0; j < sizeof(crc); j++) { - before_cobs[i] = (crc >> (j * (uint8_t)8)) & (uint32_t)0XFF; - i = i + (size_t)1; + if (ret == SIZE_MAX) { + return ret; } - - uint8_t *after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); - ssize_t twb = _z_cobs_encode(before_cobs, i, after_cobs); - after_cobs[twb] = 0x00; // Manually add the COBS delimiter if (sock._serial == NULL) { #if Z_FEATURE_LINK_SERIAL_USB == 1 - _z_usb_uart_write(after_cobs, twb + (ssize_t)1); + _z_usb_uart_write(raw_buf, ret); #endif } else { - uart_write_blocking(sock._serial, after_cobs, twb + (ssize_t)1); + uart_write_blocking(sock._serial, raw_buf, ret); } - z_free(before_cobs); - z_free(after_cobs); - - size_t sb = len; - if (ret != _Z_RES_OK) { - sb = SIZE_MAX; - } + z_free(raw_buf); + z_free(tmp_buf); - return sb; + return len; } #endif diff --git a/src/system/unix/system.c b/src/system/unix/system.c index d1c0c5886..bd28018b7 100644 --- a/src/system/unix/system.c +++ b/src/system/unix/system.c @@ -17,7 +17,6 @@ #include #include -#include "zenoh-pico/system/platform_common.h" #include "zenoh-pico/utils/result.h" #if defined(ZENOH_LINUX) diff --git a/src/system/zephyr/network.c b/src/system/zephyr/network.c index b3a0505a4..cce1a59cc 100644 --- a/src/system/zephyr/network.c +++ b/src/system/zephyr/network.c @@ -598,7 +598,7 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_ ret = _Z_ERR_GENERIC; } - return ret; + return _z_connect_serial(*sock); } z_result_t _z_listen_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, uint32_t rxpin, uint32_t baudrate) { @@ -628,113 +628,48 @@ z_result_t _z_listen_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint3 void _z_close_serial(_z_sys_net_socket_t *sock) {} -size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t *before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); +size_t _z_read_serial_internal(const _z_sys_net_socket_t sock, uint8_t *header, uint8_t *ptr, size_t len) { + uint8_t *raw_buf = z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); size_t rb = 0; for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) { int res = -1; while (res != 0) { - res = uart_poll_in(sock._serial, &before_cobs[i]); + res = uart_poll_in(sock._serial, &raw_buf[i]); } - rb = rb + (size_t)1; - if (before_cobs[i] == (uint8_t)0x00) { + rb++; + if (raw_buf[i] == (uint8_t)0x00) { break; } } - uint8_t *after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); - size_t trb = _z_cobs_decode(before_cobs, rb, after_cobs); + uint8_t *tmp_buf = z_malloc(_Z_SERIAL_MFS_SIZE); + size_t ret = _z_serial_msg_deserialize(raw_buf, rb, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - size_t i = 0; - uint16_t payload_len = 0; - for (; i < sizeof(payload_len); i++) { - payload_len |= (after_cobs[i] << ((uint8_t)i * (uint8_t)8)); - } - - if (trb == (size_t)(payload_len + (uint16_t)6)) { - (void)memcpy(ptr, &after_cobs[i], payload_len); - i = i + (size_t)payload_len; + z_free(raw_buf); + z_free(tmp_buf); - uint32_t crc = 0; - for (uint8_t j = 0; j < sizeof(crc); j++) { - crc |= (uint32_t)(after_cobs[i] << (j * (uint8_t)8)); - i = i + (size_t)1; - } - - uint32_t c_crc = _z_crc32(ptr, payload_len); - if (c_crc != crc) { - ret = _Z_ERR_GENERIC; - } - } else { - ret = _Z_ERR_GENERIC; - } - - z_free(before_cobs); - z_free(after_cobs); - - rb = payload_len; - if (ret != _Z_RES_OK) { - rb = SIZE_MAX; - } - - return rb; -} - -size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { - size_t n = 0; - uint8_t *pos = &ptr[0]; - - do { - size_t rb = _z_read_serial(sock, ptr, len - n); - if (rb == SIZE_MAX) { - n = rb; - break; - } - - n = n + rb; - pos = _z_ptr_u8_offset(pos, n); - } while (n != len); - - return n; + return ret; } -size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len) { - z_result_t ret = _Z_RES_OK; - - uint8_t *before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); - size_t i = 0; - for (; i < sizeof(uint16_t); ++i) { - before_cobs[i] = (len >> (i * (size_t)8)) & (size_t)0XFF; - } - - (void)memcpy(&before_cobs[i], ptr, len); - i = i + len; +size_t _z_send_serial_internal(const _z_sys_net_socket_t sock, uint8_t header, const uint8_t *ptr, size_t len) { + uint8_t *tmp_buf = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE); + uint8_t *raw_buf = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); + size_t ret = + _z_serial_msg_serialize(raw_buf, _Z_SERIAL_MAX_COBS_BUF_SIZE, ptr, len, header, tmp_buf, _Z_SERIAL_MFS_SIZE); - uint32_t crc = _z_crc32(ptr, len); - for (uint8_t j = 0; j < sizeof(crc); j++) { - before_cobs[i] = (crc >> (j * (uint8_t)8)) & (uint32_t)0XFF; - i = i + (size_t)1; + if (ret == SIZE_MAX) { + return ret; } - uint8_t *after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); - ssize_t twb = _z_cobs_encode(before_cobs, i, after_cobs); - after_cobs[twb] = 0x00; // Manually add the COBS delimiter - for (ssize_t i = 0; i < (twb + (ssize_t)1); i++) { - uart_poll_out(sock._serial, after_cobs[i]); + for (size_t i = 0; i < ret; i++) { + uart_poll_out(sock._serial, raw_buf[i]); } - z_free(before_cobs); - z_free(after_cobs); - - size_t sb = len; - if (ret != _Z_RES_OK) { - sb = SIZE_MAX; - } + z_free(raw_buf); + z_free(tmp_buf); - return sb; + return len; } #endif diff --git a/tests/z_msgcodec_test.c b/tests/z_msgcodec_test.c index cb7b9a927..a87c258e4 100644 --- a/tests/z_msgcodec_test.c +++ b/tests/z_msgcodec_test.c @@ -15,6 +15,7 @@ #include "zenoh-pico/api/constants.h" #include "zenoh-pico/protocol/codec/message.h" #include "zenoh-pico/protocol/codec/network.h" +#include "zenoh-pico/protocol/codec/serial.h" #include "zenoh-pico/protocol/definitions/message.h" #include "zenoh-pico/protocol/definitions/transport.h" #define ZENOH_PICO_TEST_H @@ -1976,6 +1977,69 @@ void scouting_message(void) { _z_wbuf_clear(&wbf); } +void test_serialize_deserialize(void) { + uint8_t src_data[] = "Test\0Payload\0Data"; + size_t src_len = sizeof(src_data) - 1; + uint8_t header = 0xAB; + uint8_t serialized[256] = {0}; + uint8_t deserialized[256] = {0}; + uint8_t tmp_buf[256] = {0}; + + size_t serialized_len = + _z_serial_msg_serialize(serialized, sizeof(serialized), src_data, src_len, header, tmp_buf, sizeof(tmp_buf)); + assert(serialized_len > 0); + + uint8_t decoded_header = 0; + size_t deserialized_len = _z_serial_msg_deserialize(serialized, serialized_len, deserialized, sizeof(deserialized), + &decoded_header, tmp_buf, sizeof(tmp_buf)); + assert(deserialized_len != SIZE_MAX); + + assert(decoded_header == header); + + assert(deserialized_len == src_len); + assert(memcmp(src_data, deserialized, src_len) == 0); +} + +void test_crc_mismatch(void) { + uint8_t src_data[] = "Test\0Payload\0Data"; + size_t src_len = sizeof(src_data) - 1; + uint8_t header = 0xCD; + uint8_t serialized[256] = {0}; + uint8_t tmp_buf[256] = {0}; + + size_t serialized_len = + _z_serial_msg_serialize(serialized, sizeof(serialized), src_data, src_len, header, tmp_buf, sizeof(tmp_buf)); + assert(serialized_len != SIZE_MAX); + + serialized[serialized_len - 2] ^= 0xFF; + + uint8_t decoded_header = 0; + uint8_t deserialized[256] = {0}; + size_t deserialized_len = _z_serial_msg_deserialize(serialized, serialized_len, deserialized, sizeof(deserialized), + &decoded_header, tmp_buf, sizeof(tmp_buf)); + + assert(deserialized_len == SIZE_MAX); +} + +void test_buffer_too_small(void) { + uint8_t src_data[] = "Test\0Payload\0Data"; + size_t src_len = sizeof(src_data) - 1; + uint8_t header = 0xEF; + uint8_t serialized[256] = {0}; + uint8_t tmp_buf[256] = {0}; + + size_t serialized_len = + _z_serial_msg_serialize(serialized, sizeof(serialized), src_data, src_len, header, tmp_buf, sizeof(tmp_buf)); + assert(serialized_len != SIZE_MAX); + + uint8_t decoded_header = 0; + uint8_t deserialized[4] = {0}; // Too small + size_t deserialized_len = _z_serial_msg_deserialize(serialized, serialized_len, deserialized, sizeof(deserialized), + &decoded_header, tmp_buf, sizeof(tmp_buf)); + + assert(deserialized_len == SIZE_MAX); +} + /*=============================*/ /* Main */ /*=============================*/ @@ -2029,6 +2093,11 @@ int main(void) { scouting_message(); } + // Serial serialization + test_serialize_deserialize(); + test_crc_mismatch(); + test_buffer_too_small(); + return 0; } diff --git a/tests/z_refcount_test.c b/tests/z_refcount_test.c index ded735f02..248a7b357 100644 --- a/tests/z_refcount_test.c +++ b/tests/z_refcount_test.c @@ -18,7 +18,7 @@ #include #include "zenoh-pico/collections/refcount.h" -#include "zenoh-pico/system/platform_common.h" +#include "zenoh-pico/system/common/platform.h" #undef NDEBUG #include diff --git a/zephyr/CMakeLists.txt b/zephyr/CMakeLists.txt index f358d4659..5296ba8ff 100644 --- a/zephyr/CMakeLists.txt +++ b/zephyr/CMakeLists.txt @@ -37,7 +37,7 @@ file(GLOB_RECURSE Sources "../src/session/*.c" "../src/transport/*.c" "../src/utils/*.c" - "../src/system/platform_common.c" + "../src/system/common/*.c" ) file (GLOB Sources_Zephyr "../src/system/zephyr/*.c") From f37e80693d952add7596ea05626618a0dae7928a Mon Sep 17 00:00:00 2001 From: Michael Ilyin Date: Wed, 11 Dec 2024 13:19:01 +0100 Subject: [PATCH 4/4] stabilize liveliness API (#825) * stabilize liveliness API * refeence liveliznes_stabilize cpp branch * restored main in CI cpp check --- CMakeLists.txt | 8 +------- zenohpico.pc | 2 +- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4c32bce5d..25b62fb53 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -221,7 +221,7 @@ set(Z_FEATURE_PUBLICATION 1 CACHE STRING "Toggle publication feature") set(Z_FEATURE_SUBSCRIPTION 1 CACHE STRING "Toggle subscription feature") set(Z_FEATURE_QUERY 1 CACHE STRING "Toggle query feature") set(Z_FEATURE_QUERYABLE 1 CACHE STRING "Toggle queryable feature") -set(Z_FEATURE_LIVELINESS 0 CACHE STRING "Toggle liveliness feature") +set(Z_FEATURE_LIVELINESS 1 CACHE STRING "Toggle liveliness feature") set(Z_FEATURE_INTEREST 1 CACHE STRING "Toggle interests") set(Z_FEATURE_FRAGMENTATION 1 CACHE STRING "Toggle fragmentation") set(Z_FEATURE_ENCODING_VALUES 1 CACHE STRING "Toggle encoding values") @@ -245,12 +245,6 @@ set(Z_FEATURE_PUBLISHER_SESSION_CHECK 1 CACHE STRING "Toggle publisher session c set(Z_FEATURE_BATCHING 1 CACHE STRING "Toggle batching") set(Z_FEATURE_RX_CACHE 0 CACHE STRING "Toggle RX_CACHE") -# Add a warning message if someone tries to enable Z_FEATURE_LIVELINESS directly -if(Z_FEATURE_LIVELINESS AND NOT Z_FEATURE_UNSTABLE_API) - message(WARNING "Z_FEATURE_LIVELINESS can only be enabled when Z_FEATURE_UNSTABLE_API is also enabled. Disabling Z_FEATURE_LIVELINESS.") - set(Z_FEATURE_LIVELINESS 0 CACHE STRING "Toggle liveliness feature" FORCE) -endif() - # Add a warning message if someone tries to enable Z_FEATURE_LINK_SERIAL_USB directly if(Z_FEATURE_LINK_SERIAL_USB AND NOT Z_FEATURE_UNSTABLE_API) message(WARNING "Z_FEATURE_LINK_SERIAL_USB can only be enabled when Z_FEATURE_UNSTABLE_API is also enabled. Disabling Z_FEATURE_LINK_SERIAL_USB.") diff --git a/zenohpico.pc b/zenohpico.pc index 1e3b39ae2..421fe0950 100644 --- a/zenohpico.pc +++ b/zenohpico.pc @@ -3,6 +3,6 @@ prefix=/usr/local Name: zenohpico Description: URL: -Version: 1.0.20241018dev +Version: 1.0.20241210dev Cflags: -I${prefix}/include Libs: -L${prefix}/lib -lzenohpico