From 2d5da1b2e9ba2f59b28b0d9d44ba455eb30d174d Mon Sep 17 00:00:00 2001
From: Luca Cominardi <luca.cominardi@gmail.com>
Date: Mon, 18 Mar 2024 18:27:35 +0100
Subject: [PATCH] Improved zint codec

---
 include/zenoh-pico/protocol/codec/core.h |   2 +-
 src/protocol/codec.c                     | 102 +++++++++++++----------
 zenohpico.pc                             |   2 +-
 3 files changed, 59 insertions(+), 47 deletions(-)

diff --git a/include/zenoh-pico/protocol/codec/core.h b/include/zenoh-pico/protocol/codec/core.h
index 13725f8d9..aa1d78599 100644
--- a/include/zenoh-pico/protocol/codec/core.h
+++ b/include/zenoh-pico/protocol/codec/core.h
@@ -62,7 +62,7 @@ int8_t _z_uint16_decode(uint16_t *u16, _z_zbuf_t *buf);
 int8_t _z_uint64_encode(_z_wbuf_t *buf, uint64_t v);
 int8_t _z_uint64_decode(uint64_t *u64, _z_zbuf_t *buf);
 
-uint8_t _z_zint_len(_z_zint_t v);
+uint8_t _z_zint_len(uint64_t v);
 int8_t _z_zint_encode(_z_wbuf_t *buf, _z_zint_t v);
 int8_t _z_zint64_encode(_z_wbuf_t *buf, uint64_t v);
 int8_t _z_zint16_decode(uint16_t *zint, _z_zbuf_t *buf);
diff --git a/src/protocol/codec.c b/src/protocol/codec.c
index 5320f12b1..217198f71 100644
--- a/src/protocol/codec.c
+++ b/src/protocol/codec.c
@@ -202,38 +202,54 @@ int8_t _z_uint64_decode(uint64_t *u64, _z_zbuf_t *zbf) {
 }
 
 /*------------------ z_zint ------------------*/
-uint8_t _z_zint_len(_z_zint_t v) {
-    uint8_t len = 1;
-    while (v > 0x7f) {
-        v >>= 7;
-        len++;
+#define VLE_LEN 9
+
+uint8_t _z_zint_len(uint64_t v) {
+    if ((v & (UINT64_MAX << (7 * 1))) == 0) {
+        return 1;
+    } else if ((v & (UINT64_MAX << (7 * 2))) == 0) {
+        return 2;
+    } else if ((v & (UINT64_MAX << (7 * 3))) == 0) {
+        return 3;
+    } else if ((v & (UINT64_MAX << (7 * 4))) == 0) {
+        return 4;
+    } else if ((v & (UINT64_MAX << (7 * 5))) == 0) {
+        return 5;
+    } else if ((v & (UINT64_MAX << (7 * 6))) == 0) {
+        return 6;
+    } else if ((v & (UINT64_MAX << (7 * 7))) == 0) {
+        return 7;
+    } else if ((v & (UINT64_MAX << (7 * 8))) == 0) {
+        return 8;
+    } else {
+        return 9;
     }
-    return len;
 }
-int8_t _z_zint_encode(_z_wbuf_t *wbf, _z_zint_t v) {
-    _z_zint_t lv = v;
 
-    while (lv > 0x7f) {
-        uint8_t c = (lv & 0x7f) | 0x80;
-        _Z_RETURN_IF_ERR(_z_wbuf_write(wbf, c))
-        lv = lv >> (_z_zint_t)7;
-    }
-
-    uint8_t c = lv & 0xff;
-    return _z_wbuf_write(wbf, c);
-}
 int8_t _z_zint64_encode(_z_wbuf_t *wbf, uint64_t v) {
     uint64_t lv = v;
-
-    while (lv > 0x7f) {
+    uint8_t len = 0;
+    while ((lv & ~0x7f) != 0) {
         uint8_t c = (lv & 0x7f) | 0x80;
         _Z_RETURN_IF_ERR(_z_wbuf_write(wbf, c))
+        len++;
         lv = lv >> (uint64_t)7;
     }
+    if (len != VLE_LEN) {
+        uint8_t c = (lv & 0x7f);
+        _Z_RETURN_IF_ERR(_z_wbuf_write(wbf, c))
+    }
 
-    uint8_t c = lv & 0xff;
-    return _z_wbuf_write(wbf, c);
+    return _Z_RES_OK;
 }
+
+int8_t _z_zint_encode(_z_wbuf_t *wbf, _z_zint_t v) {
+    if (v > UINT64_MAX) {
+        return _Z_ERR_MESSAGE_SERIALIZATION_FAILED;
+    }
+    return _z_zint64_encode(wbf, (uint64_t)v);
+}
+
 int8_t _z_zint16_decode(uint16_t *zint, _z_zbuf_t *zbf) {
     int8_t ret = _Z_RES_OK;
     _z_zint_t buf;
@@ -245,6 +261,7 @@ int8_t _z_zint16_decode(uint16_t *zint, _z_zbuf_t *zbf) {
     }
     return ret;
 }
+
 int8_t _z_zint32_decode(uint32_t *zint, _z_zbuf_t *zbf) {
     int8_t ret = _Z_RES_OK;
     _z_zint_t buf;
@@ -256,39 +273,34 @@ int8_t _z_zint32_decode(uint32_t *zint, _z_zbuf_t *zbf) {
     }
     return ret;
 }
+
 int8_t _z_zint_decode(_z_zint_t *zint, _z_zbuf_t *zbf) {
     int8_t ret = _Z_RES_OK;
-    *zint = 0;
-
-    uint8_t i = 0;
-    uint8_t u8 = 0;
-    do {
-        if (_z_uint8_decode(&u8, zbf) == _Z_RES_OK) {
-            *zint = *zint | (((_z_zint_t)u8 & 0x7f) << i);
-            i = i + (uint8_t)7;
-        } else {
-            ret |= _Z_ERR_MESSAGE_DESERIALIZATION_FAILED;
-        }
-    } while (u8 > (uint8_t)0x7f);
-
+    _z_zint_t buf;
+    _Z_RETURN_IF_ERR(_z_zint_decode(&buf, zbf));
+    if (buf <= UINT64_MAX) {
+        *zint = (_z_zint_t)buf;
+    } else {
+        ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED;
+    }
     return ret;
 }
+
 int8_t _z_zint64_decode(uint64_t *zint, _z_zbuf_t *zbf) {
-    int8_t ret = _Z_RES_OK;
     *zint = 0;
 
+    uint8_t b = 0;
+    _Z_RETURN_IF_ERR(_z_uint8_decode(&b, zbf));
+
     uint8_t i = 0;
-    uint8_t u8 = 0;
-    do {
-        if (_z_uint8_decode(&u8, zbf) == _Z_RES_OK) {
-            *zint = *zint | (((uint64_t)u8 & 0x7f) << i);
-            i = i + (uint8_t)7;
-        } else {
-            ret |= _Z_ERR_MESSAGE_DESERIALIZATION_FAILED;
-        }
-    } while (u8 > (uint8_t)0x7f);
+    while (((b & 0x80) != 0) && (i != 7 * (VLE_LEN - 1))) {
+        *zint = *zint | ((uint64_t)(b & 0x7f)) << i;
+        _Z_RETURN_IF_ERR(_z_uint8_decode(&b, zbf));
+        i = i + (uint8_t)7;
+    }
+    *zint = *zint | ((uint64_t)b << i);
 
-    return ret;
+    return _Z_RES_OK;
 }
 
 /*------------------ uint8_array ------------------*/
diff --git a/zenohpico.pc b/zenohpico.pc
index 9f520a887..117ca19dc 100644
--- a/zenohpico.pc
+++ b/zenohpico.pc
@@ -3,6 +3,6 @@ prefix=/usr/local
 Name: zenohpico
 Description: 
 URL: 
-Version: 0.11.20240208dev
+Version: 0.11.20240318dev
 Cflags: -I${prefix}/include
 Libs: -L${prefix}/lib -lzenohpico