diff --git a/include/zenoh-pico/system/platform/espidf.h b/include/zenoh-pico/system/platform/espidf.h index 671617989..b7353b561 100644 --- a/include/zenoh-pico/system/platform/espidf.h +++ b/include/zenoh-pico/system/platform/espidf.h @@ -17,6 +17,7 @@ #include #include +#include #include #include "zenoh-pico/config.h" @@ -24,8 +25,20 @@ #if Z_FEATURE_MULTI_THREAD == 1 #include -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; diff --git a/src/system/espidf/network.c b/src/system/espidf/network.c index 10bd34aae..f0515660c 100644 --- a/src/system/espidf/network.c +++ b/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; } diff --git a/src/system/espidf/system.c b/src/system/espidf/system.c index 55eccb52f..61f568420 100644 --- a/src/system/espidf/system.c +++ b/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;