Serial timeout

include/zenoh-pico/system/platform/espidf.h

@@ -17,15 +17,28 @@
 
 #include <driver/uart.h>
 #include <freertos/FreeRTOS.h>
+#include <freertos/event_groups.h>
 #include <freertos/task.h>
 
 #include "zenoh-pico/config.h"
 
 #if Z_FEATURE_MULTI_THREAD == 1
 #include <pthread.h>
 
-typedef TaskHandle_t z_task_t;
-typedef void *z_task_attr_t;  // Not used in ESP32
+typedef struct {
+    const char *name;
+    UBaseType_t priority;
+    size_t stack_depth;
+#if (configSUPPORT_STATIC_ALLOCATION == 1)
+    _Bool static_allocation;
+    StackType_t *stack_buffer;
+    StaticTask_t *task_buffer;
+#endif /* SUPPORT_STATIC_ALLOCATION */
+} z_task_attr_t;
+typedef struct {
+    TaskHandle_t handle;
+    EventGroupHandle_t join_event;
+} z_task_t;
 typedef pthread_mutex_t z_mutex_t;
 typedef pthread_cond_t z_condvar_t;
 #endif  // Z_FEATURE_MULTI_THREAD == 1
@@ -39,7 +52,11 @@ typedef struct {
         int _fd;
 #endif
 #if Z_FEATURE_LINK_SERIAL == 1
-        uart_port_t _serial;
+        struct {
+            uart_port_t _serial;
+            uint8_t *before_cobs;
+            uint8_t *after_cobs;
+        };
 #endif
     };
 } _z_sys_net_socket_t;

src/system/espidf/network.c

@@ -593,7 +593,9 @@ int8_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t ba
     const int uart_buffer_size = (1024 * 2);
     QueueHandle_t uart_queue;
     uart_driver_install(sock->_serial, uart_buffer_size, 0, 100, &uart_queue, 0);
-
+    uart_flush_input(sock->_serial);
+    sock->after_cobs = (uint8_t *)zp_malloc(_Z_SERIAL_MFS_SIZE);
+    sock->before_cobs = (uint8_t *)zp_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE);
     return ret;
 }
 
@@ -622,65 +624,74 @@ int8_t _z_listen_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t
     return ret;
 }
 
-void _z_close_serial(_z_sys_net_socket_t *sock) { uart_driver_delete(sock->_serial); }
+void _z_close_serial(_z_sys_net_socket_t *sock) {  
+    uart_wait_tx_done(sock->_serial, 1000);
+    uart_flush(sock->_serial);
+    uart_driver_delete(sock->_serial);
+    zp_free(sock->after_cobs);
+    zp_free(sock->before_cobs);
+    }
 
 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 *)z_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;
-        do {
-            uart_get_buffered_data_len(sock._serial, &len);
-            if (len < 1) {
-                z_sleep_ms(10);  // FIXME: Yield by sleeping.
-            } else {
-                break;
+    while (rb < _Z_SERIAL_MAX_COBS_BUF_SIZE) {
+        int r = uart_read_bytes(sock._serial, &sock.before_cobs[rb], 1, 1000);
+        if (r == 0) {
+            _Z_DEBUG("Timeout reading from serial");
+            if (rb == 0) {
+                ret = _Z_ERR_GENERIC;
             }
-        } while (1);
-        uart_read_bytes(sock._serial, &before_cobs[i], 1, 100);
-        rb = rb + (size_t)1;
-        if (before_cobs[i] == (uint8_t)0x00) {
             break;
+        } else if (r == 1) {
+            rb = rb + (size_t)1;
+            if (sock.before_cobs[rb - 1] == (uint8_t)0x00) {
+                break;
+            }
+        } else {
+            _Z_ERROR("Error reading from serial");
+            ret = _Z_ERR_GENERIC;
         }
     }
-
-    uint8_t *after_cobs = (uint8_t *)z_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 = 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 = i + (size_t)1;
+    if (ret == _Z_RES_OK) {
+        _Z_DEBUG("Read %u bytes from serial", rb);
+        size_t trb = _z_cobs_decode(sock.before_cobs, rb, sock.after_cobs);
+        _Z_DEBUG("Decoded %u bytes from serial", trb);
+        size_t i = 0;
+        for (i = 0; i < sizeof(payload_len); i++) {
+            payload_len |= (sock.after_cobs[i] << ((uint8_t)i * (uint8_t)8));
         }
+        _Z_DEBUG("payload_len = %u <= %X %X", payload_len, sock.after_cobs[1], sock.after_cobs[0]);
+
+        if (trb == (size_t)(payload_len + (uint16_t)6)) {
+            (void)memcpy(ptr, &sock.after_cobs[i], payload_len);
+            i = i + (size_t)payload_len;
+
+            uint32_t crc = 0;
+            for (uint8_t j = 0; j < sizeof(crc); j++) {
+                crc |= (uint32_t)(sock.after_cobs[i] << (j * (uint8_t)8));
+                i = i + (size_t)1;
+            }
 
-        uint32_t c_crc = _z_crc32(ptr, payload_len);
-        if (c_crc != crc) {
+            uint32_t c_crc = _z_crc32(ptr, payload_len);
+            if (c_crc != crc) {
+                _Z_ERROR("CRC mismatch: %d != %d ", c_crc, crc);
+                ret = _Z_ERR_GENERIC;
+            }
+        } else {
+            _Z_ERROR("length mismatch => %d <> %d ", trb, payload_len + (uint16_t)6);
             ret = _Z_ERR_GENERIC;
         }
-    } else {
-        ret = _Z_ERR_GENERIC;
     }
 
-    z_free(before_cobs);
-    z_free(after_cobs);
-
     rb = payload_len;
     if (ret != _Z_RES_OK) {
         rb = SIZE_MAX;
     }
-
+    _Z_DEBUG("return _z_read_serial() = %d ", rb);
     return rb;
 }
 

src/system/espidf/system.c

@@ -50,48 +50,77 @@ void z_free(void *ptr) { heap_caps_free(ptr); }
 // In FreeRTOS, tasks created using xTaskCreate must end with vTaskDelete.
 // A task function should __not__ simply return.
 typedef struct {
-    void *(*_fun)(void *);
-    void *_arg;
+    void *(*fun)(void *);
+    void *arg;
+    EventGroupHandle_t join_event;
 } z_task_arg;
 
-void z_task_wrapper(z_task_arg *targ) {
-    targ->_fun(targ->_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);
     vTaskDelete(NULL);
-    z_free(targ);
 }
 
-/*------------------ Task ------------------*/
-int8_t z_task_init(z_task_t *task, z_task_attr_t *attr, void *(*fun)(void *), void *arg) {
-    int ret = 0;
+static z_task_attr_t z_default_task_attr = {
+    .name = "",
+    .priority = configMAX_PRIORITIES / 2,
+    .stack_depth = 5120,
+#if (configSUPPORT_STATIC_ALLOCATION == 1)
+    .static_allocation = false,
+    .stack_buffer = NULL,
+    .task_buffer = NULL,
+#endif /* SUPPORT_STATIC_ALLOCATION */
+};
 
+/*------------------ Thread ------------------*/
+int8_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) {
-        z_arg->_fun = fun;
-        z_arg->_arg = arg;
-        if (xTaskCreate((void *)z_task_wrapper, "", 5120, z_arg, configMAX_PRIORITIES / 2, task) != pdPASS) {
-            ret = -1;
+    if (z_arg == NULL) {
+        return -1;
+    }
+
+    z_arg->fun = fun;
+    z_arg->arg = arg;
+    z_arg->join_event = task->join_event = xEventGroupCreate();
+
+    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,
+                                         attr->stack_buffer, attr->task_buffer);
+        if (task->handle == NULL) {
+            return -1;
         }
     } else {
-        ret = -1;
+#endif /* SUPPORT_STATIC_ALLOCATION */
+        if (xTaskCreate(z_task_wrapper, attr->name, attr->stack_depth, z_arg, attr->priority, &task->handle) !=
+            pdPASS) {
+            return -1;
+        }
+#if (configSUPPORT_STATIC_ALLOCATION == 1)
     }
+#endif /* SUPPORT_STATIC_ALLOCATION */
 
-    return ret;
+    return 0;
 }
 
 int8_t z_task_join(z_task_t *task) {
-    // Note: task/thread join not supported on FreeRTOS API, so we force its deletion instead.
-    return zp_task_cancel(task);
+    xEventGroupWaitBits(task->join_event, 1, pdFALSE, pdFALSE, portMAX_DELAY);
+    return 0;
 }
 
-int8_t zp_task_cancel(z_task_t *task) {
-    vTaskDelete(*task);
+int8_t z_task_cancel(z_task_t *task) {
+    vTaskDelete(task->handle);
     return 0;
 }
 
 void z_task_free(z_task_t **task) {
-    z_task_t *ptr = *task;
-    z_free(ptr);
-    *task = NULL;
+    z_free((*task)->join_event);
+    z_free(*task);
 }
 
 /*------------------ Mutex ------------------*/
@@ -125,7 +154,8 @@ int z_sleep_ms(size_t time) {
     // 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 (z_time_elapsed_ms(&start) < time) {
-        z_sleep_us(1000);
+        //z_sleep_us(1000);
+        vTaskDelay(1/portTICK_PERIOD_MS);
     }
 
     return 0;