diff --git a/include/zenoh-pico/collections/list.h b/include/zenoh-pico/collections/list.h index 339e7c25f..210737047 100644 --- a/include/zenoh-pico/collections/list.h +++ b/include/zenoh-pico/collections/list.h @@ -42,6 +42,7 @@ void *_z_list_head(const _z_list_t *xs); _z_list_t *_z_list_tail(const _z_list_t *xs); _z_list_t *_z_list_push(_z_list_t *xs, void *x); +_z_list_t *_z_list_push_back(_z_list_t *xs, void *x); _z_list_t *_z_list_pop(_z_list_t *xs, z_element_free_f f_f, void **x); _z_list_t *_z_list_find(const _z_list_t *xs, z_element_eq_f f_f, void *e); diff --git a/include/zenoh-pico/protocol/keyexpr.h b/include/zenoh-pico/protocol/keyexpr.h index abb165170..193d3b54e 100644 --- a/include/zenoh-pico/protocol/keyexpr.h +++ b/include/zenoh-pico/protocol/keyexpr.h @@ -34,8 +34,6 @@ static inline _z_keyexpr_t _z_keyexpr_null(void) { _z_keyexpr_t keyexpr = {0, {0}, NULL}; return keyexpr; } -_z_timestamp_t _z_timestamp_duplicate(const _z_timestamp_t *tstamp); -void _z_timestamp_clear(_z_timestamp_t *tstamp); void _z_keyexpr_clear(_z_keyexpr_t *rk); void _z_keyexpr_free(_z_keyexpr_t **rk); diff --git a/include/zenoh-pico/system/platform.h b/include/zenoh-pico/system/platform.h index 5852928de..dbfab19d9 100644 --- a/include/zenoh-pico/system/platform.h +++ b/include/zenoh-pico/system/platform.h @@ -35,6 +35,8 @@ #include "zenoh-pico/system/platform/arduino/opencr.h" #elif defined(ZENOH_EMSCRIPTEN) #include "zenoh-pico/system/platform/emscripten.h" +#elif defined(ZENOH_FLIPPER) +#include "zenoh-pico/system/platform/flipper.h" #elif defined(ZENOH_FREERTOS_PLUS_TCP) #include "zenoh-pico/system/platform/freertos_plus_tcp.h" #else diff --git a/include/zenoh-pico/system/platform/flipper.h b/include/zenoh-pico/system/platform/flipper.h new file mode 100644 index 000000000..f32e51f4f --- /dev/null +++ b/include/zenoh-pico/system/platform/flipper.h @@ -0,0 +1,46 @@ +// +// 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_FLIPPER_TYPES_H +#define ZENOH_PICO_SYSTEM_FLIPPER_TYPES_H + +#include +#include +#include + +#include "zenoh-pico/config.h" + +#define FLIPPER_DEFAULT_THREAD_STACK_SIZE 2048 +#define FLIPPER_SERIAL_STREAM_BUFFER_SIZE 512 +#define FLIPPER_SERIAL_STREAM_TRIGGERED_LEVEL 10 +#define FLIPPER_SERIAL_TIMEOUT_MS 200 + +#if Z_FEATURE_MULTI_THREAD == 1 +typedef FuriThread* zp_task_t; +typedef uint32_t zp_task_attr_t; +typedef FuriMutex* zp_mutex_t; +typedef void* zp_condvar_t; +#endif // Z_FEATURE_MULTI_THREAD == 1 + +typedef struct timespec zp_clock_t; +typedef struct timeval zp_time_t; + +typedef struct { +#if Z_FEATURE_LINK_SERIAL == 1 + FuriStreamBuffer* _rx_stream; + FuriHalSerialHandle* _serial; +#endif +} _z_sys_net_socket_t; + +#endif /* ZENOH_PICO_SYSTEM_FLIPPER_TYPES_H */ diff --git a/include/zenoh-pico/utils/logging.h b/include/zenoh-pico/utils/logging.h index 5b8f73136..c8bdc8352 100644 --- a/include/zenoh-pico/utils/logging.h +++ b/include/zenoh-pico/utils/logging.h @@ -47,7 +47,7 @@ static inline void __z_print_timestamp(void) { if (ZENOH_DEBUG >= _Z_LOG_LVL_DEBUG) { \ _Z_LOG_PREFIX(DEBUG); \ printf(__VA_ARGS__); \ - printf("\n"); \ + printf("\r\n"); \ } \ } while (false) @@ -56,7 +56,7 @@ static inline void __z_print_timestamp(void) { if (ZENOH_DEBUG >= _Z_LOG_LVL_INFO) { \ _Z_LOG_PREFIX(INFO); \ printf(__VA_ARGS__); \ - printf("\n"); \ + printf("\r\n"); \ } \ } while (false) @@ -65,7 +65,7 @@ static inline void __z_print_timestamp(void) { if (ZENOH_DEBUG >= _Z_LOG_LVL_ERROR) { \ _Z_LOG_PREFIX(ERROR); \ printf(__VA_ARGS__); \ - printf("\n"); \ + printf("\r\n"); \ } \ } while (false) #endif // ZENOH_DEBUG == 0 && !defined(Z_BUILD_DEBUG) diff --git a/src/collections/list.c b/src/collections/list.c index b47340d8b..f19b72f6f 100644 --- a/src/collections/list.c +++ b/src/collections/list.c @@ -12,10 +12,10 @@ // ZettaScale Zenoh Team, // -#include "zenoh-pico/collections/list.h" - #include +#include "zenoh-pico/collections/list.h" + /*-------- Inner single-linked list --------*/ _z_list_t *_z_list_of(void *x) { _z_list_t *xs = (_z_list_t *)zp_malloc(sizeof(_z_list_t)); @@ -32,6 +32,20 @@ _z_list_t *_z_list_push(_z_list_t *xs, void *x) { return lst; } +_z_list_t *_z_list_push_back(_z_list_t *xs, void *x) { + _z_list_t *l = (_z_list_t *)xs; + while (l != NULL && l->_tail != NULL) { + l = l->_tail; + } + if (l == NULL) { + l = _z_list_of(x); + return l; + } else { + l->_tail = _z_list_of(x); + return xs; + } +} + void *_z_list_head(const _z_list_t *xs) { return xs->_val; } _z_list_t *_z_list_tail(const _z_list_t *xs) { return xs->_tail; } diff --git a/src/system/flipper/network.c b/src/system/flipper/network.c new file mode 100644 index 000000000..ecb092879 --- /dev/null +++ b/src/system/flipper/network.c @@ -0,0 +1,228 @@ +// +// 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/config.h" +#include "zenoh-pico/system/link/serial.h" +#include "zenoh-pico/system/platform.h" +#include "zenoh-pico/utils/logging.h" + +#if Z_FEATURE_LINK_TCP == 1 +#error "TCP is not supported on Flipper port of Zenoh-Pico" +#endif + +#if Z_FEATURE_LINK_UDP_UNICAST == 1 || Z_FEATURE_LINK_UDP_MULTICAST == 1 +#error "UDP is not supported on Flipper port of Zenoh-Pico" +#endif + +#if Z_FEATURE_LINK_SERIAL == 1 + +/*------------------ Serial sockets ------------------*/ + +static void _z_serial_received_byte_callback(FuriHalSerialHandle* handle, FuriHalSerialRxEvent event, void* context) { + if (!context) { + return; + } + FuriStreamBuffer* buf = context; + + if (event == FuriHalSerialRxEventData) { + uint8_t data = furi_hal_serial_async_rx(handle); + furi_stream_buffer_send(buf, (void*)&data, 1, FLIPPER_SERIAL_STREAM_TRIGGERED_LEVEL); + } +} + +int8_t _z_open_serial_from_pins(_z_sys_net_socket_t* sock, uint32_t txpin, uint32_t rxpin, uint32_t baudrate) { + int8_t ret = _Z_ERR_GENERIC; + + return ret; +} + +int8_t _z_open_serial_from_dev(_z_sys_net_socket_t* sock, char* dev, uint32_t baudrate) { + if (furi_hal_serial_control_is_busy(FuriHalSerialIdUsart)) { + _Z_ERROR("Serial port is busy"); + return _Z_ERR_TRANSPORT_OPEN_FAILED; + } + + FuriHalSerialId sid; + if (!strcmp(dev, "usart")) { + sid = FuriHalSerialIdUsart; + } else if (!strcmp(dev, "lpuart")) { + sid = FuriHalSerialIdLpuart; + } else { + _Z_ERROR("Unknown serial port device: %s", dev); + return _Z_ERR_TRANSPORT_OPEN_FAILED; + } + + sock->_serial = furi_hal_serial_control_acquire(sid); + if (!sock->_serial) { + _Z_ERROR("Serial port control acquire failed"); + return _Z_ERR_TRANSPORT_OPEN_FAILED; + }; + furi_hal_serial_init(sock->_serial, baudrate); + + sock->_rx_stream = + furi_stream_buffer_alloc(FLIPPER_SERIAL_STREAM_BUFFER_SIZE, FLIPPER_SERIAL_STREAM_TRIGGERED_LEVEL); + if (!sock->_rx_stream) { + _Z_ERROR("Serial stream buffer allocation failed"); + return _Z_ERR_TRANSPORT_NO_SPACE; + }; + 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; +} + +int8_t _z_listen_serial_from_pins(_z_sys_net_socket_t* sock, uint32_t txpin, uint32_t rxpin, uint32_t baudrate) { + (void)(sock); + (void)(txpin); + (void)(rxpin); + (void)(baudrate); + + return _Z_ERR_GENERIC; +} + +int8_t _z_listen_serial_from_dev(_z_sys_net_socket_t* sock, char* dev, uint32_t baudrate) { + (void)(sock); + (void)(dev); + (void)(baudrate); + + // @TODO: To be implemented + + return _Z_ERR_GENERIC; +} + +void _z_close_serial(_z_sys_net_socket_t* sock) { + if (sock->_serial) { + furi_hal_serial_async_rx_stop(sock->_serial); + furi_hal_serial_deinit(sock->_serial); + furi_hal_serial_control_release(sock->_serial); + + // Wait until the serial read timeout ends + zp_sleep_ms(FLIPPER_SERIAL_TIMEOUT_MS * 2); + + furi_stream_buffer_free(sock->_rx_stream); + + sock->_serial = 0; + sock->_rx_stream = 0; + } + _Z_DEBUG("Serial port closed"); +} + +size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t* ptr, size_t len) { + int8_t ret = _Z_RES_OK; + + uint8_t* before_cobs = (uint8_t*)zp_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); + if (!len) { + zp_free(before_cobs); + return SIZE_MAX; + } + rb += 1; + + if (before_cobs[i] == (uint8_t)0x00) { + break; + } + } + + uint8_t* after_cobs = (uint8_t*)zp_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; + } + + zp_free(before_cobs); + zp_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 = len; + size_t rb = 0; + + do { + 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(const _z_sys_net_socket_t sock, const uint8_t* ptr, size_t len) { + int8_t ret = _Z_RES_OK; + + uint8_t* before_cobs = (uint8_t*)zp_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 += 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*)zp_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_wait_complete(sock._serial); + + zp_free(before_cobs); + zp_free(after_cobs); + + return len; +} +#endif + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported on Flipper port of Zenoh-Pico" +#endif diff --git a/src/system/flipper/system.c b/src/system/flipper/system.c new file mode 100644 index 000000000..fb6170853 --- /dev/null +++ b/src/system/flipper/system.c @@ -0,0 +1,295 @@ +// +// 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/config.h" +#include "zenoh-pico/system/platform.h" +#include "zenoh-pico/utils/result.h" + +/*------------------ Random ------------------*/ +uint8_t zp_random_u8(void) { return random(); } + +uint16_t zp_random_u16(void) { return random(); } + +uint32_t zp_random_u32(void) { return random(); } + +uint64_t zp_random_u64(void) { + uint64_t ret = 0; + ret |= zp_random_u32(); + ret = ret << 32; + ret |= zp_random_u32(); + + return ret; +} + +void zp_random_fill(void* buf, size_t len) { + for (size_t i = 0; i < len; i++) { + ((uint8_t*)buf)[i] = zp_random_u8(); + } +} + +/*------------------ Memory ------------------*/ +void* zp_malloc(size_t size) { + if (!size) { + return NULL; + } + return malloc(size); +} + +void* zp_realloc(void* ptr, size_t size) { + if (!size) { + free(ptr); + return NULL; + } + return realloc(ptr, size); +} + +void zp_free(void* ptr) { return free(ptr); } + +/*------------------ Task ------------------*/ + +int8_t zp_task_init(zp_task_t* task, zp_task_attr_t* attr, void* (*fun)(void*), void* arg) { + if (task == NULL) { + return -1; + } + + uint32_t stack_size = FLIPPER_DEFAULT_THREAD_STACK_SIZE; + if (attr) { + stack_size = *attr; + } + + *task = furi_thread_alloc_ex(NULL, stack_size, (FuriThreadCallback)fun, arg); + if (*task == NULL) { + return -1; + } + furi_thread_start(*task); + return _Z_RES_OK; +} + +int8_t zp_task_join(zp_task_t* task) { + if (task == NULL) { + return -1; + } + return furi_thread_join(*task); +} + +int8_t zp_task_cancel(zp_task_t* task) { return -1; } + +void zp_task_free(zp_task_t** task) { + if (task == NULL || *task == NULL) { + return; + } + furi_thread_free(**task); + *task = NULL; +} + +/*------------------ Mutex ------------------*/ +int8_t zp_mutex_init(zp_mutex_t* m) { + if (m == NULL) { + return -1; + } + *m = furi_mutex_alloc(FuriMutexTypeRecursive); + return (*m != 0) ? _Z_RES_OK : _Z_ERR_SYSTEM_TASK_FAILED; +} + +int8_t zp_mutex_free(zp_mutex_t* m) { + if (m == NULL) { + return -1; + } + if (*m == 0) { + return 0; + } + furi_mutex_free(*m); + *m = NULL; + return 0; +} + +int8_t zp_mutex_lock(zp_mutex_t* m) { + if (m == NULL) { + return -1; + } + if (*m == 0) { + return 0; + } + return furi_mutex_acquire(*m, FuriWaitForever); +} + +int8_t zp_mutex_trylock(zp_mutex_t* m) { return -1; } + +int8_t zp_mutex_unlock(zp_mutex_t* m) { + if (m == NULL) { + return -1; + } + if (*m == 0) { + return 0; + } + return furi_mutex_release(*m); +} + +/*------------------ Condvar ------------------*/ +int8_t zp_condvar_init(zp_condvar_t* cv) { return -1; } + +int8_t zp_condvar_free(zp_condvar_t* cv) { return -1; } + +int8_t zp_condvar_signal(zp_condvar_t* cv) { return -1; } + +int8_t zp_condvar_wait(zp_condvar_t* cv, zp_mutex_t* m) { return -1; } + +/*------------------ Sleep ------------------*/ +int zp_sleep_us(size_t time) { + furi_delay_us(time); + return 0; +} + +int zp_sleep_ms(size_t time) { + furi_delay_ms(time); + return 0; +} + +int zp_sleep_s(size_t time) { + zp_time_t start = zp_time_now(); + + // Most sleep APIs promise to sleep at least whatever you asked them to. + // This may compound, so this approach may make sleeps longer than expected. + // This extra check tries to minimize the amount of extra time it might sleep. + while (zp_time_elapsed_s(&start) < time) { + zp_sleep_ms(1000); + } + + return 0; +} + +/*------------------ Instant ------------------*/ +void __zp_clock_gettime(zp_clock_t* ts) { + uint64_t m = millis(); + ts->tv_sec = m / (uint64_t)1000000; + ts->tv_nsec = (m % (uint64_t)1000000) * (uint64_t)1000; +} + +zp_clock_t zp_clock_now(void) { + zp_clock_t now; + __zp_clock_gettime(&now); + return now; +} + +unsigned long zp_clock_elapsed_us(zp_clock_t* instant) { + zp_clock_t now; + __zp_clock_gettime(&now); + + unsigned long elapsed = (1000000 * (now.tv_sec - instant->tv_sec) + (now.tv_nsec - instant->tv_nsec) / 1000); + return elapsed; +} + +unsigned long zp_clock_elapsed_ms(zp_clock_t* instant) { + zp_clock_t now; + __zp_clock_gettime(&now); + + unsigned long elapsed = (1000 * (now.tv_sec - instant->tv_sec) + (now.tv_nsec - instant->tv_nsec) / 1000000); + return elapsed; +} + +unsigned long zp_clock_elapsed_s(zp_clock_t* instant) { + zp_clock_t now; + __zp_clock_gettime(&now); + + unsigned long elapsed = now.tv_sec - instant->tv_sec; + return elapsed; +} + +/*------------------ Time ------------------*/ +zp_time_t zp_time_now(void) { + zp_time_t now; + gettimeofday(&now, NULL); + return now; +} + +const char* zp_time_now_as_str(char* const buf, unsigned long buflen) { + zp_time_t tv = zp_time_now(); + struct tm ts; + ts = *localtime(&tv.tv_sec); + strftime(buf, buflen, "%Y-%m-%dT%H:%M:%SZ", &ts); + return buf; +} + +unsigned long zp_time_elapsed_us(zp_time_t* time) { + zp_time_t now; + gettimeofday(&now, NULL); + + unsigned long elapsed = (1000000 * (now.tv_sec - time->tv_sec) + (now.tv_usec - time->tv_usec)); + return elapsed; +} + +unsigned long zp_time_elapsed_ms(zp_time_t* time) { + zp_time_t now; + gettimeofday(&now, NULL); + + unsigned long elapsed = (1000 * (now.tv_sec - time->tv_sec) + (now.tv_usec - time->tv_usec) / 1000); + return elapsed; +} + +unsigned long zp_time_elapsed_s(zp_time_t* time) { + zp_time_t now; + gettimeofday(&now, NULL); + + unsigned long elapsed = now.tv_sec - time->tv_sec; + return elapsed; +} + +char* strncat(char* dest, const char* src, size_t dest_size) { + size_t dest_len = strlen(dest); + size_t i; + + for (i = 0; i < dest_size && src[i] != '\0'; i++) { + dest[dest_len + i] = src[i]; + } + dest[dest_len + i] = '\0'; + + return dest; +} + +char* strpbrk(const char* str, const char* charset) { + while (*str != '\0') { + const char* c = charset; + while (*c != '\0') { + if (*str == *c) { + return (char*)str; + } + c++; + } + str++; + } + return NULL; +} + +size_t strftime(char* s, size_t max, const char* format, const struct tm* tm) { + // not supported + s[0] = 0; + return 0; +} + +int gettimeofday(struct timeval* __restrict __p, void* __restrict __tz) { + // not supported + __p->tv_sec = 0; + __p->tv_usec = 0; + return 0; +} + +struct tm* localtime(const time_t* timep) { + // not supported + static struct tm t; + return &t; +}