FreeRTOS-Plus-TCP port fixes

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 <time.h>
+
 #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 {

src/system/freertos_plus_tcp/system.c

@@ -17,6 +17,7 @@
 #include <stddef.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <sys/time.h>
 
 #include "FreeRTOS_IP.h"
 #include "zenoh-pico/config.h"
@@ -51,26 +52,30 @@ 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;
 }
 
 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,75 +91,101 @@ 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;
     }
 
 #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;
+}