merge

eclipse-zenoh · Jun 24, 2024 · f04f702 · f04f702
1 parent 38cbee0
commit f04f702
Show file tree

Hide file tree

Showing 13 changed files with 289 additions and 46 deletions.
diff --git a/include/zenoh-pico/transport/common/rx.h b/include/zenoh-pico/transport/common/rx.h
@@ -19,6 +19,7 @@
 #include "zenoh-pico/transport/transport.h"
 
 /*------------------ Transmission and Reception helpers ------------------*/
+size_t _z_read_stream_size(_z_zbuf_t *zbuf);
 int8_t _z_link_recv_t_msg(_z_transport_message_t *t_msg, const _z_link_t *zl);
 
 #endif /* ZENOH_PICO_TRANSPORT_RX_H */
diff --git a/include/zenoh-pico/utils/endianness.h b/include/zenoh-pico/utils/endianness.h
@@ -0,0 +1,51 @@
+//
+// Copyright (c) 2022 ZettaScale Technology
+//
+// This program and the accompanying materials are made available under the
+// terms of the Eclipse Public License 2.0 which is available at
+// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0
+// which is available at https://www.apache.org/licenses/LICENSE-2.0.
+//
+// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0
+//
+// Contributors:
+//   ZettaScale Zenoh Team, <[email protected]>
+//
+
+#ifndef ZENOH_PICO_UTILS_ENDIANNESS_H
+#define ZENOH_PICO_UTILS_ENDIANNESS_H
+
+#include <stdint.h>
+#include <stdlib.h>
+
+// Load int from memory with specified endianness
+uint16_t _z_le_load16(const uint8_t *src);
+uint32_t _z_le_load32(const uint8_t *src);
+uint64_t _z_le_load64(const uint8_t *src);
+uint16_t _z_be_load16(const uint8_t *src);
+uint32_t _z_be_load32(const uint8_t *src);
+uint64_t _z_be_load64(const uint8_t *src);
+
+// Store int to memory with specified endianness
+void _z_le_store16(const uint16_t val, uint8_t *dest);
+void _z_le_store32(const uint32_t val, uint8_t *dest);
+void _z_le_store64(const uint64_t val, uint8_t *dest);
+void _z_be_store16(const uint16_t val, uint8_t *dest);
+void _z_be_store32(const uint32_t val, uint8_t *dest);
+void _z_be_store64(const uint64_t val, uint8_t *dest);
+
+// Load little-endian int from memory to host order
+uint16_t _z_host_le_load16(const uint8_t *src);
+uint32_t _z_host_le_load32(const uint8_t *src);
+uint64_t _z_host_le_load64(const uint8_t *src);
+
+// Store little-endian int to memory from host order
+void _z_host_le_store16(const uint16_t val, uint8_t *dst);
+void _z_host_le_store32(const uint32_t val, uint8_t *dst);
+void _z_host_le_store64(const uint64_t val, uint8_t *dst);
+
+// Return u16 individual bytes
+uint8_t _z_get_u16_lsb(uint_fast16_t val);
+uint8_t _z_get_u16_msb(uint_fast16_t val);
+
+#endif /* ZENOH_PICO_UTILS_ENDIANNESS_H */
diff --git a/src/api/api.c b/src/api/api.c
@@ -38,6 +38,7 @@
 #include "zenoh-pico/system/platform.h"
 #include "zenoh-pico/transport/multicast.h"
 #include "zenoh-pico/transport/unicast.h"
+#include "zenoh-pico/utils/endianness.h"
 #include "zenoh-pico/utils/logging.h"
 #include "zenoh-pico/utils/result.h"
 #include "zenoh-pico/utils/uuid.h"

diff --git a/src/collections/bytes.c b/src/collections/bytes.c
@@ -21,6 +21,7 @@
 #include "zenoh-pico/protocol/codec/core.h"
 #include "zenoh-pico/system/platform.h"
 #include "zenoh-pico/utils/result.h"
+#include "zenoh-pico/utils/endianness.h"
 
 /*-------- Bytes --------*/
 _Bool _z_bytes_check(const _z_bytes_t *bytes) { return !_z_bytes_is_empty(bytes); }
@@ -202,20 +203,34 @@ int8_t _z_bytes_to_uint8(const _z_bytes_t *bs, uint8_t *val) {
     return _z_bytes_to_buf(bs, val, sizeof(uint8_t)) == sizeof(uint8_t) ? _Z_RES_OK : _Z_ERR_DID_NOT_READ;
 }
 
-// FIXME: int16+ endianness, Issue #420
 int8_t _z_bytes_to_uint16(const _z_bytes_t *bs, uint16_t *val) {
     *val = 0;
-    return _z_bytes_to_buf(bs, (uint8_t *)val, sizeof(uint16_t)) == sizeof(uint16_t) ? _Z_RES_OK : _Z_ERR_DID_NOT_READ;
+    uint8_t buf[sizeof(uint16_t)];
+    if (_z_bytes_to_buf(bs, buf, sizeof(uint16_t)) != sizeof(uint16_t)) {
+        return _Z_ERR_DID_NOT_READ;
+    }
+    *val = _z_host_le_load16(buf);
+    return _Z_RES_OK;
 }
 
 int8_t _z_bytes_to_uint32(const _z_bytes_t *bs, uint32_t *val) {
     *val = 0;
-    return _z_bytes_to_buf(bs, (uint8_t *)val, sizeof(uint32_t)) == sizeof(uint32_t) ? _Z_RES_OK : _Z_ERR_DID_NOT_READ;
+    uint8_t buf[sizeof(uint32_t)];
+    if (_z_bytes_to_buf(bs, buf, sizeof(uint32_t)) != sizeof(uint32_t)) {
+        return _Z_ERR_DID_NOT_READ;
+    }
+    *val = _z_host_le_load32(buf);
+    return _Z_RES_OK;
 }
 
 int8_t _z_bytes_to_uint64(const _z_bytes_t *bs, uint64_t *val) {
     *val = 0;
-    return _z_bytes_to_buf(bs, (uint8_t *)val, sizeof(uint64_t)) == sizeof(uint64_t) ? _Z_RES_OK : _Z_ERR_DID_NOT_READ;
+    uint8_t buf[sizeof(uint64_t)];
+    if (_z_bytes_to_buf(bs, buf, sizeof(uint64_t)) != sizeof(uint64_t)) {
+        return _Z_ERR_DID_NOT_READ;
+    }
+    *val = _z_host_le_load64(buf);
+    return _Z_RES_OK;
 }
 
 int8_t _z_bytes_to_float(const _z_bytes_t *bs, float *val) {
@@ -228,18 +243,26 @@ int8_t _z_bytes_to_double(const _z_bytes_t *bs, double *val) {
     return _z_bytes_to_buf(bs, (uint8_t *)val, sizeof(double)) == sizeof(double) ? _Z_RES_OK : _Z_ERR_DID_NOT_READ;
 }
 
-int8_t _z_bytes_from_uint8(_z_bytes_t *b, uint8_t val) { return _z_bytes_from_buf(b, &val, sizeof(val)); }
+int8_t _z_bytes_from_uint8(_z_bytes_t *b, uint8_t val) {
+    return _z_bytes_from_buf(b, &val, sizeof(val)); 
+}
 
 int8_t _z_bytes_from_uint16(_z_bytes_t *b, uint16_t val) {
-    return _z_bytes_from_buf(b, (uint8_t *)&val, sizeof(val));
+    uint8_t buf[sizeof(uint16_t)];
+    _z_host_le_store16(val, buf);
+    return _z_bytes_from_buf(b, buf, sizeof(uint16_t));
 }
 
 int8_t _z_bytes_from_uint32(_z_bytes_t *b, uint32_t val) {
-    return _z_bytes_from_buf(b, (uint8_t *)&val, sizeof(val));
+    uint8_t buf[sizeof(uint32_t)];
+    _z_host_le_store32(val, buf);
+    return _z_bytes_from_buf(b, buf, sizeof(uint32_t));
 }
 
 int8_t _z_bytes_from_uint64(_z_bytes_t *b, uint64_t val) {
-    return _z_bytes_from_buf(b, (uint8_t *)&val, sizeof(val));
+    uint8_t buf[sizeof(uint64_t)];
+    _z_host_le_store64(val, buf);
+    return _z_bytes_from_buf(b, buf, sizeof(uint64_t));
 }
 
 int8_t _z_bytes_from_float(_z_bytes_t *b, float val) { return _z_bytes_from_buf(b, (uint8_t *)&val, sizeof(val)); }

diff --git a/src/collections/slice.c b/src/collections/slice.c
@@ -18,6 +18,7 @@
 #include <string.h>
 
 #include "zenoh-pico/system/platform.h"
+#include "zenoh-pico/utils/endianness.h"
 #include "zenoh-pico/utils/result.h"
 
 /*-------- Slice --------*/

diff --git a/src/protocol/codec.c b/src/protocol/codec.c
@@ -17,6 +17,7 @@
 #include <stdint.h>
 
 #include "zenoh-pico/protocol/core.h"
+#include "zenoh-pico/utils/endianness.h"
 #include "zenoh-pico/utils/logging.h"
 #include "zenoh-pico/utils/result.h"
 
@@ -76,44 +77,52 @@ int8_t _z_uint8_decode(uint8_t *u8, _z_zbuf_t *zbf) {
 int8_t _z_uint16_encode(_z_wbuf_t *wbf, uint16_t u16) {
     int8_t ret = _Z_RES_OK;
 
-    ret |= _z_wbuf_write(wbf, (u16 & 0xFFU));
-    ret |= _z_wbuf_write(wbf, ((u16 >> 8) & 0xFFU));
+    ret |= _z_wbuf_write(wbf, _z_get_u16_lsb(u16));
+    ret |= _z_wbuf_write(wbf, _z_get_u16_msb(u16));
 
     return ret;
 }
 
 int8_t _z_uint16_decode(uint16_t *u16, _z_zbuf_t *zbf) {
     int8_t ret = _Z_RES_OK;
-    *u16 = 0;
+    uint8_t enc_u16[2];
 
-    uint8_t u8;
-    ret |= _z_uint8_decode(&u8, zbf);
-    *u16 |= u8;
-    ret |= _z_uint8_decode(&u8, zbf);
-    *u16 |= u8 << 8;
+    ret |= _z_uint8_decode(&enc_u16[0], zbf);
+    ret |= _z_uint8_decode(&enc_u16[1], zbf);
+    *u16 = _z_host_le_load16(enc_u16);
 
     return ret;
 }
 
 /*------------------ z_zint ------------------*/
+// Zint is a variable int composed of up to 9 bytes.
+// The msb of the 8 first bytes has meaning: (1: the zint continue, 0: end of the zint)
 #define VLE_LEN 9
+#define VLE_LEN1_MASK (UINT64_MAX << (7 * 1))
+#define VLE_LEN2_MASK (UINT64_MAX << (7 * 2))
+#define VLE_LEN3_MASK (UINT64_MAX << (7 * 3))
+#define VLE_LEN4_MASK (UINT64_MAX << (7 * 4))
+#define VLE_LEN5_MASK (UINT64_MAX << (7 * 5))
+#define VLE_LEN6_MASK (UINT64_MAX << (7 * 6))
+#define VLE_LEN7_MASK (UINT64_MAX << (7 * 7))
+#define VLE_LEN8_MASK (UINT64_MAX << (7 * 8))
 
 uint8_t _z_zint_len(uint64_t v) {
-    if ((v & (UINT64_MAX << (7 * 1))) == 0) {
+    if ((v & VLE_LEN1_MASK) == 0) {
         return 1;
-    } else if ((v & (UINT64_MAX << (7 * 2))) == 0) {
+    } else if ((v & VLE_LEN2_MASK) == 0) {
         return 2;
-    } else if ((v & (UINT64_MAX << (7 * 3))) == 0) {
+    } else if ((v & VLE_LEN3_MASK) == 0) {
         return 3;
-    } else if ((v & (UINT64_MAX << (7 * 4))) == 0) {
+    } else if ((v & VLE_LEN4_MASK) == 0) {
         return 4;
-    } else if ((v & (UINT64_MAX << (7 * 5))) == 0) {
+    } else if ((v & VLE_LEN5_MASK) == 0) {
         return 5;
-    } else if ((v & (UINT64_MAX << (7 * 6))) == 0) {
+    } else if ((v & VLE_LEN6_MASK) == 0) {
         return 6;
-    } else if ((v & (UINT64_MAX << (7 * 7))) == 0) {
+    } else if ((v & VLE_LEN7_MASK) == 0) {
         return 7;
-    } else if ((v & (UINT64_MAX << (7 * 8))) == 0) {
+    } else if ((v & VLE_LEN8_MASK) == 0) {
         return 8;
     } else {
         return 9;
@@ -124,7 +133,7 @@ uint8_t _z_zint64_encode_buf(uint8_t *buf, uint64_t v) {
     uint64_t lv = v;
     uint8_t len = 0;
     size_t start = 0;
-    while ((lv & (uint64_t)~0x7f) != 0) {
+    while ((lv & VLE_LEN1_MASK) != 0) {
         uint8_t c = (lv & 0x7f) | 0x80;
         buf[start++] = c;
         len++;
@@ -145,7 +154,6 @@ int8_t _z_zint64_encode(_z_wbuf_t *wbf, uint64_t v) {
     for (size_t i = 0; i < len; i++) {
         _Z_RETURN_IF_ERR(_z_wbuf_write(wbf, buf[i]));
     }
-
     return _Z_RES_OK;
 }
 

diff --git a/src/transport/common/rx.c b/src/transport/common/rx.c
@@ -12,15 +12,26 @@
 //   ZettaScale Zenoh Team, <[email protected]>
 //
 
-#include "zenoh-pico/transport/multicast/rx.h"
+#include "zenoh-pico/transport/common/rx.h"
 
 #include <stddef.h>
 
 #include "zenoh-pico/protocol/codec/transport.h"
+#include "zenoh-pico/transport/multicast/rx.h"
 #include "zenoh-pico/transport/unicast/rx.h"
+#include "zenoh-pico/utils/endianness.h"
 #include "zenoh-pico/utils/logging.h"
 
 /*------------------ Reception helper ------------------*/
+size_t _z_read_stream_size(_z_zbuf_t *zbuf) {
+    uint8_t stream_size[_Z_MSG_LEN_ENC_SIZE];
+    // Read the bytes from stream
+    for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) {
+        stream_size[i] = _z_zbuf_read(zbuf);
+    }
+    return _z_host_le_load16(stream_size);
+}
+
 int8_t _z_link_recv_t_msg(_z_transport_message_t *t_msg, const _z_link_t *zl) {
     int8_t ret = _Z_RES_OK;
 

diff --git a/src/transport/common/tx.c b/src/transport/common/tx.c
@@ -21,6 +21,7 @@
 #include "zenoh-pico/transport/multicast/tx.h"
 #include "zenoh-pico/transport/raweth/tx.h"
 #include "zenoh-pico/transport/unicast/tx.h"
+#include "zenoh-pico/utils/endianness.h"
 #include "zenoh-pico/utils/logging.h"
 
 /*------------------ Transmission helper ------------------*/
@@ -57,9 +58,9 @@ void __unsafe_z_finalize_wbuf(_z_wbuf_t *buf, uint8_t link_flow_capability) {
         // Stream capable links
         case Z_LINK_CAP_FLOW_STREAM: {
             size_t len = _z_wbuf_len(buf) - _Z_MSG_LEN_ENC_SIZE;
-            for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) {
-                _z_wbuf_put(buf, (uint8_t)((len >> (uint8_t)8 * i) & (uint8_t)0xFF), i);
-            }
+            // Encode the u16 size as little endian
+            _z_wbuf_put(buf, _z_get_u16_lsb((uint_fast16_t)len), 0);
+            _z_wbuf_put(buf, _z_get_u16_msb((uint_fast16_t)len), 1);
             break;
         }
         // Datagram capable links

diff --git a/src/transport/multicast/read.c b/src/transport/multicast/read.c
@@ -19,6 +19,7 @@
 #include "zenoh-pico/config.h"
 #include "zenoh-pico/protocol/codec/transport.h"
 #include "zenoh-pico/protocol/iobuf.h"
+#include "zenoh-pico/transport/common/rx.h"
 #include "zenoh-pico/transport/multicast/rx.h"
 #include "zenoh-pico/transport/unicast/rx.h"
 #include "zenoh-pico/utils/logging.h"
@@ -71,11 +72,9 @@ void *_zp_multicast_read_task(void *ztm_arg) {
                         continue;
                     }
                 }
-
-                for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) {
-                    to_read |= _z_zbuf_read(&ztm->_zbuf) << (i * (uint8_t)8);
-                }
-
+                // Get stream size
+                to_read = _z_read_stream_size(&ztm->_zbuf);
+                // Read data
                 if (_z_zbuf_len(&ztm->_zbuf) < to_read) {
                     _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, NULL);
                     if (_z_zbuf_len(&ztm->_zbuf) < to_read) {

diff --git a/src/transport/multicast/rx.c b/src/transport/multicast/rx.c
@@ -24,6 +24,7 @@
 #include "zenoh-pico/protocol/definitions/transport.h"
 #include "zenoh-pico/protocol/iobuf.h"
 #include "zenoh-pico/session/utils.h"
+#include "zenoh-pico/transport/common/rx.h"
 #include "zenoh-pico/transport/utils.h"
 #include "zenoh-pico/utils/logging.h"
 
@@ -50,9 +51,9 @@ static int8_t _z_multicast_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_trans
                         break;
                     }
                 }
-                for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) {
-                    to_read |= _z_zbuf_read(&ztm->_zbuf) << (i * (uint8_t)8);
-                }
+                // Get stream size
+                to_read = _z_read_stream_size(&ztm->_zbuf);
+                // Read data
                 if (_z_zbuf_len(&ztm->_zbuf) < to_read) {
                     _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr);
                     if (_z_zbuf_len(&ztm->_zbuf) < to_read) {

diff --git a/src/transport/unicast/read.c b/src/transport/unicast/read.c
@@ -18,6 +18,7 @@
 
 #include "zenoh-pico/config.h"
 #include "zenoh-pico/protocol/codec/transport.h"
+#include "zenoh-pico/transport/common/rx.h"
 #include "zenoh-pico/transport/unicast/rx.h"
 #include "zenoh-pico/utils/logging.h"
 
@@ -66,11 +67,9 @@ void *_zp_unicast_read_task(void *ztu_arg) {
                         continue;
                     }
                 }
-
-                for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) {
-                    to_read |= _z_zbuf_read(&ztu->_zbuf) << (i * (uint8_t)8);
-                }
-
+                // Get stream size
+                to_read = _z_read_stream_size(&ztu->_zbuf);
+                // Read data
                 if (_z_zbuf_len(&ztu->_zbuf) < to_read) {
                     _z_link_recv_zbuf(&ztu->_link, &ztu->_zbuf, NULL);
                     if (_z_zbuf_len(&ztu->_zbuf) < to_read) {

diff --git a/src/transport/unicast/rx.c b/src/transport/unicast/rx.c
@@ -22,6 +22,7 @@
 #include "zenoh-pico/protocol/core.h"
 #include "zenoh-pico/protocol/iobuf.h"
 #include "zenoh-pico/session/utils.h"
+#include "zenoh-pico/transport/common/rx.h"
 #include "zenoh-pico/transport/utils.h"
 #include "zenoh-pico/utils/logging.h"
 
@@ -48,9 +49,9 @@ int8_t _z_unicast_recv_t_msg_na(_z_transport_unicast_t *ztu, _z_transport_messag
                         continue;
                     }
                 }
-                for (uint8_t i = 0; i < _Z_MSG_LEN_ENC_SIZE; i++) {
-                    to_read |= _z_zbuf_read(&ztu->_zbuf) << (i * (uint8_t)8);
-                }
+                // Get stream size
+                to_read = _z_read_stream_size(&ztu->_zbuf);
+                // Read data
                 if (_z_zbuf_len(&ztu->_zbuf) < to_read) {
                     _z_link_recv_zbuf(&ztu->_link, &ztu->_zbuf, NULL);
                     if (_z_zbuf_len(&ztu->_zbuf) < to_read) {