Skip to content

Commit

Permalink
Serial timeout (#383)
Browse files Browse the repository at this point in the history 
* correct unsigned atomic in refcount.h

* made espidf freertos compatible with timeout on read

---------

Co-authored-by: Luca Cominardi <[email protected]>
  • Loading branch information
@vortex314 @Mallets
vortex314 and Mallets authored Mar 27, 2024
1 parent 22bf428 commit 8e01007
Show file tree
Hide file tree
Showing 3 changed files with 123 additions and 65 deletions.
23 changes: 20 additions & 3 deletions include/zenoh-pico/system/platform/espidf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
Expand Down
89 changes: 50 additions & 39 deletions src/system/espidf/network.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
}

Expand Down
76 changes: 53 additions & 23 deletions src/system/espidf/system.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 ------------------*/
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 8e01007

Please sign in to comment.