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")