From 1fc24eaaa7a4bc78e3ce693c5cb216f19eb65075 Mon Sep 17 00:00:00 2001 From: Alexander Bushnev Date: Mon, 2 Dec 2024 15:25:42 +0100 Subject: [PATCH] Proper system implementation --- .../zenoh-pico/system/platform/rpi_pico_w.h | 11 +- src/system/rpi_pico_w/system.c | 123 ++++++++++++++---- 2 files changed, 105 insertions(+), 29 deletions(-) diff --git a/include/zenoh-pico/system/platform/rpi_pico_w.h b/include/zenoh-pico/system/platform/rpi_pico_w.h index 5becc4602..4a50bd6b6 100644 --- a/include/zenoh-pico/system/platform/rpi_pico_w.h +++ b/include/zenoh-pico/system/platform/rpi_pico_w.h @@ -1,5 +1,5 @@ // -// Copyright (c) 2023 Fictionlab sp. z o.o. +// 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 @@ -9,13 +9,12 @@ // SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 // // Contributors: -// Błażej Sowa, +// ZettaScale Zenoh Team, #ifndef ZENOH_PICO_SYSTEM_FREERTOS_PLUS_TCP_TYPES_H #define ZENOH_PICO_SYSTEM_FREERTOS_PLUS_TCP_TYPES_H #include "FreeRTOS.h" -// #include "FreeRTOS_IP.h" #include "event_groups.h" #include "lwip/ip4_addr.h" #include "semphr.h" @@ -42,7 +41,11 @@ typedef struct { } _z_task_t; typedef SemaphoreHandle_t _z_mutex_t; -typedef void *_z_condvar_t; +typedef struct { + SemaphoreHandle_t mutex; + SemaphoreHandle_t sem; + int waiters; +} _z_condvar_t; #endif // Z_MULTI_THREAD == 1 typedef struct timespec z_clock_t; diff --git a/src/system/rpi_pico_w/system.c b/src/system/rpi_pico_w/system.c index 4caaeefee..45cbf022f 100644 --- a/src/system/rpi_pico_w/system.c +++ b/src/system/rpi_pico_w/system.c @@ -1,6 +1,5 @@ // -// Copyright (c) 2022 ZettaScale Technology -// Copyright (c) 2023 Fictionlab sp. z o.o. +// 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 @@ -11,31 +10,26 @@ // // Contributors: // ZettaScale Zenoh Team, -// Błażej Sowa, #include #include +#include #include #include #include "FreeRTOS.h" #include "zenoh-pico/config.h" #include "zenoh-pico/system/platform.h" +#include "zenoh-pico/utils/result.h" /*------------------ Random ------------------*/ -uint8_t z_random_u8(void) { return z_random_u32(); } +uint8_t z_random_u8(void) { return (uint8_t)get_rand_32(); } -uint16_t z_random_u16(void) { return z_random_u32(); } +uint16_t z_random_u16(void) { return (uint16_t)get_rand_32(); } -uint32_t z_random_u32(void) { return rand(); } +uint32_t z_random_u32(void) { return get_rand_32(); } -uint64_t z_random_u64(void) { - uint64_t ret = 0; - ret |= z_random_u32(); - ret = ret << 32; - ret |= z_random_u32(); - return ret; -} +uint64_t z_random_u64(void) { return get_rand_64(); } void z_random_fill(void *buf, size_t len) { for (size_t i = 0; i < len; i++) { @@ -46,7 +40,13 @@ void z_random_fill(void *buf, size_t len) { /*------------------ Memory ------------------*/ void *z_malloc(size_t size) { return pvPortMalloc(size); } -void *z_realloc(void *ptr, size_t size) { return pvPortRealloc(ptr, size); } +void *z_realloc(void *ptr, size_t size) { + _ZP_UNUSED(ptr); + _ZP_UNUSED(size); + // realloc not implemented in FreeRTOS + assert(false); + return NULL; +} void z_free(void *ptr) { vPortFree(ptr); } @@ -118,7 +118,8 @@ z_result_t _z_task_join(_z_task_t *task) { } z_result_t _z_task_detach(_z_task_t *task) { - // Not implemented + _ZP_UNUSED(task); + assert(false); return _Z_ERR_GENERIC; } @@ -150,12 +151,78 @@ z_result_t _z_mutex_try_lock(_z_mutex_t *m) { return xSemaphoreTakeRecursive(*m, z_result_t _z_mutex_unlock(_z_mutex_t *m) { return xSemaphoreGiveRecursive(*m) == pdTRUE ? 0 : -1; } /*------------------ CondVar ------------------*/ -// Condition variables not supported in FreeRTOS -z_result_t _z_condvar_init(_z_condvar_t *cv) { return -1; } -z_result_t _z_condvar_drop(_z_condvar_t *cv) { return -1; } -z_result_t _z_condvar_signal(_z_condvar_t *cv) { return -1; } -z_result_t _z_condvar_signal_all(_z_condvar_t *cv) { return -1; } -z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return -1; } +static UBaseType_t CONDVAR_MAX_WAITERS_COUNT = 255; + +z_result_t _z_condvar_init(_z_condvar_t *cv) { + if (!cv) { + return _Z_ERR_GENERIC; + } + cv->mutex = xSemaphoreCreateMutex(); + cv->sem = xSemaphoreCreateCounting(CONDVAR_MAX_WAITERS_COUNT, 0); + cv->waiters = 0; + + if (!cv->mutex || !cv->sem) { + return _Z_ERR_GENERIC; + } + return _Z_RES_OK; +} + +z_result_t _z_condvar_drop(_z_condvar_t *cv) { + if (!cv) { + return _Z_ERR_GENERIC; + } + vSemaphoreDelete(cv->sem); + vSemaphoreDelete(cv->mutex); + return _Z_RES_OK; +} + +z_result_t _z_condvar_signal(_z_condvar_t *cv) { + if (!cv) { + return _Z_ERR_GENERIC; + } + + xSemaphoreTake(cv->mutex, portMAX_DELAY); + if (cv->waiters > 0) { + xSemaphoreGive(cv->sem); + cv->waiters--; + } + xSemaphoreGive(cv->mutex); + + return _Z_RES_OK; +} + +z_result_t _z_condvar_signal_all(_z_condvar_t *cv) { + if (!cv) { + return _Z_ERR_GENERIC; + } + + xSemaphoreTake(cv->mutex, portMAX_DELAY); + while (cv->waiters > 0) { + xSemaphoreGive(cv->sem); + cv->waiters--; + } + xSemaphoreGive(cv->mutex); + + return _Z_RES_OK; +} + +z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { + if (!cv || !m) { + return _Z_ERR_GENERIC; + } + + xSemaphoreTake(cv->mutex, portMAX_DELAY); + cv->waiters++; + xSemaphoreGive(cv->mutex); + + xSemaphoreGive(*m); + + xSemaphoreTake(cv->sem, portMAX_DELAY); + + xSemaphoreTake(*m, portMAX_DELAY); + + return _Z_RES_OK; +} #endif // Z_MULTI_THREAD == 1 /*------------------ Sleep ------------------*/ @@ -175,15 +242,21 @@ z_result_t z_sleep_s(size_t time) { } /*------------------ Clock ------------------*/ +void __z_clock_gettime(z_clock_t *ts) { + uint64_t m = xTaskGetTickCount() / portTICK_PERIOD_MS; + ts->tv_sec = m / (uint64_t)1000000; + ts->tv_nsec = (m % (uint64_t)1000000) * (uint64_t)1000; +} + z_clock_t z_clock_now(void) { z_clock_t now; - // clock_gettime(CLOCK_MONOTONIC, &now); + __z_clock_gettime(&now); return now; } unsigned long z_clock_elapsed_us(z_clock_t *instant) { z_clock_t now; - // clock_gettime(CLOCK_MONOTONIC, &now); + __z_clock_gettime(&now); unsigned long elapsed = (unsigned long)(1000000 * (now.tv_sec - instant->tv_sec) + (now.tv_nsec - instant->tv_nsec) / 1000); @@ -192,7 +265,7 @@ unsigned long z_clock_elapsed_us(z_clock_t *instant) { unsigned long z_clock_elapsed_ms(z_clock_t *instant) { z_clock_t now; - // clock_gettime(CLOCK_MONOTONIC, &now); + __z_clock_gettime(&now); unsigned long elapsed = (unsigned long)(1000 * (now.tv_sec - instant->tv_sec) + (now.tv_nsec - instant->tv_nsec) / 1000000); @@ -201,7 +274,7 @@ unsigned long z_clock_elapsed_ms(z_clock_t *instant) { unsigned long z_clock_elapsed_s(z_clock_t *instant) { z_clock_t now; - // clock_gettime(CLOCK_MONOTONIC, &now); + __z_clock_gettime(&now); unsigned long elapsed = (unsigned long)(now.tv_sec - instant->tv_sec); return elapsed;