Enforce little endian encoding

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 */

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, <zenoh@zettascale.tech>
+//
+
+#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 */

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"
@@ -351,9 +352,7 @@ int8_t zp_bytes_deserialize_into_pair(const z_loaned_bytes_t *bytes, z_owned_byt
         return _Z_ERR_SYSTEM_OUT_OF_MEMORY;
     }
     // Extract first item size
-    size_t first_len = 0;
-    // FIXME: size endianness, Issue #420
-    memcpy(&first_len, &bytes->_slice.start[*curr_idx], sizeof(uint32_t));
+    size_t first_len = _z_host_le_load32(&bytes->_slice.start[*curr_idx]);
     *curr_idx += sizeof(uint32_t);
     // Allocate first item bytes
     *first->_val = _z_bytes_make(first_len);
@@ -365,8 +364,7 @@ int8_t zp_bytes_deserialize_into_pair(const z_loaned_bytes_t *bytes, z_owned_byt
     *curr_idx += first_len;
 
     // Extract second item size
-    size_t second_len = 0;
-    memcpy(&second_len, &bytes->_slice.start[*curr_idx], sizeof(uint32_t));
+    size_t second_len = _z_host_le_load32(&bytes->_slice.start[*curr_idx]);
     *curr_idx += sizeof(uint32_t);
     // Allocate second item bytes
     *second->_val = _z_bytes_make(second_len);
@@ -571,12 +569,11 @@ int8_t zp_bytes_serialize_from_pair(z_owned_bytes_t *bytes, z_owned_bytes_t *fir
     size_t first_len = z_slice_len(&first->_val->_slice);
     size_t second_len = z_slice_len(&second->_val->_slice);
     // Copy data
-    // FIXME: size endianness, Issue #420
-    memcpy((uint8_t *)&bytes->_val->_slice.start[*curr_idx], &first_len, sizeof(uint32_t));
+    _z_host_le_store32((uint32_t)first_len, (uint8_t *)&bytes->_val->_slice.start[*curr_idx]);
     *curr_idx += sizeof(uint32_t);
     memcpy((uint8_t *)&bytes->_val->_slice.start[*curr_idx], z_slice_data(&first->_val->_slice), first_len);
     *curr_idx += first_len;
-    memcpy((uint8_t *)&bytes->_val->_slice.start[*curr_idx], &second_len, sizeof(uint32_t));
+    _z_host_le_store32((uint32_t)second_len, (uint8_t *)&bytes->_val->_slice.start[*curr_idx]);
     *curr_idx += sizeof(uint32_t);
     memcpy((uint8_t *)&bytes->_val->_slice.start[*curr_idx], z_slice_data(&second->_val->_slice), second_len);
     *curr_idx += second_len;

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 --------*/
@@ -162,24 +163,11 @@ uint8_t _z_bytes_to_uint8(const _z_bytes_t *bs) {
     return val;
 }
 
-// FIXME: int16+ endianness, Issue #420
-uint16_t _z_bytes_to_uint16(const _z_bytes_t *bs) {
-    uint16_t val = 0;
-    memcpy(&val, bs->_slice.start, sizeof(val));
-    return val;
-}
+uint16_t _z_bytes_to_uint16(const _z_bytes_t *bs) { return _z_host_le_load16(bs->_slice.start); }
 
-uint32_t _z_bytes_to_uint32(const _z_bytes_t *bs) {
-    uint32_t val = 0;
-    memcpy(&val, bs->_slice.start, sizeof(val));
-    return val;
-}
+uint32_t _z_bytes_to_uint32(const _z_bytes_t *bs) { return _z_host_le_load32(bs->_slice.start); }
 
-uint64_t _z_bytes_to_uint64(const _z_bytes_t *bs) {
-    uint64_t val = 0;
-    memcpy(&val, bs->_slice.start, sizeof(val));
-    return val;
-}
+uint64_t _z_bytes_to_uint64(const _z_bytes_t *bs) { return _z_host_le_load64(bs->_slice.start); }
 
 float _z_bytes_to_float(const _z_bytes_t *bs) {
     float val = 0;
@@ -222,7 +210,7 @@ _z_bytes_t _z_bytes_from_uint16(uint16_t val) {
         return ret;
     }
     // Encode int
-    memcpy((uint8_t *)ret._slice.start, &val, sizeof(val));
+    _z_host_le_store16(val, (uint8_t *)ret._slice.start);
     return ret;
 }
 
@@ -233,7 +221,7 @@ _z_bytes_t _z_bytes_from_uint32(uint32_t val) {
         return ret;
     }
     // Encode int
-    memcpy((uint8_t *)ret._slice.start, &val, sizeof(val));
+    _z_host_le_store32(val, (uint8_t *)ret._slice.start);
     return ret;
 }
 
@@ -244,7 +232,7 @@ _z_bytes_t _z_bytes_from_uint64(uint64_t val) {
         return ret;
     }
     // Encode int
-    memcpy((uint8_t *)ret._slice.start, &val, sizeof(val));
+    _z_host_le_store64(val, (uint8_t *)ret._slice.start);
     return ret;
 }
 

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;
@@ -123,7 +132,7 @@ uint8_t _z_zint_len(uint64_t v) {
 int8_t _z_zint64_encode(_z_wbuf_t *wbf, uint64_t v) {
     uint64_t lv = v;
     uint8_t len = 0;
-    while ((lv & (uint64_t)~0x7f) != 0) {
+    while ((lv & VLE_LEN1_MASK) != 0) {
         uint8_t c = (lv & 0x7f) | 0x80;
         _Z_RETURN_IF_ERR(_z_wbuf_write(wbf, c))
         len++;
@@ -133,7 +142,6 @@ int8_t _z_zint64_encode(_z_wbuf_t *wbf, uint64_t v) {
         uint8_t c = (lv & 0xff);
         _Z_RETURN_IF_ERR(_z_wbuf_write(wbf, c))
     }
-
     return _Z_RES_OK;
 }
 

src/transport/common/rx.c

@@ -12,15 +12,26 @@
 //   ZettaScale Zenoh Team, <zenoh@zettascale.tech>
 //
 
-#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;
 

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

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) {

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) {

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) {

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) {