From 45b7b0af74de97a77591e884df64f432ba38fd05 Mon Sep 17 00:00:00 2001 From: Alexander Bushnev Date: Mon, 9 Dec 2024 12:34:02 +0100 Subject: [PATCH] Update serial protocol --- CMakeLists.txt | 2 +- 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/utils/logging.h | 2 +- src/api/api.c | 2 +- src/link/unicast/serial.c | 1 + src/protocol/codec/serial.c | 116 +++++++++++++++++ src/system/arduino/esp32/system.c | 1 - .../{platform_common.c => common/platform.c} | 2 +- src/system/common/serial.c | 80 ++++++++++++ src/system/rpi_pico/network.c | 121 ++++-------------- src/system/unix/system.c | 1 - tests/z_msgcodec_test.c | 69 ++++++++++ tests/z_refcount_test.c | 2 +- 19 files changed, 434 insertions(+), 105 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/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/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/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..b5c0e0b4a --- /dev/null +++ b/src/protocol/codec/serial.c @@ -0,0 +1,116 @@ +// +// 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 "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%08lX, Computed: 0x%08lX", received_crc, computed_crc); + return SIZE_MAX; + } + + return wire_size; +} 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..2104818ea --- /dev/null +++ b/src/system/common/serial.c @@ -0,0 +1,80 @@ +// +// 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" + +#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; +} 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/tests/z_msgcodec_test.c b/tests/z_msgcodec_test.c index cb7b9a927..fd827c620 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() { + 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}; + + int 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; + int 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() { + 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}; + + int 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}; + int 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() { + 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}; + + int 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 + int 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