avoid dynamic allocation at each message received

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

@@ -17,8 +17,8 @@
 
 #include <driver/uart.h>
 #include <freertos/FreeRTOS.h>
-#include <freertos/task.h>
 #include <freertos/event_groups.h>
+#include <freertos/task.h>
 
 #include "zenoh-pico/config.h"
 
@@ -52,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/session/subscription.c

@@ -184,13 +184,13 @@ int8_t _z_trigger_subscriptions(_z_session_t *zn, const _z_keyexpr_t keyexpr, co
 #if Z_FEATURE_MULTI_THREAD == 1
     zp_mutex_lock(&zn->_mutex_inner);
 #endif  // Z_FEATURE_MULTI_THREAD == 1
-#if Z_FEATURE_SUBSCRIPTION==1
+#if Z_FEATURE_SUBSCRIPTION == 1
 
-    _Z_DEBUG(" %x - %s", keyexpr._id, keyexpr._suffix);
-    _Z_DEBUG("Resolving %d - %s on mapping 0x%x", keyexpr._id, keyexpr._suffix, _z_keyexpr_mapping_id(&keyexpr));
+//    _Z_DEBUG(" %x - %s", keyexpr._id, keyexpr._suffix);
+//    _Z_DEBUG("Resolving %d - %s on mapping 0x%x", keyexpr._id, keyexpr._suffix, _z_keyexpr_mapping_id(&keyexpr));
 #endif
     _z_keyexpr_t key = __unsafe_z_get_expanded_key_from_key(zn, &keyexpr);
-   _Z_DEBUG("Triggering subs for %d - %s", key._id, key._suffix);
+    _Z_DEBUG("Triggering subs for %d - %s", key._id, key._suffix);
     if (key._suffix != NULL) {
         _z_subscription_rc_list_t *subs = __unsafe_z_get_subscriptions_by_key(zn, _Z_RESOURCE_IS_LOCAL, key);
 

src/system/espidf/network.c

@@ -591,10 +591,10 @@ int8_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t ba
     uart_set_pin(sock->_serial, txpin, rxpin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
 
     const int uart_buffer_size = (1024 * 2);
-    QueueHandle_t uart_queue;
     uart_driver_install(sock->_serial, uart_buffer_size, 0, 100, NULL, 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;
 }
 
@@ -624,69 +624,69 @@ int8_t _z_listen_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t
 }
 
 void _z_close_serial(_z_sys_net_socket_t *sock) {
+    printf("Closing serial %d\n", sock->_serial);
     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 *)zp_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE);
     size_t rb = 0;
-    for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) {
-        int r = uart_read_bytes(sock._serial, &before_cobs[i], 1, 1000);
+    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");
-                zp_free(before_cobs);
-                return 0;
+            _Z_DEBUG("Timeout reading from serial");
+            if ( rb == 0 ) {
+                ret = _Z_ERR_GENERIC;
+            }
+            break;
         } else if (r == 1) {
             rb = rb + (size_t)1;
-            if (before_cobs[i] == (uint8_t)0x00) {
+            if (sock.before_cobs[rb-1] == (uint8_t)0x00) {
                 break;
             }
         } else {
             _Z_ERROR("Error reading from serial");
-            zp_free(before_cobs);
-            return _Z_ERR_GENERIC;
+            ret = _Z_ERR_GENERIC;
         }
     }
-    _Z_DEBUG("Read %u bytes from serial", rb);
-    uint8_t *after_cobs = (uint8_t *)zp_malloc(_Z_SERIAL_MFS_SIZE);
-    size_t trb = _z_cobs_decode(before_cobs, rb, after_cobs);
-    _Z_DEBUG("before decode %X %X %X %X %X ", before_cobs[0], before_cobs[1], before_cobs[2], before_cobs[3],
-             before_cobs[4]);
-    _Z_DEBUG("after decode %X %X %X %X %X ", after_cobs[0], after_cobs[1], after_cobs[2], after_cobs[3], after_cobs[4]);
-    _Z_DEBUG("Decoded %u bytes from serial", trb);
-    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));
-    }
-    _Z_DEBUG("payload_len = %u <= %X %X", payload_len, after_cobs[1], after_cobs[0]);
 
-    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 c_crc = _z_crc32(ptr, payload_len);
-        if (c_crc != crc) {
-            _Z_ERROR("CRC mismatch: %d != %d ", c_crc, crc);
+            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) {
+                _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 {
-        _Z_ERROR("length mismatch => %d <> %d ", trb, payload_len + (uint16_t)6);
-        ret = _Z_ERR_GENERIC;
     }
 
-    zp_free(before_cobs);
-    zp_free(after_cobs);
     rb = payload_len;
     if (ret != _Z_RES_OK) {
         rb = SIZE_MAX;