Rework arduino/esp32 serail (not tested)

Author: sashacmc

include/zenoh-pico/system/platform/arduino/esp32.h

@@ -47,7 +47,11 @@ typedef struct {
         BluetoothSerial *_bts;  // As pointer to cross the boundary between C and C++
 #endif
 #if Z_FEATURE_LINK_SERIAL == 1
-        HardwareSerial *_serial;  // As pointer to cross the boundary between C and C++
+        struct {
+            HardwareSerial *_serial;  // As pointer to cross the boundary between C and C++
+            uint8_t *tmp_buf;
+            uint8_t *raw_buf;
+        };
 #endif
     };
 } _z_sys_net_socket_t;

src/system/arduino/esp32/network.cpp

@@ -24,6 +24,8 @@ extern "C" {
 #include "zenoh-pico/collections/slice.h"
 #include "zenoh-pico/collections/string.h"
 #include "zenoh-pico/config.h"
+#include "zenoh-pico/protocol/codec/serial.h"
+#include "zenoh-pico/system/common/serial.h"
 #include "zenoh-pico/system/link/bt.h"
 #include "zenoh-pico/system/link/serial.h"
 #include "zenoh-pico/system/platform.h"
@@ -687,8 +689,6 @@ z_result_t _z_open_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, u
 }
 
 z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t baudrate) {
-    z_result_t ret = _Z_RES_OK;
-
     uint8_t uart = 255;
     uint32_t rxpin = 0;
     uint32_t txpin = 0;
@@ -705,28 +705,26 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_
         rxpin = 16;
         txpin = 17;
     } else {
-        ret = _Z_ERR_GENERIC;
+        return _Z_ERR_GENERIC;
     }
 
-    if (ret == _Z_RES_OK) {
-        // WARNING: Glitch on the very 1st byte is common when the serial input line is allowed to float.
-        //          To minimize this issue the RxPin is set to INPUT_PULLUP, and set the TxPin is driven to HIGH. This
-        //          will keep the pins high and upon initialization of the serial port using serial.begin() the line
-        //          does not float (drops to low).
-        pinMode(rxpin, INPUT_PULLUP);
-        pinMode(txpin, OUTPUT);
-        digitalWrite(txpin, HIGH);
-
-        sock->_serial = new HardwareSerial(uart);
-        if (sock->_serial != NULL) {
-            sock->_serial->begin(baudrate);
-            sock->_serial->flush();
-        } else {
-            ret = _Z_ERR_GENERIC;
-        }
+    // WARNING: Glitch on the very 1st byte is common when the serial input line is allowed to float.
+    //          To minimize this issue the RxPin is set to INPUT_PULLUP, and set the TxPin is driven to HIGH. This
+    //          will keep the pins high and upon initialization of the serial port using serial.begin() the line
+    //          does not float (drops to low).
+    pinMode(rxpin, INPUT_PULLUP);
+    pinMode(txpin, OUTPUT);
+    digitalWrite(txpin, HIGH);
+
+    sock->_serial = new HardwareSerial(uart);
+    if (sock->_serial != NULL) {
+        sock->_serial->begin(baudrate);
+        sock->_serial->flush();
+    } else {
+        return _Z_ERR_GENERIC;
     }
 
-    return ret;
+    return _z_connect_serial(*sock);
 }
 
 z_result_t _z_listen_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, uint32_t rxpin, uint32_t baudrate) {
@@ -757,112 +755,40 @@ z_result_t _z_listen_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint3
 void _z_close_serial(_z_sys_net_socket_t *sock) {
     sock->_serial->end();
     delete sock->_serial;
+    z_free(sock->tmp_buf);
+    z_free(sock->raw_buf);
 }
 
-size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) {
-    z_result_t ret = _Z_RES_OK;
-
-    uint8_t *before_cobs = new uint8_t[_Z_SERIAL_MAX_COBS_BUF_SIZE]();
+size_t _z_read_serial_internal(const _z_sys_net_socket_t sock, uint8_t *header, uint8_t *ptr, size_t len) {
     size_t rb = 0;
     for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) {
         while (sock._serial->available() < 1) {
             z_sleep_ms(1);  // FIXME: Yield by sleeping.
         }
-        before_cobs[i] = sock._serial->read();
+        sock.raw_buf[i] = sock._serial->read();
         rb = rb + (size_t)1;
-        if (before_cobs[i] == (uint8_t)0x00) {
+        if (sock.raw_buf[i] == (uint8_t)0x00) {
             break;
         }
     }
 
-    uint8_t *after_cobs = new uint8_t[_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;
-        }
-
-        uint32_t c_crc = _z_crc32(ptr, payload_len);
-        if (c_crc != crc) {
-            ret = _Z_ERR_GENERIC;
-        }
-    } else {
-        ret = _Z_ERR_GENERIC;
-    }
-
-    delete before_cobs;
-    delete after_cobs;
-
-    rb = payload_len;
-    if (ret != _Z_RES_OK) {
-        rb = SIZE_MAX;
-    }
-
-    return rb;
+    return _z_serial_msg_deserialize(sock.raw_buf, rb, ptr, len, header, sock.tmp_buf, _Z_SERIAL_MFS_SIZE);
 }
 
-size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) {
-    size_t n = len;
-    size_t rb = 0;
-
-    do {
-        rb = _z_read_serial(sock, ptr, n);
-        if (rb == SIZE_MAX) return rb;
-
-        n -= rb;
-        ptr = ptr + (len - n);
-    } while (n > 0);
+size_t _z_send_serial_internal(const _z_sys_net_socket_t sock, uint8_t header, const uint8_t *ptr, size_t len) {
+    size_t ret = _z_serial_msg_serialize(sock.raw_buf, _Z_SERIAL_MAX_COBS_BUF_SIZE, ptr, len, header, sock.tmp_buf,
+                                         _Z_SERIAL_MFS_SIZE);
 
-    return len;
-}
-
-size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len) {
-    z_result_t ret = _Z_RES_OK;
-
-    uint8_t *before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE);
-    size_t i = 0;
-    for (i = 0; i < sizeof(uint16_t); ++i) {
-        before_cobs[i] = (len >> (i * (size_t)8)) & (size_t)0XFF;
-    }
-
-    (void)memcpy(&before_cobs[i], ptr, len);
-    i = i + len;
-
-    uint32_t crc = _z_crc32(ptr, len);
-    for (uint8_t j = 0; j < sizeof(crc); j++) {
-        before_cobs[i] = (crc >> (j * (uint8_t)8)) & (uint32_t)0XFF;
-        i = i + (size_t)1;
+    if (ret == SIZE_MAX) {
+        return ret;
     }
 
-    uint8_t *after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE);
-    ssize_t twb = _z_cobs_encode(before_cobs, i, after_cobs);
-    after_cobs[twb] = 0x00;  // Manually add the COBS delimiter
-    ssize_t wb = sock._serial->write(after_cobs, twb + (ssize_t)1);
-    if (wb != (twb + (ssize_t)1)) {
-        ret = _Z_ERR_GENERIC;
+    ssize_t wb = sock._serial->write(sock.raw_buf, ret);
+    if (wb != (ssize_t)ret) {
+        ret = SIZE_MAX;
     }
 
-    z_free(before_cobs);
-    z_free(after_cobs);
-
-    size_t sb = len;
-    if (ret != _Z_RES_OK) {
-        sb = SIZE_MAX;
-    }
-
-    return sb;
+    return len;
 }
 #endif