From 8b9a9fb11f8c06c52b933f3df877341a881c00f5 Mon Sep 17 00:00:00 2001
From: Alexander Bushnev <Alexander@Bushnev.pro>
Date: Tue, 10 Dec 2024 11:00:44 +0100
Subject: [PATCH] Rework mbed serial (not tested)

---
 include/zenoh-pico/system/platform/mbed.h |   6 +-
 src/system/mbed/network.cpp               | 113 ++++------------------
 2 files changed, 26 insertions(+), 93 deletions(-)

diff --git a/include/zenoh-pico/system/platform/mbed.h b/include/zenoh-pico/system/platform/mbed.h
index 9d9d91aeb..b2911acd2 100644
--- a/include/zenoh-pico/system/platform/mbed.h
+++ b/include/zenoh-pico/system/platform/mbed.h
@@ -51,7 +51,11 @@ typedef struct {
         UDPSocket *_udp;  // As pointer to cross the boundary between C and C++
 #endif
 #if Z_FEATURE_LINK_SERIAL == 1
-        BufferedSerial *_serial;  // As pointer to cross the boundary between C and C++
+        struct {
+            BufferedSerial *_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;
diff --git a/src/system/mbed/network.cpp b/src/system/mbed/network.cpp
index a9d365782..5a6c6c6df 100644
--- a/src/system/mbed/network.cpp
+++ b/src/system/mbed/network.cpp
@@ -332,7 +332,10 @@ z_result_t _z_open_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, u
         ret = _Z_ERR_GENERIC;
     }
 
-    return ret;
+    sock->tmp_buf = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE);
+    sock->raw_buf = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE);
+
+    return _z_connect_serial(*sock);
 }
 
 z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *device, uint32_t baudrate) {
@@ -370,113 +373,39 @@ z_result_t _z_listen_serial_from_dev(_z_sys_net_socket_t *sock, char *device, ui
     return ret;
 }
 
-void _z_close_serial(_z_sys_net_socket_t *sock) { delete sock->_serial; }
+void _z_close_serial(_z_sys_net_socket_t *sock) {
+    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 rb = 0;
     for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) {
-        sock._serial->read(&before_cobs[i], 1);
-        rb = rb + (size_t)1;
+        sock._serial->read(&sock.raw_buf[i], 1);
+        rb++;
         if (before_cobs[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 = 0;
-    uint8_t *pos = &ptr[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);
 
-    do {
-        size_t rb = _z_read_serial(sock, ptr, len - n);
-        if (rb == SIZE_MAX) {
-            n = rb;
-            break;
-        }
-
-        n = n + rb;
-        pos = _z_ptr_u8_offset(pos, n);
-    } while (n != len);
-
-    return n;
-}
-
-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 = new uint8_t[_Z_SERIAL_MFS_SIZE]();
-    size_t i = 0;
-    for (; 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;
-    }
-
-    uint8_t *after_cobs = new uint8_t[_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;
+    if (ret == SIZE_MAX) {
+        return ret;
     }
 
-    delete before_cobs;
-    delete after_cobs;
-
-    size_t sb = len;
-    if (ret != _Z_RES_OK) {
-        sb = SIZE_MAX;
+    ssize_t wb = sock._serial->write(after_cobs, sock.raw_buf, ret);
+    if (wb != (ssize_t)ret) {
+        ret = SIZE_MAX;
     }
 
-    return sb;
+    return len;
 }
 #endif