From beb3b15953efefc777de83cb48742c02a4ae4ebe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 13:36:33 +0100 Subject: [PATCH 01/10] Fix mutex drop and support static mutex allocation # Conflicts: # include/zenoh-pico/system/platform/freertos_plus_tcp.h --- .../system/platform/freertos_plus_tcp.h | 7 +++++- src/system/freertos_plus_tcp/system.c | 24 +++++++++++++------ 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/include/zenoh-pico/system/platform/freertos_plus_tcp.h b/include/zenoh-pico/system/platform/freertos_plus_tcp.h index 97d6c1b10..06f750f93 100644 --- a/include/zenoh-pico/system/platform/freertos_plus_tcp.h +++ b/include/zenoh-pico/system/platform/freertos_plus_tcp.h @@ -39,7 +39,12 @@ typedef struct { EventGroupHandle_t join_event; } _z_task_t; -typedef SemaphoreHandle_t _z_mutex_t; +typedef struct { + SemaphoreHandle_t handle; +#if (configSUPPORT_STATIC_ALLOCATION == 1) + StaticSemaphore_t buffer; +#endif /* SUPPORT_STATIC_ALLOCATION */ +} _z_mutex_t; typedef struct { SemaphoreHandle_t mutex; SemaphoreHandle_t sem; diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 395e6bc59..84522c4a0 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -141,20 +141,30 @@ void _z_task_free(_z_task_t **task) { /*------------------ Mutex ------------------*/ z_result_t _z_mutex_init(_z_mutex_t *m) { - *m = xSemaphoreCreateRecursiveMutex(); - return *m == NULL ? -1 : 0; +#if (configSUPPORT_STATIC_ALLOCATION == 1) + m->handle = xSemaphoreCreateRecursiveMutexStatic(&m->buffer); +#else + m->handle = xSemaphoreCreateRecursiveMutex(); +#endif /* SUPPORT_STATIC_ALLOCATION */ + return m->handle == NULL ? _Z_ERR_GENERIC : _Z_RES_OK; } z_result_t _z_mutex_drop(_z_mutex_t *m) { - z_free(*m); - return 0; + vSemaphoreDelete(m->handle); + return _Z_RES_OK; } -z_result_t _z_mutex_lock(_z_mutex_t *m) { return xSemaphoreTakeRecursive(*m, portMAX_DELAY) == pdTRUE ? 0 : -1; } +z_result_t _z_mutex_lock(_z_mutex_t *m) { + return xSemaphoreTakeRecursive(m->handle, portMAX_DELAY) == pdTRUE ? _Z_RES_OK : _Z_ERR_GENERIC; +} -z_result_t _z_mutex_try_lock(_z_mutex_t *m) { return xSemaphoreTakeRecursive(*m, 0) == pdTRUE ? 0 : -1; } +z_result_t _z_mutex_try_lock(_z_mutex_t *m) { + return xSemaphoreTakeRecursive(m->handle, 0) == pdTRUE ? _Z_RES_OK : _Z_ERR_GENERIC; +} -z_result_t _z_mutex_unlock(_z_mutex_t *m) { return xSemaphoreGiveRecursive(*m) == pdTRUE ? 0 : -1; } +z_result_t _z_mutex_unlock(_z_mutex_t *m) { + return xSemaphoreGiveRecursive(m->handle) == pdTRUE ? _Z_RES_OK : _Z_ERR_GENERIC; +} /*------------------ CondVar ------------------*/ z_result_t _z_condvar_init(_z_condvar_t *cv) { From b04551e308ce20574d2e65a90340457254179d81 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 13:40:39 +0100 Subject: [PATCH 02/10] Mark realloc args as unused --- src/system/freertos_plus_tcp/system.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 84522c4a0..7ebb48f33 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -51,6 +51,8 @@ void z_random_fill(void *buf, size_t len) { void *z_malloc(size_t size) { return pvPortMalloc(size); } void *z_realloc(void *ptr, size_t size) { + _ZP_UNUSED(ptr); + _ZP_UNUSED(size); // realloc not implemented in FreeRTOS return NULL; } From 078bda6eecafdb613d05cbf5db9494cad41715de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 13:59:59 +0100 Subject: [PATCH 03/10] Use _z_task_t as a task wrapper argument --- .../system/platform/freertos_plus_tcp.h | 5 +++ src/system/freertos_plus_tcp/system.c | 36 ++++++++----------- 2 files changed, 20 insertions(+), 21 deletions(-) diff --git a/include/zenoh-pico/system/platform/freertos_plus_tcp.h b/include/zenoh-pico/system/platform/freertos_plus_tcp.h index 06f750f93..05ec64c66 100644 --- a/include/zenoh-pico/system/platform/freertos_plus_tcp.h +++ b/include/zenoh-pico/system/platform/freertos_plus_tcp.h @@ -37,6 +37,11 @@ typedef struct { typedef struct { TaskHandle_t handle; EventGroupHandle_t join_event; + void *(*fun)(void *); + void *arg; +#if (configSUPPORT_STATIC_ALLOCATION == 1) + StaticEventGroup_t join_event_buffer; +#endif /* SUPPORT_STATIC_ALLOCATION */ } _z_task_t; typedef struct { diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 7ebb48f33..b3f0497e9 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -62,16 +62,11 @@ void z_free(void *ptr) { vPortFree(ptr); } #if Z_FEATURE_MULTI_THREAD == 1 // In FreeRTOS, tasks created using xTaskCreate must end with vTaskDelete. // A task function should __not__ simply return. -typedef struct { - void *(*fun)(void *); - void *arg; - EventGroupHandle_t join_event; -} z_task_arg; - static void z_task_wrapper(void *arg) { - z_task_arg *targ = (z_task_arg *)arg; - targ->fun(targ->arg); - xEventGroupSetBits(targ->join_event, 1); + _z_task_t *task = (_z_task_t *)arg; + task->fun(task->arg); + xEventGroupSetBits(task->join_event, 1); + task->handle = NULL; vTaskDelete(NULL); } @@ -88,14 +83,14 @@ static z_task_attr_t z_default_task_attr = { /*------------------ Thread ------------------*/ z_result_t _z_task_init(_z_task_t *task, z_task_attr_t *attr, void *(*fun)(void *), void *arg) { - z_task_arg *z_arg = (z_task_arg *)z_malloc(sizeof(z_task_arg)); - if (z_arg == NULL) { - return -1; - } + task->fun = fun; + task->arg = arg; - z_arg->fun = fun; - z_arg->arg = arg; - z_arg->join_event = task->join_event = xEventGroupCreate(); +#if (configSUPPORT_STATIC_ALLOCATION == 1) + task->join_event = xEventGroupCreateStatic(&task->join_event_buffer); +#else + task->join_event = xEventGroupCreate(); +#endif /* SUPPORT_STATIC_ALLOCATION */ if (attr == NULL) { attr = &z_default_task_attr; @@ -103,22 +98,21 @@ z_result_t _z_task_init(_z_task_t *task, z_task_attr_t *attr, void *(*fun)(void #if (configSUPPORT_STATIC_ALLOCATION == 1) if (attr->static_allocation) { - task->handle = xTaskCreateStatic(z_task_wrapper, attr->name, attr->stack_depth, z_arg, attr->priority, + task->handle = xTaskCreateStatic(z_task_wrapper, attr->name, attr->stack_depth, task, attr->priority, attr->stack_buffer, attr->task_buffer); if (task->handle == NULL) { - return -1; + return _Z_ERR_GENERIC; } } else { #endif /* SUPPORT_STATIC_ALLOCATION */ - if (xTaskCreate(z_task_wrapper, attr->name, attr->stack_depth, z_arg, attr->priority, &task->handle) != - pdPASS) { + if (xTaskCreate(z_task_wrapper, attr->name, attr->stack_depth, task, attr->priority, &task->handle) != pdPASS) { return -1; } #if (configSUPPORT_STATIC_ALLOCATION == 1) } #endif /* SUPPORT_STATIC_ALLOCATION */ - return 0; + return _Z_RES_OK; } z_result_t _z_task_join(_z_task_t *task) { From 4cb02ec2dd63c3158364d806aab1b085e19e9424 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 14:03:35 +0100 Subject: [PATCH 04/10] Fix task cancelling --- src/system/freertos_plus_tcp/system.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index b3f0497e9..4ea0ecd5e 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -121,12 +121,16 @@ z_result_t _z_task_join(_z_task_t *task) { } z_result_t _z_task_detach(_z_task_t *task) { - // Not implemented - return _Z_ERR_GENERIC; + // Note: task/thread detach not supported on FreeRTOS API, so we force its deletion instead. + return _z_task_cancel(task); } z_result_t _z_task_cancel(_z_task_t *task) { - vTaskDelete(task->handle); + xEventGroupSetBits(task->join_event, 1); + if (task->handle != NULL) { + vTaskDelete(task->handle); + task->handle = NULL; + } return 0; } From f342344384ef0e221fd0346d5494ac9bacde0c0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 14:04:05 +0100 Subject: [PATCH 05/10] Fix task free --- src/system/freertos_plus_tcp/system.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 4ea0ecd5e..94d286fcf 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -135,8 +135,9 @@ z_result_t _z_task_cancel(_z_task_t *task) { } void _z_task_free(_z_task_t **task) { - z_free((*task)->join_event); - z_free(*task); + _z_task_t *ptr = *task; + vEventGroupDelete(ptr->join_event); + *task = NULL; } /*------------------ Mutex ------------------*/ From e0d9c4f6715a61e031ebdcc7a18a8d0b529531af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 14:07:33 +0100 Subject: [PATCH 06/10] Use _Z_RES_OK instead of 0 --- src/system/freertos_plus_tcp/system.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 94d286fcf..c2e980a1e 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -117,7 +117,7 @@ z_result_t _z_task_init(_z_task_t *task, z_task_attr_t *attr, void *(*fun)(void z_result_t _z_task_join(_z_task_t *task) { xEventGroupWaitBits(task->join_event, 1, pdFALSE, pdFALSE, portMAX_DELAY); - return 0; + return _Z_RES_OK; } z_result_t _z_task_detach(_z_task_t *task) { @@ -131,7 +131,7 @@ z_result_t _z_task_cancel(_z_task_t *task) { vTaskDelete(task->handle); task->handle = NULL; } - return 0; + return _Z_RES_OK; } void _z_task_free(_z_task_t **task) { @@ -249,17 +249,17 @@ z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { /*------------------ Sleep ------------------*/ z_result_t z_sleep_us(size_t time) { vTaskDelay(pdMS_TO_TICKS(time / 1000)); - return 0; + return _Z_RES_OK; } z_result_t z_sleep_ms(size_t time) { vTaskDelay(pdMS_TO_TICKS(time)); - return 0; + return _Z_RES_OK; } z_result_t z_sleep_s(size_t time) { vTaskDelay(pdMS_TO_TICKS(time * 1000)); - return 0; + return _Z_RES_OK; } /*------------------ Clock ------------------*/ From 34e47b8bdb2530c241772355a9d6fba03fc3090a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 16:56:20 +0100 Subject: [PATCH 07/10] Use gettimeofday for z_time functions --- .../system/platform/freertos_plus_tcp.h | 4 +- src/system/freertos_plus_tcp/system.c | 50 +++++++++++++++---- 2 files changed, 44 insertions(+), 10 deletions(-) diff --git a/include/zenoh-pico/system/platform/freertos_plus_tcp.h b/include/zenoh-pico/system/platform/freertos_plus_tcp.h index 05ec64c66..a0a76e906 100644 --- a/include/zenoh-pico/system/platform/freertos_plus_tcp.h +++ b/include/zenoh-pico/system/platform/freertos_plus_tcp.h @@ -14,6 +14,8 @@ #ifndef ZENOH_PICO_SYSTEM_FREERTOS_PLUS_TCP_TYPES_H #define ZENOH_PICO_SYSTEM_FREERTOS_PLUS_TCP_TYPES_H +#include + #include "FreeRTOS.h" #include "FreeRTOS_IP.h" #include "semphr.h" @@ -62,7 +64,7 @@ typedef struct { #endif // Z_MULTI_THREAD == 1 typedef TickType_t z_clock_t; -typedef TickType_t z_time_t; +typedef struct timeval z_time_t; typedef struct { union { diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index c2e980a1e..28eda993c 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -17,6 +17,7 @@ #include #include #include +#include #include "FreeRTOS_IP.h" #include "zenoh-pico/config.h" @@ -263,31 +264,62 @@ z_result_t z_sleep_s(size_t time) { } /*------------------ Clock ------------------*/ -z_clock_t z_clock_now(void) { return z_time_now(); } +z_clock_t z_clock_now(void) { return xTaskGetTickCount(); } unsigned long z_clock_elapsed_us(z_clock_t *instant) { return z_clock_elapsed_ms(instant) * 1000; } -unsigned long z_clock_elapsed_ms(z_clock_t *instant) { return z_time_elapsed_ms(instant); } +unsigned long z_clock_elapsed_ms(z_clock_t *instant) { + z_clock_t now = z_clock_now(); + + unsigned long elapsed = (now - *instant) * portTICK_PERIOD_MS; + return elapsed; +} unsigned long z_clock_elapsed_s(z_clock_t *instant) { return z_clock_elapsed_ms(instant) / 1000; } /*------------------ Time ------------------*/ -z_time_t z_time_now(void) { return xTaskGetTickCount(); } +z_time_t z_time_now(void) { + z_time_t now; + gettimeofday(&now, NULL); + return now; +} const char *z_time_now_as_str(char *const buf, unsigned long buflen) { - snprintf(buf, buflen, "%u", z_time_now()); + z_time_t tv = z_time_now(); + struct tm ts; + ts = *localtime(&tv.tv_sec); + strftime(buf, buflen, "%Y-%m-%dT%H:%M:%SZ", &ts); return buf; } -unsigned long z_time_elapsed_us(z_time_t *time) { return z_time_elapsed_ms(time) * 1000; } +unsigned long z_time_elapsed_us(z_time_t *time) { + z_time_t now; + gettimeofday(&now, NULL); + + unsigned long elapsed = (unsigned long)(1000000 * (now.tv_sec - time->tv_sec) + (now.tv_usec - time->tv_usec)); + return elapsed; +} unsigned long z_time_elapsed_ms(z_time_t *time) { - z_time_t now = z_time_now(); + z_time_t now; + gettimeofday(&now, NULL); - unsigned long elapsed = (now - *time) * portTICK_PERIOD_MS; + unsigned long elapsed = (unsigned long)(1000 * (now.tv_sec - time->tv_sec) + (now.tv_usec - time->tv_usec) / 1000); return elapsed; } -unsigned long z_time_elapsed_s(z_time_t *time) { return z_time_elapsed_ms(time) / 1000; } +unsigned long z_time_elapsed_s(z_time_t *time) { + z_time_t now; + gettimeofday(&now, NULL); -z_result_t _z_get_time_since_epoch(_z_time_since_epoch *t) { return -1; } + unsigned long elapsed = (unsigned long)(now.tv_sec - time->tv_sec); + return elapsed; +} + +z_result_t _z_get_time_since_epoch(_z_time_since_epoch *t) { + z_time_t now; + gettimeofday(&now, NULL); + t->secs = (uint32_t)now.tv_sec; + t->nanos = (uint32_t)now.tv_usec * 1000; + return 0; +} From 17d689fbfa6b933791c6cb7e41ef689d60ceaf4e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 8 Dec 2024 17:12:01 +0100 Subject: [PATCH 08/10] Replace z_result returns with _Z_RES and _Z_ERR --- src/system/freertos_plus_tcp/system.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 28eda993c..5fe1ba1f4 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -107,7 +107,7 @@ z_result_t _z_task_init(_z_task_t *task, z_task_attr_t *attr, void *(*fun)(void } else { #endif /* SUPPORT_STATIC_ALLOCATION */ if (xTaskCreate(z_task_wrapper, attr->name, attr->stack_depth, task, attr->priority, &task->handle) != pdPASS) { - return -1; + return _Z_ERR_GENERIC; } #if (configSUPPORT_STATIC_ALLOCATION == 1) } @@ -321,5 +321,5 @@ z_result_t _z_get_time_since_epoch(_z_time_since_epoch *t) { gettimeofday(&now, NULL); t->secs = (uint32_t)now.tv_sec; t->nanos = (uint32_t)now.tv_usec * 1000; - return 0; + return _Z_RES_OK; } From c0c562e5f0623d91438c6ee6342c4cadf90616a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 9 Dec 2024 10:55:40 +0100 Subject: [PATCH 09/10] Fix potential race condition in z_task_cancel --- src/system/freertos_plus_tcp/system.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 5fe1ba1f4..52b80feca 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -128,10 +128,14 @@ z_result_t _z_task_detach(_z_task_t *task) { z_result_t _z_task_cancel(_z_task_t *task) { xEventGroupSetBits(task->join_event, 1); + + taskENTER_CRITICAL(); if (task->handle != NULL) { vTaskDelete(task->handle); task->handle = NULL; } + taskEXIT_CRITICAL(); + return _Z_RES_OK; } From 09dc516f1732f2aa50a86a2c30f5a996c92d9c29 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Tue, 10 Dec 2024 00:52:22 +0000 Subject: [PATCH 10/10] Fix race condition on task deletion --- src/system/freertos_plus_tcp/system.c | 27 +++++++++++++++++++++------ 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/src/system/freertos_plus_tcp/system.c b/src/system/freertos_plus_tcp/system.c index 52b80feca..632a9b9c5 100644 --- a/src/system/freertos_plus_tcp/system.c +++ b/src/system/freertos_plus_tcp/system.c @@ -61,14 +61,21 @@ void *z_realloc(void *ptr, size_t size) { void z_free(void *ptr) { vPortFree(ptr); } #if Z_FEATURE_MULTI_THREAD == 1 -// In FreeRTOS, tasks created using xTaskCreate must end with vTaskDelete. -// A task function should __not__ simply return. +/*------------------ Thread ------------------*/ static void z_task_wrapper(void *arg) { _z_task_t *task = (_z_task_t *)arg; + + // Run the task function task->fun(task->arg); + + // Notify the joiner that the task has finished xEventGroupSetBits(task->join_event, 1); - task->handle = NULL; - vTaskDelete(NULL); + + // In FreeRTOS, when a task deletes itself, it adds itself to a list of tasks awaiting to be terminated by the idle + // task. There is no guarantee when exactly that will happen, which could lead to race conditions on freeing the + // task resources. To avoid this, we suspend the task indefinitely and delete this task from another task running + // z_task_join or z_task_detach. + vTaskSuspend(NULL); } static z_task_attr_t z_default_task_attr = { @@ -118,6 +125,14 @@ z_result_t _z_task_init(_z_task_t *task, z_task_attr_t *attr, void *(*fun)(void z_result_t _z_task_join(_z_task_t *task) { xEventGroupWaitBits(task->join_event, 1, pdFALSE, pdFALSE, portMAX_DELAY); + + taskENTER_CRITICAL(); + if (task->handle != NULL) { + vTaskDelete(task->handle); + task->handle = NULL; + } + taskEXIT_CRITICAL(); + return _Z_RES_OK; } @@ -127,8 +142,6 @@ z_result_t _z_task_detach(_z_task_t *task) { } z_result_t _z_task_cancel(_z_task_t *task) { - xEventGroupSetBits(task->join_event, 1); - taskENTER_CRITICAL(); if (task->handle != NULL) { vTaskDelete(task->handle); @@ -136,6 +149,8 @@ z_result_t _z_task_cancel(_z_task_t *task) { } taskEXIT_CRITICAL(); + xEventGroupSetBits(task->join_event, 1); + return _Z_RES_OK; }