diff --git a/include/zenoh-pico/system/platform/freertos_plus_tcp.h b/include/zenoh-pico/system/platform/freertos_plus_tcp.h index 97d6c1b10..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" @@ -37,9 +39,19 @@ 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 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; @@ -52,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 395e6bc59..632a9b9c5 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" @@ -51,6 +52,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; } @@ -58,19 +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. -typedef struct { - void *(*fun)(void *); - void *arg; - EventGroupHandle_t join_event; -} z_task_arg; - +/*------------------ Thread ------------------*/ static void z_task_wrapper(void *arg) { - z_task_arg *targ = (z_task_arg *)arg; - targ->fun(targ->arg); - xEventGroupSetBits(targ->join_event, 1); - vTaskDelete(NULL); + _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); + + // 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 = { @@ -86,14 +91,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; @@ -101,60 +106,86 @@ 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) { - return -1; + if (xTaskCreate(z_task_wrapper, attr->name, attr->stack_depth, task, attr->priority, &task->handle) != pdPASS) { + return _Z_ERR_GENERIC; } #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) { xEventGroupWaitBits(task->join_event, 1, pdFALSE, pdFALSE, portMAX_DELAY); - return 0; + + taskENTER_CRITICAL(); + if (task->handle != NULL) { + vTaskDelete(task->handle); + task->handle = NULL; + } + taskEXIT_CRITICAL(); + + return _Z_RES_OK; } 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); - return 0; + taskENTER_CRITICAL(); + if (task->handle != NULL) { + vTaskDelete(task->handle); + task->handle = NULL; + } + taskEXIT_CRITICAL(); + + xEventGroupSetBits(task->join_event, 1); + + return _Z_RES_OK; } 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 ------------------*/ 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) { @@ -238,45 +269,76 @@ 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 ------------------*/ -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 _Z_RES_OK; +}