diff --git a/CMakeLists.txt b/CMakeLists.txt index 7a9fb6d10..d986b9975 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -233,6 +233,10 @@ set(Z_FEATURE_MULTICAST_TRANSPORT 1 CACHE STRING "Toggle multicast transport") set(Z_FEATURE_UNICAST_TRANSPORT 1 CACHE STRING "Toggle unicast transport") set(Z_FEATURE_RAWETH_TRANSPORT 0 CACHE STRING "Toggle raw ethernet transport") set(Z_FEATURE_TCP_NODELAY 1 CACHE STRING "Toggle TCP_NODELAY") +set(Z_FEATURE_LOCAL_SUBSCRIBER 0 CACHE STRING "Toggle local subscriptions") +set(Z_FEATURE_PUBLISHER_SESSION_CHECK 1 CACHE STRING "Toggle publisher session check") +set(Z_FEATURE_BATCHING 1 CACHE STRING "Toggle batching") +set(Z_FEATURE_RX_CACHE 0 CACHE STRING "Toggle RX_CACHE") # Add a warning message if someone tries to enable Z_FEATURE_LIVELINESS directly if(Z_FEATURE_LIVELINESS AND NOT Z_FEATURE_UNSTABLE_API) @@ -454,7 +458,7 @@ if(BUILD_EXAMPLES) add_subdirectory(examples) endif() -if(ASAN) +if(ASAN AND !MSVC) add_compile_options(-fsanitize=address) add_link_options(-fsanitize=address) endif() diff --git a/GNUmakefile b/GNUmakefile index 12a97bf06..15985b481 100644 --- a/GNUmakefile +++ b/GNUmakefile @@ -67,7 +67,7 @@ Z_FEATURE_RAWETH_TRANSPORT?=0 # Buffer sizes FRAG_MAX_SIZE?=300000 BATCH_UNICAST_SIZE?=65535 -BATCH_MULTICAST_SIZE?=8096 +BATCH_MULTICAST_SIZE?=8192 # zenoh-pico/ directory ROOT_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST)))) diff --git a/examples/unix/c11/z_get_attachment.c b/examples/unix/c11/z_get_attachment.c index f3b18037d..1d20e3c96 100644 --- a/examples/unix/c11/z_get_attachment.c +++ b/examples/unix/c11/z_get_attachment.c @@ -68,12 +68,11 @@ void reply_handler(z_loaned_reply_t *reply, void *ctx) { // Check attachment const z_loaned_bytes_t *attachment = z_sample_attachment(sample); - if (attachment == NULL) { - return; - } ze_deserializer_t deserializer = ze_deserializer_from_bytes(attachment); size_t attachment_len; - ze_deserializer_deserialize_sequence_length(&deserializer, &attachment_len); + if (ze_deserializer_deserialize_sequence_length(&deserializer, &attachment_len) < 0) { + return; + } kv_pair_t *kvp = (kv_pair_t *)malloc(sizeof(kv_pair_t) * attachment_len); for (size_t i = 0; i < attachment_len; ++i) { ze_deserializer_deserialize_string(&deserializer, &kvp[i].key); @@ -178,7 +177,7 @@ int main(int argc, char **argv) { ze_owned_serializer_t serializer; ze_serializer_empty(&serializer); - ze_serializer_serialize_sequence_length(z_loan_mut(serializer), 2); + ze_serializer_serialize_sequence_length(z_loan_mut(serializer), 1); for (size_t i = 0; i < 1; ++i) { ze_serializer_serialize_string(z_loan_mut(serializer), z_loan(kvs[i].key)); ze_serializer_serialize_string(z_loan_mut(serializer), z_loan(kvs[i].value)); diff --git a/examples/unix/c11/z_queryable_attachment.c b/examples/unix/c11/z_queryable_attachment.c index f915d7767..fceed2e2f 100644 --- a/examples/unix/c11/z_queryable_attachment.c +++ b/examples/unix/c11/z_queryable_attachment.c @@ -68,10 +68,9 @@ void query_handler(z_loaned_query_t *query, void *ctx) { // Check attachment const z_loaned_bytes_t *attachment = z_query_attachment(query); - if (attachment != NULL) { - ze_deserializer_t deserializer = ze_deserializer_from_bytes(attachment); - size_t attachment_len; - ze_deserializer_deserialize_sequence_length(&deserializer, &attachment_len); + ze_deserializer_t deserializer = ze_deserializer_from_bytes(attachment); + size_t attachment_len; + if (ze_deserializer_deserialize_sequence_length(&deserializer, &attachment_len) == Z_OK) { kv_pair_t *kvp = (kv_pair_t *)malloc(sizeof(kv_pair_t) * attachment_len); for (size_t i = 0; i < attachment_len; ++i) { ze_deserializer_deserialize_string(&deserializer, &kvp[i].key); diff --git a/examples/unix/c11/z_sub_attachment.c b/examples/unix/c11/z_sub_attachment.c index 73a320d4e..0421f419e 100644 --- a/examples/unix/c11/z_sub_attachment.c +++ b/examples/unix/c11/z_sub_attachment.c @@ -64,12 +64,10 @@ void data_handler(z_loaned_sample_t *sample, void *ctx) { printf(" with timestamp: %" PRIu64 "\n", z_timestamp_ntp64_time(ts)); } // Check attachment - const z_loaned_bytes_t *attachment = z_sample_attachment(sample); - if (attachment != NULL) { - ze_deserializer_t deserializer = ze_deserializer_from_bytes(attachment); - size_t attachment_len; - ze_deserializer_deserialize_sequence_length(&deserializer, &attachment_len); + ze_deserializer_t deserializer = ze_deserializer_from_bytes(attachment); + size_t attachment_len; + if (ze_deserializer_deserialize_sequence_length(&deserializer, &attachment_len) == Z_OK) { kv_pair_t *kvp = (kv_pair_t *)malloc(sizeof(kv_pair_t) * attachment_len); for (size_t i = 0; i < attachment_len; ++i) { ze_deserializer_deserialize_string(&deserializer, &kvp[i].key); diff --git a/include/zenoh-pico/api/primitives.h b/include/zenoh-pico/api/primitives.h index 0ad1a5d6a..3da9ab8f0 100644 --- a/include/zenoh-pico/api/primitives.h +++ b/include/zenoh-pico/api/primitives.h @@ -2060,6 +2060,45 @@ z_result_t z_declare_background_subscriber(const z_loaned_session_t *zs, const z const z_loaned_keyexpr_t *z_subscriber_keyexpr(const z_loaned_subscriber_t *subscriber); #endif +#ifdef Z_FEATURE_UNSTABLE_API +#if Z_FEATURE_BATCHING == 1 +/** + * Activate the batching mechanism, any message that would have been sent on the network by a subsequent api call (e.g + * z_put, z_get) will be instead stored until the batch is full, flushed with :c:func:`zp_batch_flush` or batching is + * stopped with :c:func:`zp_batch_stop`. + * + * Parameters: + * zs: Pointer to a :c:type:`z_loaned_session_t` that will start batching messages. + * + * Return: + * ``0`` if batching started, ``negative value`` otherwise. + */ +z_result_t zp_batch_start(const z_loaned_session_t *zs); + +/** + * Send the currently batched messages on the network. + * + * Parameters: + * zs: Pointer to a :c:type:`z_loaned_session_t` that will send its batched messages. + * + * Return: + * ``0`` if batch successfully sent, ``negative value`` otherwise. + */ +z_result_t zp_batch_flush(const z_loaned_session_t *zs); + +/** + * Deactivate the batching mechanism and send the currently batched on the network. + * + * Parameters: + * zs: Pointer to a :c:type:`z_loaned_session_t` that will stop batching messages. + * + * Return: + * ``0`` if batching stopped and batch successfully sent, ``negative value`` otherwise. + */ +z_result_t zp_batch_stop(const z_loaned_session_t *zs); +#endif +#endif + /************* Multi Thread Tasks helpers **************/ /** * Builds a :c:type:`zp_task_read_options_t` with default value. diff --git a/include/zenoh-pico/collections/arc_slice.h b/include/zenoh-pico/collections/arc_slice.h index 7e03c936c..782373141 100644 --- a/include/zenoh-pico/collections/arc_slice.h +++ b/include/zenoh-pico/collections/arc_slice.h @@ -28,29 +28,30 @@ extern "C" { #endif -_Z_REFCOUNT_DEFINE(_z_slice, _z_slice) +_Z_SIMPLE_REFCOUNT_DEFINE(_z_slice, _z_slice) /*-------- ArcSlice --------*/ /** * An atomically reference counted subslice. * * Members: - * _z_slice_rc_t len: Rc counted slice. + * _z_slice_simple_rc_t len: Rc counted slice. * size_t start: Offset to the subslice start. * size_t len: Length of the subslice. */ typedef struct { - _z_slice_rc_t slice; + _z_slice_simple_rc_t slice; size_t start; size_t len; } _z_arc_slice_t; -_z_arc_slice_t _z_arc_slice_empty(void); +static inline _z_arc_slice_t _z_arc_slice_empty(void) { return (_z_arc_slice_t){0}; } +static inline size_t _z_arc_slice_len(const _z_arc_slice_t* s) { return s->len; } +static inline bool _z_arc_slice_is_empty(const _z_arc_slice_t* s) { return _z_arc_slice_len(s) == 0; } _z_arc_slice_t _z_arc_slice_wrap(_z_slice_t s, size_t offset, size_t len); +_z_arc_slice_t _z_arc_slice_wrap_slice_rc(_z_slice_simple_rc_t* slice_rc, size_t offset, size_t len); _z_arc_slice_t _z_arc_slice_get_subslice(const _z_arc_slice_t* s, size_t offset, size_t len); -size_t _z_arc_slice_len(const _z_arc_slice_t* s); -bool _z_arc_slice_is_empty(const _z_arc_slice_t* s); const uint8_t* _z_arc_slice_data(const _z_arc_slice_t* s); z_result_t _z_arc_slice_copy(_z_arc_slice_t* dst, const _z_arc_slice_t* src); z_result_t _z_arc_slice_move(_z_arc_slice_t* dst, _z_arc_slice_t* src); diff --git a/include/zenoh-pico/collections/bytes.h b/include/zenoh-pico/collections/bytes.h index 05ab01e37..19e0d3136 100644 --- a/include/zenoh-pico/collections/bytes.h +++ b/include/zenoh-pico/collections/bytes.h @@ -31,10 +31,7 @@ inline size_t _z_arc_slice_size(const _z_arc_slice_t *s) { (void)s; return sizeof(_z_arc_slice_t); } -static inline void _z_arc_slice_elem_move(void *dst, void *src) { - _z_arc_slice_move((_z_arc_slice_t *)dst, (_z_arc_slice_t *)src); -} -_Z_ELEM_DEFINE(_z_arc_slice, _z_arc_slice_t, _z_arc_slice_size, _z_arc_slice_drop, _z_arc_slice_copy) +_Z_ELEM_DEFINE(_z_arc_slice, _z_arc_slice_t, _z_arc_slice_size, _z_arc_slice_drop, _z_arc_slice_copy, _z_arc_slice_move) _Z_SVEC_DEFINE(_z_arc_slice, _z_arc_slice_t) /*-------- Bytes --------*/ @@ -49,8 +46,13 @@ typedef struct { _z_arc_slice_svec_t _slices; } _z_bytes_t; +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_bytes_t _z_bytes_null(void) { return (_z_bytes_t){0}; } +static inline void _z_bytes_alias_arc_slice(_z_bytes_t *dst, _z_arc_slice_t *s) { + dst->_slices = _z_arc_slice_svec_alias_element(s); +} +_z_bytes_t _z_bytes_alias(const _z_bytes_t src); bool _z_bytes_check(const _z_bytes_t *bytes); -_z_bytes_t _z_bytes_null(void); z_result_t _z_bytes_append_bytes(_z_bytes_t *dst, _z_bytes_t *src); z_result_t _z_bytes_append_slice(_z_bytes_t *dst, _z_arc_slice_t *s); z_result_t _z_bytes_copy(_z_bytes_t *dst, const _z_bytes_t *src); diff --git a/include/zenoh-pico/collections/element.h b/include/zenoh-pico/collections/element.h index bd5701187..5657a63b9 100644 --- a/include/zenoh-pico/collections/element.h +++ b/include/zenoh-pico/collections/element.h @@ -34,7 +34,7 @@ typedef void (*z_element_move_f)(void *dst, void *src); typedef void *(*z_element_clone_f)(const void *e); typedef bool (*z_element_eq_f)(const void *left, const void *right); -#define _Z_ELEM_DEFINE(name, type, elem_size_f, elem_clear_f, elem_copy_f) \ +#define _Z_ELEM_DEFINE(name, type, elem_size_f, elem_clear_f, elem_copy_f, elem_move_f) \ typedef bool (*name##_eq_f)(const type *left, const type *right); \ static inline void name##_elem_clear(void *e) { elem_clear_f((type *)e); } \ static inline void name##_elem_free(void **e) { \ @@ -45,6 +45,7 @@ typedef bool (*z_element_eq_f)(const void *left, const void *right); *e = NULL; \ } \ } \ + static inline void name##_elem_move(void *dst, void *src) { elem_move_f((type *)dst, (type *)src); } \ static inline void name##_elem_copy(void *dst, const void *src) { elem_copy_f((type *)dst, (type *)src); } \ static inline void *name##_elem_clone(const void *src) { \ type *dst = (type *)z_malloc(elem_size_f((type *)src)); \ @@ -76,7 +77,7 @@ static inline void _z_noop_move(void *dst, void *src) { _ZP_UNUSED(src); } -_Z_ELEM_DEFINE(_z_noop, _z_noop_t, _z_noop_size, _z_noop_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_noop, _z_noop_t, _z_noop_size, _z_noop_clear, _z_noop_copy, _z_noop_move) #ifdef __cplusplus } diff --git a/include/zenoh-pico/collections/refcount.h b/include/zenoh-pico/collections/refcount.h index 099ab04ae..86775c68f 100644 --- a/include/zenoh-pico/collections/refcount.h +++ b/include/zenoh-pico/collections/refcount.h @@ -34,6 +34,12 @@ z_result_t _z_rc_weak_upgrade(void *cnt); size_t _z_rc_weak_count(void *cnt); size_t _z_rc_strong_count(void *cnt); +z_result_t _z_simple_rc_init(void **cnt); +z_result_t _z_simple_rc_increase(void *cnt); +bool _z_simple_rc_decrease(void **cnt); + +size_t _z_simple_rc_strong_count(void *cnt); + /*------------------ Internal Array Macros ------------------*/ #define _Z_REFCOUNT_DEFINE(name, type) \ typedef struct name##_rc_t { \ @@ -46,18 +52,8 @@ size_t _z_rc_strong_count(void *cnt); void *_cnt; \ } name##_weak_t; \ \ - static inline name##_rc_t name##_rc_null(void) { \ - name##_rc_t p; \ - p._val = NULL; \ - p._cnt = NULL; \ - return p; \ - } \ - static inline name##_weak_t name##_weak_null(void) { \ - name##_weak_t p; \ - p._val = NULL; \ - p._cnt = NULL; \ - return p; \ - } \ + static inline name##_rc_t name##_rc_null(void) { return (name##_rc_t){0}; } \ + static inline name##_weak_t name##_weak_null(void) { return (name##_weak_t){0}; } \ \ static inline name##_rc_t name##_rc_new(type##_t *val) { \ name##_rc_t p = name##_rc_null(); \ @@ -80,11 +76,10 @@ size_t _z_rc_strong_count(void *cnt); return p; \ } \ static inline name##_rc_t name##_rc_clone(const name##_rc_t *p) { \ - name##_rc_t c = name##_rc_null(); \ if (_z_rc_increase_strong(p->_cnt) == _Z_RES_OK) { \ - c = *p; \ + return *p; \ } \ - return c; \ + return name##_rc_null(); \ } \ static inline name##_rc_t *name##_rc_clone_as_ptr(const name##_rc_t *p) { \ name##_rc_t *c = (name##_rc_t *)z_malloc(sizeof(name##_rc_t)); \ @@ -97,12 +92,13 @@ size_t _z_rc_strong_count(void *cnt); return c; \ } \ static inline name##_weak_t name##_rc_clone_as_weak(const name##_rc_t *p) { \ - name##_weak_t c = name##_weak_null(); \ if (_z_rc_increase_weak(p->_cnt) == _Z_RES_OK) { \ - c._val = p->_val; \ - c._cnt = p->_cnt; \ + name##_weak_t ret; \ + ret._val = p->_val; \ + ret._cnt = p->_cnt; \ + return ret; \ } \ - return c; \ + return name##_weak_null(); \ } \ static inline name##_weak_t *name##_rc_clone_as_weak_ptr(const name##_rc_t *p) { \ name##_weak_t *c = (name##_weak_t *)z_malloc(sizeof(name##_weak_t)); \ @@ -141,20 +137,20 @@ size_t _z_rc_strong_count(void *cnt); return res; \ } \ static inline name##_weak_t name##_weak_clone(const name##_weak_t *p) { \ - name##_weak_t c = name##_weak_null(); \ if (_z_rc_increase_weak(p->_cnt) == _Z_RES_OK) { \ - c = *p; \ + return *p; \ } \ - return c; \ + return name##_weak_null(); \ } \ static inline void name##_weak_copy(name##_weak_t *dst, const name##_weak_t *p) { *dst = name##_weak_clone(p); } \ static inline name##_rc_t name##_weak_upgrade(const name##_weak_t *p) { \ - name##_rc_t c = name##_rc_null(); \ if (_z_rc_weak_upgrade(p->_cnt) == _Z_RES_OK) { \ - c._val = p->_val; \ - c._cnt = p->_cnt; \ + name##_rc_t ret; \ + ret._val = p->_val; \ + ret._cnt = p->_cnt; \ + return ret; \ } \ - return c; \ + return name##_rc_null(); \ } \ static inline bool name##_weak_eq(const name##_weak_t *left, const name##_weak_t *right) { \ return (left->_val == right->_val); \ @@ -175,6 +171,86 @@ size_t _z_rc_strong_count(void *cnt); return sizeof(name##_rc_t); \ } +#define _Z_SIMPLE_REFCOUNT_DEFINE(name, type) \ + typedef struct name##_simple_rc_t { \ + type##_t *_val; \ + void *_cnt; \ + } name##_simple_rc_t; \ + \ + static inline name##_simple_rc_t name##_simple_rc_null(void) { return (name##_simple_rc_t){0}; } \ + \ + static inline name##_simple_rc_t name##_simple_rc_new(type##_t *val) { \ + name##_simple_rc_t p = name##_simple_rc_null(); \ + if (_z_simple_rc_init(&p._cnt) == _Z_RES_OK) { \ + p._val = val; \ + } \ + return p; \ + } \ + static inline name##_simple_rc_t name##_simple_rc_new_from_val(const type##_t *val) { \ + type##_t *v = (type##_t *)z_malloc(sizeof(type##_t)); \ + if (v == NULL) { \ + return name##_simple_rc_null(); \ + } \ + *v = *val; \ + name##_simple_rc_t p = name##_simple_rc_new(v); \ + if (p._cnt == NULL) { \ + z_free(v); \ + return name##_simple_rc_null(); \ + } \ + return p; \ + } \ + static inline name##_simple_rc_t name##_simple_rc_clone(const name##_simple_rc_t *p) { \ + if (_z_simple_rc_increase(p->_cnt) == _Z_RES_OK) { \ + return *p; \ + } \ + return name##_simple_rc_null(); \ + } \ + static inline name##_simple_rc_t *name##_simple_rc_clone_as_ptr(const name##_simple_rc_t *p) { \ + name##_simple_rc_t *c = (name##_simple_rc_t *)z_malloc(sizeof(name##_simple_rc_t)); \ + if (c != NULL) { \ + *c = name##_simple_rc_clone(p); \ + if (c->_cnt == NULL) { \ + z_free(c); \ + } \ + } \ + return c; \ + } \ + static inline void name##_simple_rc_copy(name##_simple_rc_t *dst, const name##_simple_rc_t *p) { \ + *dst = name##_simple_rc_clone(p); \ + } \ + static inline bool name##_simple_rc_eq(const name##_simple_rc_t *left, const name##_simple_rc_t *right) { \ + return (left->_val == right->_val); \ + } \ + static inline bool name##_simple_rc_decr(name##_simple_rc_t *p) { \ + if ((p == NULL) || (p->_cnt == NULL)) { \ + return false; \ + } \ + if (_z_simple_rc_decrease(&p->_cnt)) { \ + return true; \ + } \ + return false; \ + } \ + static inline bool name##_simple_rc_drop(name##_simple_rc_t *p) { \ + if (p == NULL) { \ + return false; \ + } \ + bool res = false; \ + if (name##_simple_rc_decr(p) && p->_val != NULL) { \ + type##_clear(p->_val); \ + z_free(p->_val); \ + res = true; \ + } \ + *p = name##_simple_rc_null(); \ + return res; \ + } \ + static inline size_t name##_simple_rc_count(const name##_simple_rc_t *p) { \ + return _z_simple_rc_strong_count(p->_cnt); \ + } \ + static inline size_t name##_simple_rc_size(name##_simple_rc_t *p) { \ + _ZP_UNUSED(p); \ + return sizeof(name##_simple_rc_t); \ + } + #ifdef __cplusplus } #endif diff --git a/include/zenoh-pico/collections/slice.h b/include/zenoh-pico/collections/slice.h index c57af6ff9..278048a82 100644 --- a/include/zenoh-pico/collections/slice.h +++ b/include/zenoh-pico/collections/slice.h @@ -30,9 +30,16 @@ typedef struct { void *context; } _z_delete_context_t; -_z_delete_context_t _z_delete_context_null(void); -bool _z_delete_context_is_null(const _z_delete_context_t *c); -_z_delete_context_t _z_delete_context_create(void (*deleter)(void *context, void *data), void *context); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_delete_context_t _z_delete_context_null(void) { return (_z_delete_context_t){0}; } + +static inline _z_delete_context_t _z_delete_context_create(void (*deleter)(void *context, void *data), void *context) { + _z_delete_context_t ret; + ret.deleter = deleter; + ret.context = context; + return ret; +} +static inline bool _z_delete_context_is_null(const _z_delete_context_t *c) { return c->deleter == NULL; } _z_delete_context_t _z_delete_context_default(void); void _z_delete_context_delete(_z_delete_context_t *c, void *data); @@ -51,21 +58,27 @@ typedef struct { _z_delete_context_t _delete_context; } _z_slice_t; -_z_slice_t _z_slice_empty(void); -inline static bool _z_slice_check(const _z_slice_t *slice) { return slice->start != NULL; } +static inline _z_slice_t _z_slice_null(void) { return (_z_slice_t){0}; } +static inline void _z_slice_reset(_z_slice_t *bs) { *bs = _z_slice_null(); } +static inline bool _z_slice_is_empty(const _z_slice_t *bs) { return bs->len == 0; } +static inline bool _z_slice_check(const _z_slice_t *slice) { return slice->start != NULL; } +static inline _z_slice_t _z_slice_alias(const _z_slice_t bs) { + _z_slice_t ret; + ret.len = bs.len; + ret.start = bs.start; + ret._delete_context = _z_delete_context_null(); + return ret; +} z_result_t _z_slice_init(_z_slice_t *bs, size_t capacity); _z_slice_t _z_slice_make(size_t capacity); _z_slice_t _z_slice_alias_buf(const uint8_t *bs, size_t len); _z_slice_t _z_slice_from_buf_custom_deleter(const uint8_t *p, size_t len, _z_delete_context_t dc); _z_slice_t _z_slice_copy_from_buf(const uint8_t *bs, size_t len); _z_slice_t _z_slice_steal(_z_slice_t *b); -_z_slice_t _z_slice_alias(const _z_slice_t *bs); z_result_t _z_slice_copy(_z_slice_t *dst, const _z_slice_t *src); z_result_t _z_slice_n_copy(_z_slice_t *dst, const _z_slice_t *src, size_t offset, size_t len); _z_slice_t _z_slice_duplicate(const _z_slice_t *src); void _z_slice_move(_z_slice_t *dst, _z_slice_t *src); -void _z_slice_reset(_z_slice_t *bs); -bool _z_slice_is_empty(const _z_slice_t *bs); bool _z_slice_eq(const _z_slice_t *left, const _z_slice_t *right); void _z_slice_clear(_z_slice_t *bs); void _z_slice_free(_z_slice_t **bs); diff --git a/include/zenoh-pico/collections/string.h b/include/zenoh-pico/collections/string.h index a54ac68dd..9e094b795 100644 --- a/include/zenoh-pico/collections/string.h +++ b/include/zenoh-pico/collections/string.h @@ -37,7 +37,7 @@ bool _z_str_eq(const char *left, const char *right); size_t _z_str_size(const char *src); void _z_str_copy(char *dst, const char *src); void _z_str_n_copy(char *dst, const char *src, size_t size); -_Z_ELEM_DEFINE(_z_str, char, _z_str_size, _z_noop_clear, _z_str_copy) +_Z_ELEM_DEFINE(_z_str, char, _z_str_size, _z_noop_clear, _z_str_copy, _z_noop_move) _Z_VEC_DEFINE(_z_str, char) _Z_LIST_DEFINE(_z_str, char) @@ -70,11 +70,19 @@ typedef struct { _z_slice_t _slice; } _z_string_t; -_z_string_t _z_string_null(void); -bool _z_string_check(const _z_string_t *value); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_string_t _z_string_null(void) { return (_z_string_t){0}; } +static inline bool _z_string_check(const _z_string_t *value) { return !_z_slice_is_empty(&value->_slice); } +static inline _z_string_t _z_string_alias(const _z_string_t str) { + _z_string_t ret; + ret._slice = _z_slice_alias(str._slice); + return ret; +} + _z_string_t _z_string_copy_from_str(const char *value); _z_string_t _z_string_copy_from_substr(const char *value, size_t len); _z_string_t *_z_string_copy_from_str_as_ptr(const char *value); +_z_string_t _z_string_alias_slice(const _z_slice_t *slice); _z_string_t _z_string_alias_str(const char *value); _z_string_t _z_string_alias_substr(const char *value, size_t len); _z_string_t _z_string_from_str_custom_deleter(char *value, _z_delete_context_t c); @@ -88,7 +96,6 @@ z_result_t _z_string_copy(_z_string_t *dst, const _z_string_t *src); z_result_t _z_string_copy_substring(_z_string_t *dst, const _z_string_t *src, size_t offset, size_t len); void _z_string_move(_z_string_t *dst, _z_string_t *src); _z_string_t _z_string_steal(_z_string_t *str); -_z_string_t _z_string_alias(const _z_string_t *str); void _z_string_move_str(_z_string_t *dst, char *src); void _z_string_clear(_z_string_t *s); void _z_string_free(_z_string_t **s); @@ -99,9 +106,7 @@ _z_string_t _z_string_preallocate(const size_t len); char *_z_str_from_string_clone(const _z_string_t *str); -_Z_ELEM_DEFINE(_z_string, _z_string_t, _z_string_len, _z_string_clear, _z_string_copy) - -static inline void _z_string_elem_move(void *dst, void *src) { _z_string_move((_z_string_t *)dst, (_z_string_t *)src); } +_Z_ELEM_DEFINE(_z_string, _z_string_t, _z_string_len, _z_string_clear, _z_string_copy, _z_string_move) _Z_SVEC_DEFINE(_z_string, _z_string_t) _Z_LIST_DEFINE(_z_string, _z_string_t) _Z_INT_MAP_DEFINE(_z_string, _z_string_t) diff --git a/include/zenoh-pico/collections/vec.h b/include/zenoh-pico/collections/vec.h index d89acd93c..a9277d423 100644 --- a/include/zenoh-pico/collections/vec.h +++ b/include/zenoh-pico/collections/vec.h @@ -33,8 +33,11 @@ typedef struct { void **_val; } _z_vec_t; +static inline _z_vec_t _z_vec_null(void) { return (_z_vec_t){0}; } +static inline _z_vec_t _z_vec_alias(const _z_vec_t *src) { return *src; } _z_vec_t _z_vec_make(size_t capacity); void _z_vec_copy(_z_vec_t *dst, const _z_vec_t *src, z_element_clone_f f); +void _z_vec_move(_z_vec_t *dst, _z_vec_t *src); size_t _z_vec_len(const _z_vec_t *v); bool _z_vec_is_empty(const _z_vec_t *v); @@ -63,6 +66,8 @@ void _z_vec_release(_z_vec_t *v); static inline void name##_vec_copy(name##_vec_t *dst, const name##_vec_t *src) { \ _z_vec_copy(dst, src, name##_elem_clone); \ } \ + static inline name##_vec_t name##_vec_alias(const name##_vec_t *v) { return _z_vec_alias(v); } \ + static inline void name##_vec_move(name##_vec_t *dst, name##_vec_t *src) { _z_vec_move(dst, src); } \ static inline void name##_vec_reset(name##_vec_t *v) { _z_vec_reset(v, name##_elem_free); } \ static inline void name##_vec_clear(name##_vec_t *v) { _z_vec_clear(v, name##_elem_free); } \ static inline void name##_vec_free(name##_vec_t **v) { _z_vec_free(v, name##_elem_free); } \ @@ -76,46 +81,82 @@ typedef struct { size_t _capacity; size_t _len; void *_val; + bool _aliased; } _z_svec_t; +static inline _z_svec_t _z_svec_null(void) { return (_z_svec_t){0}; } +static inline _z_svec_t _z_svec_alias(const _z_svec_t *src) { + _z_svec_t ret; + ret._capacity = src->_capacity; + ret._len = src->_len; + ret._val = src->_val; + ret._aliased = true; + return ret; +} +static inline _z_svec_t _z_svec_alias_element(void *element) { + _z_svec_t ret; + ret._capacity = 1; + ret._len = 1; + ret._val = element; + ret._aliased = true; + return ret; +} +void _z_svec_init(_z_svec_t *v, size_t offset, size_t element_size); _z_svec_t _z_svec_make(size_t capacity, size_t element_size); -bool _z_svec_copy(_z_svec_t *dst, const _z_svec_t *src, z_element_copy_f copy, size_t element_size); +z_result_t _z_svec_copy(_z_svec_t *dst, const _z_svec_t *src, z_element_copy_f copy, size_t element_size, + bool use_elem_f); +void _z_svec_move(_z_svec_t *dst, _z_svec_t *src); size_t _z_svec_len(const _z_svec_t *v); bool _z_svec_is_empty(const _z_svec_t *v); -bool _z_svec_append(_z_svec_t *v, const void *e, z_element_move_f m, size_t element_size); +z_result_t _z_svec_expand(_z_svec_t *v, z_element_move_f move, size_t element_size, bool use_elem_f); +z_result_t _z_svec_append(_z_svec_t *v, const void *e, z_element_move_f m, size_t element_size, bool use_elem_f); void *_z_svec_get(const _z_svec_t *v, size_t pos, size_t element_size); +void *_z_svec_get_mut(_z_svec_t *v, size_t i, size_t element_size); void _z_svec_set(_z_svec_t *sv, size_t pos, void *e, z_element_clear_f f, size_t element_size); -void _z_svec_remove(_z_svec_t *sv, size_t pos, z_element_clear_f f, z_element_move_f m, size_t element_size); +void _z_svec_remove(_z_svec_t *sv, size_t pos, z_element_clear_f f, z_element_move_f m, size_t element_size, + bool use_elem_f); void _z_svec_reset(_z_svec_t *v, z_element_clear_f f, size_t element_size); void _z_svec_clear(_z_svec_t *v, z_element_clear_f f, size_t element_size); void _z_svec_free(_z_svec_t **v, z_element_clear_f f, size_t element_size); void _z_svec_release(_z_svec_t *v); -#define _Z_SVEC_DEFINE(name, type) \ - typedef _z_svec_t name##_svec_t; \ - static inline name##_svec_t name##_svec_make(size_t capacity) { return _z_svec_make(capacity, sizeof(type)); } \ - static inline size_t name##_svec_len(const name##_svec_t *v) { return _z_svec_len(v); } \ - static inline bool name##_svec_is_empty(const name##_svec_t *v) { return _z_svec_is_empty(v); } \ - static inline bool name##_svec_append(name##_svec_t *v, type *e) { \ - return _z_svec_append(v, e, name##_elem_move, sizeof(type)); \ - } \ - static inline type *name##_svec_get(const name##_svec_t *v, size_t pos) { \ - return (type *)_z_svec_get(v, pos, sizeof(type)); \ - } \ - static inline void name##_svec_set(name##_svec_t *v, size_t pos, type *e) { \ - _z_svec_set(v, pos, e, name##_elem_clear, sizeof(type)); \ - } \ - static inline void name##_svec_remove(name##_svec_t *v, size_t pos) { \ - _z_svec_remove(v, pos, name##_elem_clear, name##_elem_move, sizeof(type)); \ - } \ - static inline bool name##_svec_copy(name##_svec_t *dst, const name##_svec_t *src) { \ - return _z_svec_copy(dst, src, name##_elem_copy, sizeof(type)); \ - } \ - static inline void name##_svec_reset(name##_svec_t *v) { _z_svec_reset(v, name##_elem_clear, sizeof(type)); } \ - static inline void name##_svec_clear(name##_svec_t *v) { _z_svec_clear(v, name##_elem_clear, sizeof(type)); } \ +#define _Z_SVEC_DEFINE(name, type) \ + typedef _z_svec_t name##_svec_t; \ + static inline name##_svec_t name##_svec_null(void) { return _z_svec_null(); } \ + static inline name##_svec_t name##_svec_make(size_t capacity) { return _z_svec_make(capacity, sizeof(type)); } \ + static inline void name##_svec_init(name##_svec_t *v, size_t offset) { _z_svec_init(v, offset, sizeof(type)); } \ + static inline size_t name##_svec_len(const name##_svec_t *v) { return _z_svec_len(v); } \ + static inline bool name##_svec_is_empty(const name##_svec_t *v) { return _z_svec_is_empty(v); } \ + static inline z_result_t name##_svec_expand(name##_svec_t *v, bool use_elem_f) { \ + return _z_svec_expand(v, name##_elem_move, sizeof(type), use_elem_f); \ + } \ + static inline z_result_t name##_svec_append(name##_svec_t *v, const type *e, bool use_elem_f) { \ + return _z_svec_append(v, e, name##_elem_move, sizeof(type), use_elem_f); \ + } \ + static inline type *name##_svec_get(const name##_svec_t *v, size_t pos) { \ + return (type *)_z_svec_get(v, pos, sizeof(type)); \ + } \ + static inline type *name##_svec_get_mut(name##_svec_t *v, size_t pos) { \ + return (type *)_z_svec_get_mut(v, pos, sizeof(type)); \ + } \ + static inline void name##_svec_set(name##_svec_t *v, size_t pos, type *e) { \ + _z_svec_set(v, pos, e, name##_elem_clear, sizeof(type)); \ + } \ + static inline void name##_svec_remove(name##_svec_t *v, size_t pos, bool use_elem_f) { \ + _z_svec_remove(v, pos, name##_elem_clear, name##_elem_move, sizeof(type), use_elem_f); \ + } \ + static inline z_result_t name##_svec_copy(name##_svec_t *dst, const name##_svec_t *src, bool use_elem_f) { \ + return _z_svec_copy(dst, src, name##_elem_copy, sizeof(type), use_elem_f); \ + } \ + static inline name##_svec_t name##_svec_alias(const name##_svec_t *v) { return _z_svec_alias(v); } \ + static inline name##_svec_t name##_svec_alias_element(type *e) { return _z_svec_alias_element((void *)e); } \ + static inline void name##_svec_move(name##_svec_t *dst, name##_svec_t *src) { _z_svec_move(dst, src); } \ + static inline void name##_svec_reset(name##_svec_t *v) { _z_svec_reset(v, name##_elem_clear, sizeof(type)); } \ + static inline void name##_svec_clear(name##_svec_t *v) { _z_svec_clear(v, name##_elem_clear, sizeof(type)); } \ + static inline void name##_svec_release(name##_svec_t *v) { _z_svec_release(v); } \ static inline void name##_svec_free(name##_svec_t **v) { _z_svec_free(v, name##_elem_clear, sizeof(type)); } #ifdef __cplusplus diff --git a/include/zenoh-pico/config.h b/include/zenoh-pico/config.h index 1578b1f50..840dff4ee 100644 --- a/include/zenoh-pico/config.h +++ b/include/zenoh-pico/config.h @@ -43,6 +43,10 @@ #define Z_FEATURE_FRAGMENTATION 1 #define Z_FEATURE_ENCODING_VALUES 1 #define Z_FEATURE_TCP_NODELAY 1 +#define Z_FEATURE_LOCAL_SUBSCRIBER 0 +#define Z_FEATURE_PUBLISHER_SESSION_CHECK 1 +#define Z_FEATURE_BATCHING 1 +#define Z_FEATURE_RX_CACHE 0 // End of CMake generation /*------------------ Runtime configuration properties ------------------*/ diff --git a/include/zenoh-pico/config.h.in b/include/zenoh-pico/config.h.in index 175fc8fdf..48929af42 100644 --- a/include/zenoh-pico/config.h.in +++ b/include/zenoh-pico/config.h.in @@ -43,6 +43,10 @@ #define Z_FEATURE_FRAGMENTATION @Z_FEATURE_FRAGMENTATION@ #define Z_FEATURE_ENCODING_VALUES @Z_FEATURE_ENCODING_VALUES@ #define Z_FEATURE_TCP_NODELAY @Z_FEATURE_TCP_NODELAY@ +#define Z_FEATURE_LOCAL_SUBSCRIBER @Z_FEATURE_LOCAL_SUBSCRIBER@ +#define Z_FEATURE_PUBLISHER_SESSION_CHECK @Z_FEATURE_PUBLISHER_SESSION_CHECK@ +#define Z_FEATURE_BATCHING @Z_FEATURE_BATCHING@ +#define Z_FEATURE_RX_CACHE @Z_FEATURE_RX_CACHE@ // End of CMake generation /*------------------ Runtime configuration properties ------------------*/ diff --git a/include/zenoh-pico/link/endpoint.h b/include/zenoh-pico/link/endpoint.h index d6b83e30a..8aa03390e 100644 --- a/include/zenoh-pico/link/endpoint.h +++ b/include/zenoh-pico/link/endpoint.h @@ -58,7 +58,7 @@ z_result_t _z_locator_from_string(_z_locator_t *lc, _z_string_t *s); size_t _z_locator_size(_z_locator_t *lc); void _z_locator_clear(_z_locator_t *lc); -_Z_ELEM_DEFINE(_z_locator, _z_locator_t, _z_locator_size, _z_locator_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_locator, _z_locator_t, _z_locator_size, _z_locator_clear, _z_noop_copy, _z_noop_move) /*------------------ Locator array ------------------*/ _Z_ARRAY_DEFINE(_z_locator, _z_locator_t) diff --git a/include/zenoh-pico/link/manager.h b/include/zenoh-pico/link/manager.h index 7a962e68f..43146ae01 100644 --- a/include/zenoh-pico/link/manager.h +++ b/include/zenoh-pico/link/manager.h @@ -23,10 +23,9 @@ extern "C" { #endif -#if Z_FEATURE_LINK_TCP == 1 z_result_t _z_endpoint_tcp_valid(_z_endpoint_t *ep); z_result_t _z_new_link_tcp(_z_link_t *zl, _z_endpoint_t *ep); -#endif + #if Z_FEATURE_LINK_UDP_UNICAST == 1 z_result_t _z_endpoint_udp_unicast_valid(_z_endpoint_t *ep); z_result_t _z_new_link_udp_unicast(_z_link_t *zl, _z_endpoint_t ep); diff --git a/include/zenoh-pico/net/encoding.h b/include/zenoh-pico/net/encoding.h index 0b35ce338..a100071b4 100644 --- a/include/zenoh-pico/net/encoding.h +++ b/include/zenoh-pico/net/encoding.h @@ -31,12 +31,16 @@ typedef struct _z_encoding_t { uint16_t id; } _z_encoding_t; -z_result_t _z_encoding_make(_z_encoding_t *encoding, uint16_t id, const char *schema, size_t len); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_encoding_t _z_encoding_null(void) { return (_z_encoding_t){0}; } +static inline bool _z_encoding_check(const _z_encoding_t *encoding) { + return ((encoding->id != _Z_ENCODING_ID_DEFAULT) || _z_string_check(&encoding->schema)); +} _z_encoding_t _z_encoding_wrap(uint16_t id, const char *schema); -_z_encoding_t _z_encoding_null(void); +z_result_t _z_encoding_make(_z_encoding_t *encoding, uint16_t id, const char *schema, size_t len); void _z_encoding_clear(_z_encoding_t *encoding); -bool _z_encoding_check(const _z_encoding_t *encoding); z_result_t _z_encoding_copy(_z_encoding_t *dst, const _z_encoding_t *src); +_z_encoding_t _z_encoding_alias(_z_encoding_t src); void _z_encoding_move(_z_encoding_t *dst, _z_encoding_t *src); _z_encoding_t _z_encoding_steal(_z_encoding_t *val); diff --git a/include/zenoh-pico/net/liveliness.h b/include/zenoh-pico/net/liveliness.h index 2f1be9e9d..3fb7c6ded 100644 --- a/include/zenoh-pico/net/liveliness.h +++ b/include/zenoh-pico/net/liveliness.h @@ -26,7 +26,7 @@ extern "C" { #if Z_FEATURE_LIVELINESS == 1 z_result_t _z_declare_liveliness_token(const _z_session_rc_t *zn, _z_liveliness_token_t *ret_token, - _z_keyexpr_t keyexpr); + _z_keyexpr_t *keyexpr); z_result_t _z_undeclare_liveliness_token(_z_liveliness_token_t *token); #if Z_FEATURE_SUBSCRIPTION == 1 @@ -43,7 +43,7 @@ z_result_t _z_undeclare_liveliness_token(_z_liveliness_token_t *token); * Returns: * The created :c:type:`_z_subscriber_t` (in null state if the declaration failed). */ -_z_subscriber_t _z_declare_liveliness_subscriber(const _z_session_rc_t *zn, _z_keyexpr_t keyexpr, +_z_subscriber_t _z_declare_liveliness_subscriber(const _z_session_rc_t *zn, _z_keyexpr_t *keyexpr, _z_closure_sample_callback_t callback, _z_drop_handler_t dropper, bool history, void *arg); @@ -71,7 +71,7 @@ z_result_t _z_undeclare_liveliness_subscriber(_z_subscriber_t *sub); * arg: A pointer that will be passed to the **callback** on each call. * timeout_ms: The timeout value of this query. */ -z_result_t _z_liveliness_query(_z_session_t *zn, _z_keyexpr_t keyexpr, _z_closure_reply_callback_t callback, +z_result_t _z_liveliness_query(_z_session_t *zn, const _z_keyexpr_t *keyexpr, _z_closure_reply_callback_t callback, _z_drop_handler_t dropper, void *arg, uint64_t timeout_ms); #endif // Z_FEATURE_QUERY == 1 diff --git a/include/zenoh-pico/net/publish.h b/include/zenoh-pico/net/publish.h index 2f29b0b05..b270f4eb3 100644 --- a/include/zenoh-pico/net/publish.h +++ b/include/zenoh-pico/net/publish.h @@ -41,10 +41,11 @@ typedef struct _z_publisher_t { } _z_publisher_t; #if Z_FEATURE_PUBLICATION == 1 +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_publisher_t _z_publisher_null(void) { return (_z_publisher_t){0}; } +static inline bool _z_publisher_check(const _z_publisher_t *publisher) { return !_Z_RC_IS_NULL(&publisher->_zn); } void _z_publisher_clear(_z_publisher_t *pub); void _z_publisher_free(_z_publisher_t **pub); -bool _z_publisher_check(const _z_publisher_t *publisher); -_z_publisher_t _z_publisher_null(void); #endif #ifdef __cplusplus diff --git a/include/zenoh-pico/net/query.h b/include/zenoh-pico/net/query.h index ed2c7579c..2a392b987 100644 --- a/include/zenoh-pico/net/query.h +++ b/include/zenoh-pico/net/query.h @@ -20,6 +20,7 @@ #include "zenoh-pico/collections/bytes.h" #include "zenoh-pico/net/session.h" #include "zenoh-pico/protocol/core.h" +#include "zenoh-pico/protocol/keyexpr.h" #ifdef __cplusplus extern "C" { @@ -29,18 +30,23 @@ extern "C" { * The query to be answered by a queryable. */ typedef struct _z_query_t { - _z_value_t _value; _z_keyexpr_t _key; + _z_value_t _value; uint32_t _request_id; - _z_session_weak_t _zn; // Can't be an rc because of cross referencing - _z_bytes_t attachment; - char *_parameters; + _z_session_weak_t _zn; + _z_bytes_t _attachment; + _z_string_t _parameters; bool _anyke; } _z_query_t; -_z_query_t _z_query_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_query_t _z_query_null(void) { return (_z_query_t){0}; } +static inline bool _z_query_check(const _z_query_t *query) { + return _z_keyexpr_check(&query->_key) || _z_value_check(&query->_value) || _z_bytes_check(&query->_attachment) || + _z_string_check(&query->_parameters); +} +z_result_t _z_query_send_reply_final(_z_query_t *q); void _z_query_clear(_z_query_t *q); -z_result_t _z_query_copy(_z_query_t *dst, const _z_query_t *src); void _z_query_free(_z_query_t **query); _Z_REFCOUNT_DEFINE(_z_query, _z_query) @@ -54,12 +60,25 @@ typedef struct { } _z_queryable_t; #if Z_FEATURE_QUERYABLE == 1 -_z_query_t _z_query_create(_z_value_t *value, _z_keyexpr_t *key, const _z_slice_t *parameters, _z_session_rc_t *zn, - uint32_t request_id, const _z_bytes_t attachment); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_queryable_t _z_queryable_null(void) { return (_z_queryable_t){0}; } +static inline bool _z_queryable_check(const _z_queryable_t *queryable) { return !_Z_RC_IS_NULL(&queryable->_zn); } +static inline _z_query_t _z_query_alias(_z_value_t *value, _z_keyexpr_t *key, const _z_slice_t *parameters, + _z_session_rc_t *zn, uint32_t request_id, const _z_bytes_t *attachment, + bool anyke) { + _z_query_t ret; + ret._key = _z_keyexpr_alias(*key); + ret._value = _z_value_alias(*value); + ret._request_id = request_id; + ret._zn = _z_session_rc_clone_as_weak(zn); + ret._attachment = _z_bytes_alias(*attachment); + ret._parameters = _z_string_alias_slice(parameters); + ret._anyke = anyke; + return ret; +} void _z_queryable_clear(_z_queryable_t *qbl); void _z_queryable_free(_z_queryable_t **qbl); -_z_queryable_t _z_queryable_null(void); -bool _z_queryable_check(const _z_queryable_t *queryable); + #endif #ifdef __cplusplus diff --git a/include/zenoh-pico/net/reply.h b/include/zenoh-pico/net/reply.h index df2a2742e..17008c14e 100644 --- a/include/zenoh-pico/net/reply.h +++ b/include/zenoh-pico/net/reply.h @@ -61,10 +61,12 @@ typedef struct _z_reply_data_t { _z_reply_tag_t _tag; } _z_reply_data_t; +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_reply_data_t _z_reply_data_null(void) { return (_z_reply_data_t){0}; } void _z_reply_data_clear(_z_reply_data_t *rd); z_result_t _z_reply_data_copy(_z_reply_data_t *dst, const _z_reply_data_t *src); -_Z_ELEM_DEFINE(_z_reply_data, _z_reply_data_t, _z_noop_size, _z_reply_data_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_reply_data, _z_reply_data_t, _z_noop_size, _z_reply_data_clear, _z_noop_copy, _z_noop_move) _Z_LIST_DEFINE(_z_reply_data, _z_reply_data_t) /** @@ -80,15 +82,29 @@ typedef struct _z_reply_t { _z_reply_data_t data; } _z_reply_t; +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_reply_t _z_reply_null(void) { return (_z_reply_t){0}; } +static inline _z_reply_t _z_reply_alias(_z_keyexpr_t *keyexpr, _z_id_t id, const _z_bytes_t *payload, + const _z_timestamp_t *timestamp, _z_encoding_t *encoding, z_sample_kind_t kind, + const _z_bytes_t *attachment) { + _z_reply_t r; + r.data.replier_id = id; + r.data._tag = _Z_REPLY_TAG_DATA; + r.data._result.sample = _z_sample_alias(keyexpr, payload, timestamp, encoding, kind, _Z_N_QOS_DEFAULT, attachment, + Z_RELIABILITY_DEFAULT); + return r; +} +static inline _z_reply_t _z_reply_err_alias(const _z_bytes_t *payload, _z_encoding_t *encoding) { + _z_reply_t r; + r.data._tag = _Z_REPLY_TAG_ERROR; + r.data._result.error.payload = *payload; + r.data._result.error.encoding = *encoding; + return r; +} _z_reply_t _z_reply_move(_z_reply_t *src_reply); - -_z_reply_t _z_reply_null(void); void _z_reply_clear(_z_reply_t *src); void _z_reply_free(_z_reply_t **hello); z_result_t _z_reply_copy(_z_reply_t *dst, const _z_reply_t *src); -_z_reply_t _z_reply_create(_z_keyexpr_t keyexpr, _z_id_t id, const _z_bytes_t payload, const _z_timestamp_t *timestamp, - _z_encoding_t *encoding, z_sample_kind_t kind, const _z_bytes_t attachment); -_z_reply_t _z_reply_err_create(const _z_bytes_t payload, _z_encoding_t *encoding); typedef struct _z_pending_reply_t { _z_reply_t _reply; @@ -98,7 +114,7 @@ typedef struct _z_pending_reply_t { bool _z_pending_reply_eq(const _z_pending_reply_t *one, const _z_pending_reply_t *two); void _z_pending_reply_clear(_z_pending_reply_t *res); -_Z_ELEM_DEFINE(_z_pending_reply, _z_pending_reply_t, _z_noop_size, _z_pending_reply_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_pending_reply, _z_pending_reply_t, _z_noop_size, _z_pending_reply_clear, _z_noop_copy, _z_noop_move) _Z_LIST_DEFINE(_z_pending_reply, _z_pending_reply_t) #ifdef __cplusplus diff --git a/include/zenoh-pico/net/sample.h b/include/zenoh-pico/net/sample.h index 3133c9706..ec05a6de8 100644 --- a/include/zenoh-pico/net/sample.h +++ b/include/zenoh-pico/net/sample.h @@ -44,8 +44,27 @@ typedef struct _z_sample_t { } _z_sample_t; void _z_sample_clear(_z_sample_t *sample); -_z_sample_t _z_sample_null(void); -bool _z_sample_check(const _z_sample_t *sample); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_sample_t _z_sample_null(void) { return (_z_sample_t){0}; } +static inline bool _z_sample_check(const _z_sample_t *sample) { + return _z_keyexpr_check(&sample->keyexpr) || _z_encoding_check(&sample->encoding) || + _z_bytes_check(&sample->payload) || _z_bytes_check(&sample->attachment); +} +static inline _z_sample_t _z_sample_alias(const _z_keyexpr_t *key, const _z_bytes_t *payload, + const _z_timestamp_t *timestamp, const _z_encoding_t *encoding, + const z_sample_kind_t kind, const _z_qos_t qos, const _z_bytes_t *attachment, + z_reliability_t reliability) { + _z_sample_t ret; + ret.keyexpr = *key; + ret.payload = *payload; + ret.timestamp = *timestamp; + ret.encoding = *encoding; + ret.kind = kind; + ret.qos = qos; + ret.attachment = *attachment; + ret.reliability = reliability; + return ret; +} void _z_sample_move(_z_sample_t *dst, _z_sample_t *src); /** @@ -59,11 +78,7 @@ void _z_sample_free(_z_sample_t **sample); z_result_t _z_sample_copy(_z_sample_t *dst, const _z_sample_t *src); _z_sample_t _z_sample_duplicate(const _z_sample_t *src); -_z_sample_t _z_sample_create(_z_keyexpr_t *key, const _z_bytes_t payload, const _z_timestamp_t *timestamp, - _z_encoding_t *encoding, const z_sample_kind_t kind, const _z_qos_t qos, - const _z_bytes_t attachment, z_reliability_t reliability); #ifdef __cplusplus } #endif - #endif /* ZENOH_PICO_SAMPLE_NETAPI_H */ diff --git a/include/zenoh-pico/net/session.h b/include/zenoh-pico/net/session.h index ba43ca095..5ee1556dc 100644 --- a/include/zenoh-pico/net/session.h +++ b/include/zenoh-pico/net/session.h @@ -22,7 +22,9 @@ #include "zenoh-pico/config.h" #include "zenoh-pico/protocol/core.h" #include "zenoh-pico/session/liveliness.h" +#include "zenoh-pico/session/queryable.h" #include "zenoh-pico/session/session.h" +#include "zenoh-pico/session/subscription.h" #include "zenoh-pico/utils/config.h" #ifdef __cplusplus @@ -57,6 +59,9 @@ typedef struct _z_session_t { #if Z_FEATURE_SUBSCRIPTION == 1 _z_subscription_rc_list_t *_subscriptions; _z_subscription_rc_list_t *_liveliness_subscriptions; +#if Z_FEATURE_RX_CACHE == 1 + _z_subscription_cache_t _subscription_cache; +#endif #endif #if Z_FEATURE_LIVELINESS == 1 @@ -71,6 +76,9 @@ typedef struct _z_session_t { // Session queryables #if Z_FEATURE_QUERYABLE == 1 _z_session_queryable_rc_list_t *_local_queryable; +#if Z_FEATURE_RX_CACHE == 1 + _z_queryable_cache_t _queryable_cache; +#endif #endif #if Z_FEATURE_QUERY == 1 _z_pending_query_list_t *_pending_queries; diff --git a/include/zenoh-pico/net/subscribe.h b/include/zenoh-pico/net/subscribe.h index ae9fad18f..2357a0c8a 100644 --- a/include/zenoh-pico/net/subscribe.h +++ b/include/zenoh-pico/net/subscribe.h @@ -33,11 +33,12 @@ typedef struct { } _z_subscriber_t; #if Z_FEATURE_SUBSCRIPTION == 1 - +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_subscriber_t _z_subscriber_null(void) { return (_z_subscriber_t){0}; } +static inline bool _z_subscriber_check(const _z_subscriber_t *subscriber) { return !_Z_RC_IS_NULL(&subscriber->_zn); } void _z_subscriber_clear(_z_subscriber_t *sub); void _z_subscriber_free(_z_subscriber_t **sub); -bool _z_subscriber_check(const _z_subscriber_t *subscriber); -_z_subscriber_t _z_subscriber_null(void); + #endif #ifdef __cplusplus diff --git a/include/zenoh-pico/protocol/codec/core.h b/include/zenoh-pico/protocol/codec/core.h index 00eec6890..db0acee39 100644 --- a/include/zenoh-pico/protocol/codec/core.h +++ b/include/zenoh-pico/protocol/codec/core.h @@ -65,7 +65,7 @@ z_result_t _z_slice_val_decode_na(_z_slice_t *bs, _z_zbuf_t *zbf); z_result_t _z_slice_encode(_z_wbuf_t *buf, const _z_slice_t *bs); z_result_t _z_slice_decode(_z_slice_t *bs, _z_zbuf_t *buf); -z_result_t _z_bytes_decode(_z_bytes_t *bs, _z_zbuf_t *zbf); +z_result_t _z_bytes_decode(_z_bytes_t *bs, _z_zbuf_t *zbf, _z_arc_slice_t *arcs); z_result_t _z_bytes_encode(_z_wbuf_t *wbf, const _z_bytes_t *bs); z_result_t _z_zbuf_read_exact(_z_zbuf_t *zbf, uint8_t *dest, size_t length); diff --git a/include/zenoh-pico/protocol/codec/message.h b/include/zenoh-pico/protocol/codec/message.h index ae4b88567..c877ea884 100644 --- a/include/zenoh-pico/protocol/codec/message.h +++ b/include/zenoh-pico/protocol/codec/message.h @@ -23,19 +23,19 @@ extern "C" { #endif z_result_t _z_push_body_encode(_z_wbuf_t *wbf, const _z_push_body_t *pshb); -z_result_t _z_push_body_decode(_z_push_body_t *body, _z_zbuf_t *zbf, uint8_t header); +z_result_t _z_push_body_decode(_z_push_body_t *body, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs); z_result_t _z_query_encode(_z_wbuf_t *wbf, const _z_msg_query_t *query); z_result_t _z_query_decode(_z_msg_query_t *query, _z_zbuf_t *zbf, uint8_t header); z_result_t _z_reply_encode(_z_wbuf_t *wbf, const _z_msg_reply_t *reply); -z_result_t _z_reply_decode(_z_msg_reply_t *reply, _z_zbuf_t *zbf, uint8_t header); +z_result_t _z_reply_decode(_z_msg_reply_t *reply, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs); z_result_t _z_err_encode(_z_wbuf_t *wbf, const _z_msg_err_t *err); -z_result_t _z_err_decode(_z_msg_err_t *err, _z_zbuf_t *zbf, uint8_t header); +z_result_t _z_err_decode(_z_msg_err_t *err, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs); z_result_t _z_put_encode(_z_wbuf_t *wbf, const _z_msg_put_t *put); -z_result_t _z_put_decode(_z_msg_put_t *put, _z_zbuf_t *zbf, uint8_t header); +z_result_t _z_put_decode(_z_msg_put_t *put, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs); z_result_t _z_del_encode(_z_wbuf_t *wbf, const _z_msg_del_t *del); z_result_t _z_del_decode(_z_msg_del_t *del, _z_zbuf_t *zbf, uint8_t header); diff --git a/include/zenoh-pico/protocol/codec/network.h b/include/zenoh-pico/protocol/codec/network.h index e74322eb0..661ee9de2 100644 --- a/include/zenoh-pico/protocol/codec/network.h +++ b/include/zenoh-pico/protocol/codec/network.h @@ -25,11 +25,11 @@ extern "C" { #endif z_result_t _z_push_encode(_z_wbuf_t *wbf, const _z_n_msg_push_t *msg); -z_result_t _z_push_decode(_z_n_msg_push_t *msg, _z_zbuf_t *zbf, uint8_t header); +z_result_t _z_push_decode(_z_n_msg_push_t *msg, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs); z_result_t _z_request_encode(_z_wbuf_t *wbf, const _z_n_msg_request_t *msg); -z_result_t _z_request_decode(_z_n_msg_request_t *msg, _z_zbuf_t *zbf, uint8_t header); +z_result_t _z_request_decode(_z_n_msg_request_t *msg, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs); z_result_t _z_response_encode(_z_wbuf_t *wbf, const _z_n_msg_response_t *msg); -z_result_t _z_response_decode(_z_n_msg_response_t *msg, _z_zbuf_t *zbf, uint8_t header); +z_result_t _z_response_decode(_z_n_msg_response_t *msg, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs); z_result_t _z_response_final_encode(_z_wbuf_t *wbf, const _z_n_msg_response_final_t *msg); z_result_t _z_response_final_decode(_z_n_msg_response_final_t *msg, _z_zbuf_t *zbf, uint8_t header); z_result_t _z_declare_encode(_z_wbuf_t *wbf, const _z_n_msg_declare_t *decl); @@ -38,7 +38,7 @@ z_result_t _z_n_interest_encode(_z_wbuf_t *wbf, const _z_n_msg_interest_t *inter z_result_t _z_n_interest_decode(_z_n_msg_interest_t *interest, _z_zbuf_t *zbf, uint8_t header); z_result_t _z_network_message_encode(_z_wbuf_t *wbf, const _z_network_message_t *msg); -z_result_t _z_network_message_decode(_z_network_message_t *msg, _z_zbuf_t *zbf); +z_result_t _z_network_message_decode(_z_network_message_t *msg, _z_zbuf_t *zbf, _z_arc_slice_t *arcs); #ifdef __cplusplus } diff --git a/include/zenoh-pico/protocol/codec/transport.h b/include/zenoh-pico/protocol/codec/transport.h index d7b4320a5..eee9feb47 100644 --- a/include/zenoh-pico/protocol/codec/transport.h +++ b/include/zenoh-pico/protocol/codec/transport.h @@ -22,13 +22,12 @@ extern "C" { #endif -#define _ZENOH_PICO_FRAME_MESSAGES_VEC_SIZE 32 - z_result_t _z_scouting_message_encode(_z_wbuf_t *buf, const _z_scouting_message_t *msg); z_result_t _z_scouting_message_decode(_z_scouting_message_t *msg, _z_zbuf_t *buf); z_result_t _z_transport_message_encode(_z_wbuf_t *buf, const _z_transport_message_t *msg); -z_result_t _z_transport_message_decode(_z_transport_message_t *msg, _z_zbuf_t *buf); +z_result_t _z_transport_message_decode(_z_transport_message_t *msg, _z_zbuf_t *buf, _z_arc_slice_svec_t *arc_pool, + _z_network_message_svec_t *msg_pool); z_result_t _z_join_encode(_z_wbuf_t *wbf, uint8_t header, const _z_t_msg_join_t *msg); z_result_t _z_join_decode(_z_t_msg_join_t *msg, _z_zbuf_t *zbf, uint8_t header); @@ -46,14 +45,12 @@ z_result_t _z_keep_alive_encode(_z_wbuf_t *wbf, uint8_t header, const _z_t_msg_k z_result_t _z_keep_alive_decode(_z_t_msg_keep_alive_t *msg, _z_zbuf_t *zbf, uint8_t header); z_result_t _z_frame_encode(_z_wbuf_t *wbf, uint8_t header, const _z_t_msg_frame_t *msg); -z_result_t _z_frame_decode(_z_t_msg_frame_t *msg, _z_zbuf_t *zbf, uint8_t header); +z_result_t _z_frame_decode(_z_t_msg_frame_t *msg, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_svec_t *arc_pool, + _z_network_message_svec_t *msg_pool); z_result_t _z_fragment_encode(_z_wbuf_t *wbf, uint8_t header, const _z_t_msg_fragment_t *msg); z_result_t _z_fragment_decode(_z_t_msg_fragment_t *msg, _z_zbuf_t *zbf, uint8_t header); -z_result_t _z_transport_message_encode(_z_wbuf_t *wbf, const _z_transport_message_t *msg); -z_result_t _z_transport_message_decode(_z_transport_message_t *msg, _z_zbuf_t *zbf); - #ifdef __cplusplus } #endif diff --git a/include/zenoh-pico/protocol/core.h b/include/zenoh-pico/protocol/core.h index 40dd8355e..0d98b5b4f 100644 --- a/include/zenoh-pico/protocol/core.h +++ b/include/zenoh-pico/protocol/core.h @@ -58,22 +58,28 @@ typedef size_t _z_zint_t; typedef struct { uint8_t id[16]; } _z_id_t; +extern const _z_id_t empty_id; uint8_t _z_id_len(_z_id_t id); -bool _z_id_check(_z_id_t id); -_z_id_t _z_id_empty(void); +static inline bool _z_id_check(_z_id_t id) { return memcmp(&id, &empty_id, sizeof(id)) != 0; } +static inline _z_id_t _z_id_empty(void) { return (_z_id_t){0}; } /** * A zenoh timestamp. */ typedef struct { + bool valid; _z_id_t id; uint64_t time; } _z_timestamp_t; +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_timestamp_t _z_timestamp_null(void) { return (_z_timestamp_t){0}; } +static inline void _z_timestamp_invalid(_z_timestamp_t *tstamp) { tstamp->valid = false; } +static inline bool _z_timestamp_check(const _z_timestamp_t *tstamp) { return tstamp->valid; } +void _z_timestamp_copy(_z_timestamp_t *dst, const _z_timestamp_t *src); _z_timestamp_t _z_timestamp_duplicate(const _z_timestamp_t *tstamp); -_z_timestamp_t _z_timestamp_null(void); void _z_timestamp_clear(_z_timestamp_t *tstamp); -bool _z_timestamp_check(const _z_timestamp_t *stamp); +void _z_timestamp_move(_z_timestamp_t *dst, _z_timestamp_t *src); uint64_t _z_timestamp_ntp64_from_time(uint32_t seconds, uint32_t nanos); /** @@ -167,9 +173,15 @@ typedef struct { _z_bytes_t payload; _z_encoding_t encoding; } _z_value_t; -_z_value_t _z_value_null(void); + +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_value_t _z_value_null(void) { return (_z_value_t){0}; } +static inline bool _z_value_check(const _z_value_t *value) { + return _z_bytes_check(&value->payload) || _z_encoding_check(&value->encoding); +} _z_value_t _z_value_steal(_z_value_t *value); z_result_t _z_value_copy(_z_value_t *dst, const _z_value_t *src); +_z_value_t _z_value_alias(_z_value_t src); void _z_value_move(_z_value_t *dst, _z_value_t *src); void _z_value_clear(_z_value_t *src); void _z_value_free(_z_value_t **hello); @@ -188,13 +200,16 @@ typedef struct { z_whatami_t _whatami; uint8_t _version; } _z_hello_t; + +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_hello_t _z_hello_null(void) { return (_z_hello_t){0}; } void _z_hello_clear(_z_hello_t *src); void _z_hello_free(_z_hello_t **hello); z_result_t _z_hello_copy(_z_hello_t *dst, const _z_hello_t *src); -_z_hello_t _z_hello_null(void); + bool _z_hello_check(const _z_hello_t *hello); -_Z_ELEM_DEFINE(_z_hello, _z_hello_t, _z_noop_size, _z_hello_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_hello, _z_hello_t, _z_noop_size, _z_hello_clear, _z_noop_copy, _z_noop_move) _Z_LIST_DEFINE(_z_hello, _z_hello_t) typedef struct { @@ -206,7 +221,9 @@ typedef struct { uint32_t _entity_id; uint32_t _source_sn; } _z_source_info_t; -_z_source_info_t _z_source_info_null(void); + +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_source_info_t _z_source_info_null(void) { return (_z_source_info_t){0}; } typedef struct { uint32_t _request_id; diff --git a/include/zenoh-pico/protocol/definitions/declarations.h b/include/zenoh-pico/protocol/definitions/declarations.h index e6f715722..35c5e3a8e 100644 --- a/include/zenoh-pico/protocol/definitions/declarations.h +++ b/include/zenoh-pico/protocol/definitions/declarations.h @@ -28,22 +28,26 @@ typedef struct { uint16_t _id; _z_keyexpr_t _keyexpr; } _z_decl_kexpr_t; -_z_decl_kexpr_t _z_decl_kexpr_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_decl_kexpr_t _z_decl_kexpr_null(void) { return (_z_decl_kexpr_t){0}; } typedef struct { uint16_t _id; } _z_undecl_kexpr_t; -_z_undecl_kexpr_t _z_undecl_kexpr_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_undecl_kexpr_t _z_undecl_kexpr_null(void) { return (_z_undecl_kexpr_t){0}; } typedef struct { _z_keyexpr_t _keyexpr; uint32_t _id; } _z_decl_subscriber_t; -_z_decl_subscriber_t _z_decl_subscriber_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_decl_subscriber_t _z_decl_subscriber_null(void) { return (_z_decl_subscriber_t){0}; } typedef struct { uint32_t _id; _z_keyexpr_t _ext_keyexpr; } _z_undecl_subscriber_t; -_z_undecl_subscriber_t _z_undecl_subscriber_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_undecl_subscriber_t _z_undecl_subscriber_null(void) { return (_z_undecl_subscriber_t){0}; } typedef struct { _z_keyexpr_t _keyexpr; @@ -53,28 +57,33 @@ typedef struct { uint16_t _distance; } _ext_queryable_info; } _z_decl_queryable_t; -_z_decl_queryable_t _z_decl_queryable_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_decl_queryable_t _z_decl_queryable_null(void) { return (_z_decl_queryable_t){0}; } typedef struct { uint32_t _id; _z_keyexpr_t _ext_keyexpr; } _z_undecl_queryable_t; -_z_undecl_queryable_t _z_undecl_queryable_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_undecl_queryable_t _z_undecl_queryable_null(void) { return (_z_undecl_queryable_t){0}; } typedef struct { _z_keyexpr_t _keyexpr; uint32_t _id; } _z_decl_token_t; -_z_decl_token_t _z_decl_token_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_decl_token_t _z_decl_token_null(void) { return (_z_decl_token_t){0}; } typedef struct { uint32_t _id; _z_keyexpr_t _ext_keyexpr; } _z_undecl_token_t; -_z_undecl_token_t _z_undecl_token_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_undecl_token_t _z_undecl_token_null(void) { return (_z_undecl_token_t){0}; } typedef struct { bool _placeholder; // In case we add extensions } _z_decl_final_t; -_z_decl_final_t _z_decl_final_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_decl_final_t _z_decl_final_null(void) { return (_z_decl_final_t){0}; } typedef struct { enum { diff --git a/include/zenoh-pico/protocol/definitions/interest.h b/include/zenoh-pico/protocol/definitions/interest.h index 7e0e849ae..46a9ac7a3 100644 --- a/include/zenoh-pico/protocol/definitions/interest.h +++ b/include/zenoh-pico/protocol/definitions/interest.h @@ -40,10 +40,10 @@ typedef struct { uint32_t _id; uint8_t flags; } _z_interest_t; -_z_interest_t _z_interest_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_interest_t _z_interest_null(void) { return (_z_interest_t){0}; } void _z_interest_clear(_z_interest_t* decl); - _z_interest_t _z_make_interest(_Z_MOVE(_z_keyexpr_t) key, uint32_t id, uint8_t flags); _z_interest_t _z_make_interest_final(uint32_t id); diff --git a/include/zenoh-pico/protocol/definitions/network.h b/include/zenoh-pico/protocol/definitions/network.h index 489c996b3..8895b77ed 100644 --- a/include/zenoh-pico/protocol/definitions/network.h +++ b/include/zenoh-pico/protocol/definitions/network.h @@ -87,16 +87,23 @@ static inline _z_qos_t _z_n_qos_create(bool express, z_congestion_control_t cong return ret; } static inline z_priority_t _z_n_qos_get_priority(_z_n_qos_t n_qos) { - return (z_priority_t)(n_qos._val & 0x07 /* 0b111 */); + z_priority_t ret = (z_priority_t)(n_qos._val & 0x07); // 0b0111 + return ret; } static inline z_congestion_control_t _z_n_qos_get_congestion_control(_z_n_qos_t n_qos) { - return (n_qos._val & 0x08 /* 0b1000 */) ? Z_CONGESTION_CONTROL_BLOCK : Z_CONGESTION_CONTROL_DROP; + z_congestion_control_t ret = + (n_qos._val & 0x08) ? Z_CONGESTION_CONTROL_BLOCK : Z_CONGESTION_CONTROL_DROP; // 0b1000 + return ret; +} +static inline bool _z_n_qos_get_express(_z_n_qos_t n_qos) { + bool ret = (n_qos._val & 0x10) != 0; // 0b10000 + return ret; } -static inline bool _z_n_qos_get_express(_z_n_qos_t n_qos) { return (bool)(n_qos._val & 0x10 /* 0b10000 */); } #define _z_n_qos_make(express, nodrop, priority) \ _z_n_qos_create((bool)express, nodrop ? Z_CONGESTION_CONTROL_BLOCK : Z_CONGESTION_CONTROL_DROP, \ (z_priority_t)priority) -#define _Z_N_QOS_DEFAULT _z_n_qos_make(0, 0, 5) + +extern const _z_qos_t _Z_N_QOS_DEFAULT; // RESPONSE FINAL message flags: // Z Extensions if Z==1 then Zenoh extensions are present @@ -154,7 +161,9 @@ _z_n_msg_request_exts_t _z_n_msg_request_needed_exts(const _z_n_msg_request_t *m void _z_n_msg_request_clear(_z_n_msg_request_t *msg); typedef _z_reply_body_t _z_push_body_t; -_z_push_body_t _z_push_body_null(void); +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_push_body_t _z_push_body_null(void) { return (_z_push_body_t){0}; } + _z_push_body_t _z_push_body_steal(_z_push_body_t *msg); void _z_push_body_clear(_z_push_body_t *msg); @@ -292,8 +301,8 @@ void _z_n_msg_clear(_z_network_message_t *m); void _z_n_msg_free(_z_network_message_t **m); inline static void _z_msg_clear(_z_zenoh_message_t *msg) { _z_n_msg_clear(msg); } inline static void _z_msg_free(_z_zenoh_message_t **msg) { _z_n_msg_free(msg); } -_Z_ELEM_DEFINE(_z_network_message, _z_network_message_t, _z_noop_size, _z_n_msg_clear, _z_noop_copy) -_Z_VEC_DEFINE(_z_network_message, _z_network_message_t) +_Z_ELEM_DEFINE(_z_network_message, _z_network_message_t, _z_noop_size, _z_n_msg_clear, _z_noop_copy, _z_noop_move) +_Z_SVEC_DEFINE(_z_network_message, _z_network_message_t) void _z_msg_fix_mapping(_z_zenoh_message_t *msg, uint16_t mapping); _z_network_message_t _z_msg_make_query(_Z_MOVE(_z_keyexpr_t) key, _Z_MOVE(_z_slice_t) parameters, _z_zint_t qid, @@ -305,6 +314,7 @@ _z_network_message_t _z_n_msg_make_response_final(_z_zint_t rid); _z_network_message_t _z_n_msg_make_declare(_z_declaration_t declaration, bool has_interest_id, uint32_t interest_id); _z_network_message_t _z_n_msg_make_push(_Z_MOVE(_z_keyexpr_t) key, _Z_MOVE(_z_push_body_t) body); _z_network_message_t _z_n_msg_make_interest(_z_interest_t interest); +z_result_t _z_n_msg_copy(_z_network_message_t *dst, const _z_network_message_t *src); #ifdef __cplusplus } diff --git a/include/zenoh-pico/protocol/definitions/transport.h b/include/zenoh-pico/protocol/definitions/transport.h index 1320de7d3..7e4414681 100644 --- a/include/zenoh-pico/protocol/definitions/transport.h +++ b/include/zenoh-pico/protocol/definitions/transport.h @@ -464,7 +464,7 @@ void _z_t_msg_keep_alive_clear(_z_t_msg_keep_alive_t *msg); // - if R==1 then the FRAME is sent on the reliable channel, best-effort otherwise. // typedef struct { - _z_network_message_vec_t _messages; + _z_network_message_svec_t _messages; _z_zint_t _sn; } _z_t_msg_frame_t; void _z_t_msg_frame_clear(_z_t_msg_frame_t *msg); @@ -527,7 +527,7 @@ _z_transport_message_t _z_t_msg_make_open_syn(_z_zint_t lease, _z_zint_t initial _z_transport_message_t _z_t_msg_make_open_ack(_z_zint_t lease, _z_zint_t initial_sn); _z_transport_message_t _z_t_msg_make_close(uint8_t reason, bool link_only); _z_transport_message_t _z_t_msg_make_keep_alive(void); -_z_transport_message_t _z_t_msg_make_frame(_z_zint_t sn, _z_network_message_vec_t messages, +_z_transport_message_t _z_t_msg_make_frame(_z_zint_t sn, _z_network_message_svec_t messages, z_reliability_t reliability); _z_transport_message_t _z_t_msg_make_frame_header(_z_zint_t sn, z_reliability_t reliability); _z_transport_message_t _z_t_msg_make_fragment_header(_z_zint_t sn, z_reliability_t reliability, bool is_last, diff --git a/include/zenoh-pico/protocol/ext.h b/include/zenoh-pico/protocol/ext.h index a8afd0029..2a8daf574 100644 --- a/include/zenoh-pico/protocol/ext.h +++ b/include/zenoh-pico/protocol/ext.h @@ -105,7 +105,7 @@ void _z_msg_ext_copy_unit(_z_msg_ext_unit_t *clone, const _z_msg_ext_unit_t *ext void _z_msg_ext_copy_zint(_z_msg_ext_zint_t *clone, const _z_msg_ext_zint_t *ext); void _z_msg_ext_copy_zbuf(_z_msg_ext_zbuf_t *clone, const _z_msg_ext_zbuf_t *ext); -_Z_ELEM_DEFINE(_z_msg_ext, _z_msg_ext_t, _z_noop_size, _z_msg_ext_clear, _z_msg_ext_copy) +_Z_ELEM_DEFINE(_z_msg_ext, _z_msg_ext_t, _z_noop_size, _z_msg_ext_clear, _z_msg_ext_copy, _z_noop_move) _Z_VEC_DEFINE(_z_msg_ext, _z_msg_ext_t) #ifdef __cplusplus diff --git a/include/zenoh-pico/protocol/iobuf.h b/include/zenoh-pico/protocol/iobuf.h index abd5b1a36..bea659d34 100644 --- a/include/zenoh-pico/protocol/iobuf.h +++ b/include/zenoh-pico/protocol/iobuf.h @@ -19,6 +19,7 @@ #include #include +#include "zenoh-pico/collections/arc_slice.h" #include "zenoh-pico/collections/element.h" #include "zenoh-pico/collections/slice.h" #include "zenoh-pico/collections/vec.h" @@ -37,13 +38,16 @@ typedef struct { bool _is_alloc; } _z_iosli_t; +static inline _z_iosli_t _z_iosli_null(void) { return (_z_iosli_t){0}; } _z_iosli_t _z_iosli_make(size_t capacity); _z_iosli_t *_z_iosli_new(size_t capacity); _z_iosli_t _z_iosli_wrap(const uint8_t *buf, size_t length, size_t r_pos, size_t w_pos); +_z_iosli_t _z_iosli_steal(_z_iosli_t *ios); size_t _z_iosli_readable(const _z_iosli_t *ios); uint8_t _z_iosli_read(_z_iosli_t *ios); void _z_iosli_read_bytes(_z_iosli_t *ios, uint8_t *dest, size_t offset, size_t length); +void _z_iosli_copy_bytes(_z_iosli_t *dst, const _z_iosli_t *src); uint8_t _z_iosli_get(const _z_iosli_t *ios, size_t pos); size_t _z_iosli_writable(const _z_iosli_t *ios); @@ -60,14 +64,17 @@ void _z_iosli_free(_z_iosli_t **ios); void _z_iosli_copy(_z_iosli_t *dst, const _z_iosli_t *src); _z_iosli_t *_z_iosli_clone(const _z_iosli_t *src); -_Z_ELEM_DEFINE(_z_iosli, _z_iosli_t, _z_iosli_size, _z_iosli_clear, _z_iosli_copy) +_Z_ELEM_DEFINE(_z_iosli, _z_iosli_t, _z_iosli_size, _z_iosli_clear, _z_iosli_copy, _z_noop_move) _Z_VEC_DEFINE(_z_iosli, _z_iosli_t) /*------------------ ZBuf ------------------*/ typedef struct { _z_iosli_t _ios; + _z_slice_simple_rc_t _slice; } _z_zbuf_t; +static inline size_t _z_zbuf_get_ref_count(const _z_zbuf_t *zbf) { return _z_slice_simple_rc_count(&zbf->_slice); } +static inline _z_zbuf_t _z_zbuf_null(void) { return (_z_zbuf_t){0}; } _z_zbuf_t _z_zbuf_make(size_t capacity); _z_zbuf_t _z_zbuf_view(_z_zbuf_t *zbf, size_t length); /// Constructs a _borrowing_ reader on `slice` @@ -76,6 +83,7 @@ _z_zbuf_t _z_slice_as_zbuf(_z_slice_t slice); size_t _z_zbuf_capacity(const _z_zbuf_t *zbf); uint8_t const *_z_zbuf_start(const _z_zbuf_t *zbf); size_t _z_zbuf_len(const _z_zbuf_t *zbf); +void _z_zbuf_copy_bytes(_z_zbuf_t *dst, const _z_zbuf_t *src); bool _z_zbuf_can_read(const _z_zbuf_t *zbf); size_t _z_zbuf_space_left(const _z_zbuf_t *zbf); @@ -105,6 +113,7 @@ typedef struct { size_t _expansion_step; } _z_wbuf_t; +static inline _z_wbuf_t _z_wbuf_null(void) { return (_z_wbuf_t){0}; } _z_wbuf_t _z_wbuf_make(size_t capacity, bool is_expandable); size_t _z_wbuf_capacity(const _z_wbuf_t *wbf); @@ -126,6 +135,7 @@ _z_iosli_t *_z_wbuf_get_iosli(const _z_wbuf_t *wbf, size_t idx); size_t _z_wbuf_len_iosli(const _z_wbuf_t *wbf); _z_zbuf_t _z_wbuf_to_zbuf(const _z_wbuf_t *wbf); +_z_zbuf_t _z_wbuf_moved_as_zbuf(_z_wbuf_t *wbf); z_result_t _z_wbuf_siphon(_z_wbuf_t *dst, _z_wbuf_t *src, size_t length); void _z_wbuf_copy(_z_wbuf_t *dst, const _z_wbuf_t *src); diff --git a/include/zenoh-pico/protocol/keyexpr.h b/include/zenoh-pico/protocol/keyexpr.h index 8d790f669..f6126a06d 100644 --- a/include/zenoh-pico/protocol/keyexpr.h +++ b/include/zenoh-pico/protocol/keyexpr.h @@ -30,11 +30,21 @@ bool _z_keyexpr_suffix_intersects(const _z_keyexpr_t *left, const _z_keyexpr_t * bool _z_keyexpr_suffix_equals(const _z_keyexpr_t *left, const _z_keyexpr_t *right); /*------------------ clone/Copy/Free helpers ------------------*/ +// Warning: None of the sub-types require a non-0 initialization. Add a init function if it changes. +static inline _z_keyexpr_t _z_keyexpr_null(void) { return (_z_keyexpr_t){0}; } +static inline _z_keyexpr_t _z_keyexpr_alias(const _z_keyexpr_t src) { + _z_keyexpr_t ret; + ret._id = src._id; + ret._mapping = src._mapping; + ret._suffix = _z_string_alias(src._suffix); + return ret; +} + _z_keyexpr_t _z_keyexpr_from_string(uint16_t rid, _z_string_t *str); _z_keyexpr_t _z_keyexpr_from_substr(uint16_t rid, const char *str, size_t len); size_t _z_keyexpr_size(_z_keyexpr_t *p); z_result_t _z_keyexpr_copy(_z_keyexpr_t *dst, const _z_keyexpr_t *src); -_z_keyexpr_t _z_keyexpr_duplicate(_z_keyexpr_t src); +_z_keyexpr_t _z_keyexpr_duplicate(const _z_keyexpr_t *src); _z_keyexpr_t *_z_keyexpr_clone(const _z_keyexpr_t *src); _z_keyexpr_t _z_keyexpr_alias(_z_keyexpr_t src); /// Returns either keyexpr defined by id + mapping with null suffix if try_declared is true and id is non-zero, @@ -42,10 +52,6 @@ _z_keyexpr_t _z_keyexpr_alias(_z_keyexpr_t src); /// keyexpr in user api to properly separate declared keyexpr from its suffix. _z_keyexpr_t _z_keyexpr_alias_from_user_defined(_z_keyexpr_t src, bool try_declared); _z_keyexpr_t _z_keyexpr_steal(_Z_MOVE(_z_keyexpr_t) src); -static inline _z_keyexpr_t _z_keyexpr_null(void) { - _z_keyexpr_t keyexpr = {0, {0}, _z_string_null()}; - return keyexpr; -} bool _z_keyexpr_equals(const _z_keyexpr_t *left, const _z_keyexpr_t *right); void _z_keyexpr_move(_z_keyexpr_t *dst, _z_keyexpr_t *src); void _z_keyexpr_clear(_z_keyexpr_t *rk); diff --git a/include/zenoh-pico/session/liveliness.h b/include/zenoh-pico/session/liveliness.h index 3f375f70a..a3df1dc04 100644 --- a/include/zenoh-pico/session/liveliness.h +++ b/include/zenoh-pico/session/liveliness.h @@ -36,19 +36,19 @@ void _z_liveliness_pending_query_copy(_z_liveliness_pending_query_t *dst, const _z_liveliness_pending_query_t *_z_liveliness_pending_query_clone(const _z_liveliness_pending_query_t *src); _Z_ELEM_DEFINE(_z_liveliness_pending_query, _z_liveliness_pending_query_t, _z_noop_size, - _z_liveliness_pending_query_clear, _z_liveliness_pending_query_copy) + _z_liveliness_pending_query_clear, _z_liveliness_pending_query_copy, _z_noop_move) _Z_INT_MAP_DEFINE(_z_liveliness_pending_query, _z_liveliness_pending_query_t) uint32_t _z_liveliness_get_query_id(_z_session_t *zn); -z_result_t _z_liveliness_register_token(_z_session_t *zn, uint32_t id, const _z_keyexpr_t keyexpr); +z_result_t _z_liveliness_register_token(_z_session_t *zn, uint32_t id, const _z_keyexpr_t *keyexpr); void _z_liveliness_unregister_token(_z_session_t *zn, uint32_t id); #if Z_FEATURE_SUBSCRIPTION == 1 -z_result_t _z_liveliness_subscription_declare(_z_session_t *zn, uint32_t id, const _z_keyexpr_t keyexpr, +z_result_t _z_liveliness_subscription_declare(_z_session_t *zn, uint32_t id, const _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp); z_result_t _z_liveliness_subscription_undeclare(_z_session_t *zn, uint32_t id, const _z_timestamp_t *timestamp); -z_result_t _z_liveliness_subscription_trigger_history(_z_session_t *zn, _z_keyexpr_t keyexpr); +z_result_t _z_liveliness_subscription_trigger_history(_z_session_t *zn, const _z_keyexpr_t *keyexpr); #endif #if Z_FEATURE_QUERY == 1 diff --git a/include/zenoh-pico/session/query.h b/include/zenoh-pico/session/query.h index c096bd793..f7a671a05 100644 --- a/include/zenoh-pico/session/query.h +++ b/include/zenoh-pico/session/query.h @@ -22,6 +22,8 @@ extern "C" { #endif +void _z_pending_query_process_timeout(_z_session_t *zn); + #if Z_FEATURE_QUERY == 1 /*------------------ Query ------------------*/ _z_zint_t _z_get_query_id(_z_session_t *zn); @@ -29,7 +31,7 @@ _z_zint_t _z_get_query_id(_z_session_t *zn); _z_pending_query_t *_z_get_pending_query_by_id(_z_session_t *zn, const _z_zint_t id); z_result_t _z_register_pending_query(_z_session_t *zn, _z_pending_query_t *pq); -z_result_t _z_trigger_query_reply_partial(_z_session_t *zn, _z_zint_t reply_context, const _z_keyexpr_t keyexpr, +z_result_t _z_trigger_query_reply_partial(_z_session_t *zn, _z_zint_t reply_context, _z_keyexpr_t *keyexpr, _z_msg_put_t *msg, z_sample_kind_t kind); z_result_t _z_trigger_query_reply_err(_z_session_t *zn, _z_zint_t id, _z_msg_err_t *msg); z_result_t _z_trigger_query_reply_final(_z_session_t *zn, _z_zint_t id); diff --git a/include/zenoh-pico/session/queryable.h b/include/zenoh-pico/session/queryable.h index 4498a5bed..684880c17 100644 --- a/include/zenoh-pico/session/queryable.h +++ b/include/zenoh-pico/session/queryable.h @@ -16,20 +16,37 @@ #define ZENOH_PICO_SESSION_QUERYABLE_H #include +#include -#include "zenoh-pico/net/session.h" +// Forward declaration to avoid cyclical include +typedef struct _z_session_t _z_session_t; +typedef struct _z_session_rc_t _z_session_rc_t; + +// Queryable infos +typedef struct { + _z_closure_query_callback_t callback; + void *arg; +} _z_queryable_infos_t; + +_Z_ELEM_DEFINE(_z_queryable_infos, _z_queryable_infos_t, _z_noop_size, _z_noop_clear, _z_noop_copy, _z_noop_move) +_Z_SVEC_DEFINE(_z_queryable_infos, _z_queryable_infos_t) + +typedef struct { + _z_keyexpr_t ke_in; + _z_keyexpr_t ke_out; + _z_queryable_infos_svec_t infos; + size_t qle_nb; +} _z_queryable_cache_t; #if Z_FEATURE_QUERYABLE == 1 #define _Z_QUERYABLE_COMPLETE_DEFAULT false #define _Z_QUERYABLE_DISTANCE_DEFAULT 0 /*------------------ Queryable ------------------*/ +void _z_queryable_cache_clear(_z_queryable_cache_t *cache); _z_session_queryable_rc_t *_z_get_session_queryable_by_id(_z_session_t *zn, const _z_zint_t id); -_z_session_queryable_rc_list_t *_z_get_session_queryable_by_key(_z_session_t *zn, const _z_keyexpr_t key); - _z_session_queryable_rc_t *_z_register_session_queryable(_z_session_t *zn, _z_session_queryable_t *q); -z_result_t _z_trigger_queryables(_z_session_rc_t *zn, _z_msg_query_t *query, const _z_keyexpr_t q_key, uint32_t qid, - const _z_bytes_t attachment); +z_result_t _z_trigger_queryables(_z_session_rc_t *zn, _z_msg_query_t *query, _z_keyexpr_t *q_key, uint32_t qid); void _z_unregister_session_queryable(_z_session_t *zn, _z_session_queryable_rc_t *q); void _z_flush_session_queryable(_z_session_t *zn); #endif diff --git a/include/zenoh-pico/session/reply.h b/include/zenoh-pico/session/reply.h index 6e5221f49..b2aea9e5c 100644 --- a/include/zenoh-pico/session/reply.h +++ b/include/zenoh-pico/session/reply.h @@ -26,7 +26,7 @@ extern "C" { #ifndef ZENOH_PICO_SESSION_REPLY_H #define ZENOH_PICO_SESSION_REPLY_H -z_result_t _z_trigger_reply_partial(_z_session_t *zn, _z_zint_t id, _z_keyexpr_t key, _z_msg_reply_t *reply); +z_result_t _z_trigger_reply_partial(_z_session_t *zn, _z_zint_t id, _z_keyexpr_t *key, _z_msg_reply_t *reply); z_result_t _z_trigger_reply_err(_z_session_t *zn, _z_zint_t id, _z_msg_err_t *error); diff --git a/include/zenoh-pico/session/resource.h b/include/zenoh-pico/session/resource.h index bedffac06..61af9989b 100644 --- a/include/zenoh-pico/session/resource.h +++ b/include/zenoh-pico/session/resource.h @@ -36,7 +36,7 @@ void _z_unregister_resource(_z_session_t *zn, uint16_t id, uint16_t mapping); void _z_unregister_resources_for_peer(_z_session_t *zn, uint16_t mapping); void _z_flush_resources(_z_session_t *zn); -_z_keyexpr_t __unsafe_z_get_expanded_key_from_key(_z_session_t *zn, const _z_keyexpr_t *keyexpr); +_z_keyexpr_t __unsafe_z_get_expanded_key_from_key(_z_session_t *zn, const _z_keyexpr_t *keyexpr, bool force_alias); _z_resource_t *__unsafe_z_get_resource_by_id(_z_session_t *zn, uint16_t mapping, _z_zint_t id); _z_resource_t *__unsafe_z_get_resource_matching_key(_z_session_t *zn, const _z_keyexpr_t *keyexpr); diff --git a/include/zenoh-pico/session/session.h b/include/zenoh-pico/session/session.h index 3c1d50dbe..9f1c02475 100644 --- a/include/zenoh-pico/session/session.h +++ b/include/zenoh-pico/session/session.h @@ -52,10 +52,10 @@ void _z_resource_copy(_z_resource_t *dst, const _z_resource_t *src); void _z_resource_free(_z_resource_t **res); size_t _z_resource_size(_z_resource_t *p); -_Z_ELEM_DEFINE(_z_resource, _z_resource_t, _z_resource_size, _z_resource_clear, _z_resource_copy) +_Z_ELEM_DEFINE(_z_resource, _z_resource_t, _z_noop_size, _z_resource_clear, _z_resource_copy, _z_noop_move) _Z_LIST_DEFINE(_z_resource, _z_resource_t) -_Z_ELEM_DEFINE(_z_keyexpr, _z_keyexpr_t, _z_keyexpr_size, _z_keyexpr_clear, _z_keyexpr_copy) +_Z_ELEM_DEFINE(_z_keyexpr, _z_keyexpr_t, _z_keyexpr_size, _z_keyexpr_clear, _z_keyexpr_copy, _z_keyexpr_move) _Z_INT_MAP_DEFINE(_z_keyexpr, _z_keyexpr_t) // Forward declaration to avoid cyclical include @@ -79,9 +79,9 @@ bool _z_subscription_eq(const _z_subscription_t *one, const _z_subscription_t *t void _z_subscription_clear(_z_subscription_t *sub); _Z_REFCOUNT_DEFINE(_z_subscription, _z_subscription) -_Z_ELEM_DEFINE(_z_subscriber, _z_subscription_t, _z_noop_size, _z_subscription_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_subscriber, _z_subscription_t, _z_noop_size, _z_subscription_clear, _z_noop_copy, _z_noop_move) _Z_ELEM_DEFINE(_z_subscription_rc, _z_subscription_rc_t, _z_subscription_rc_size, _z_subscription_rc_drop, - _z_subscription_rc_copy) + _z_subscription_rc_copy, _z_noop_move) _Z_LIST_DEFINE(_z_subscription_rc, _z_subscription_rc_t) typedef struct { @@ -110,9 +110,10 @@ bool _z_session_queryable_eq(const _z_session_queryable_t *one, const _z_session void _z_session_queryable_clear(_z_session_queryable_t *res); _Z_REFCOUNT_DEFINE(_z_session_queryable, _z_session_queryable) -_Z_ELEM_DEFINE(_z_session_queryable, _z_session_queryable_t, _z_noop_size, _z_session_queryable_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_session_queryable, _z_session_queryable_t, _z_noop_size, _z_session_queryable_clear, _z_noop_copy, + _z_noop_move) _Z_ELEM_DEFINE(_z_session_queryable_rc, _z_session_queryable_rc_t, _z_noop_size, _z_session_queryable_rc_drop, - _z_noop_copy) + _z_noop_copy, _z_noop_move) _Z_LIST_DEFINE(_z_session_queryable_rc, _z_session_queryable_rc_t) // Forward declaration to avoid cyclical includes @@ -131,6 +132,8 @@ typedef struct { _z_zint_t _id; _z_closure_reply_callback_t _callback; _z_drop_handler_t _dropper; + z_clock_t _start_time; + uint64_t _timeout; void *_arg; _z_pending_reply_list_t *_pending_replies; z_query_target_t _target; @@ -141,7 +144,7 @@ typedef struct { bool _z_pending_query_eq(const _z_pending_query_t *one, const _z_pending_query_t *two); void _z_pending_query_clear(_z_pending_query_t *res); -_Z_ELEM_DEFINE(_z_pending_query, _z_pending_query_t, _z_noop_size, _z_pending_query_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_pending_query, _z_pending_query_t, _z_noop_size, _z_pending_query_clear, _z_noop_copy, _z_noop_move) _Z_LIST_DEFINE(_z_pending_query, _z_pending_query_t) typedef struct { @@ -192,9 +195,10 @@ bool _z_session_interest_eq(const _z_session_interest_t *one, const _z_session_i void _z_session_interest_clear(_z_session_interest_t *res); _Z_REFCOUNT_DEFINE(_z_session_interest, _z_session_interest) -_Z_ELEM_DEFINE(_z_session_interest, _z_session_interest_t, _z_noop_size, _z_session_interest_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_session_interest, _z_session_interest_t, _z_noop_size, _z_session_interest_clear, _z_noop_copy, + _z_noop_move) _Z_ELEM_DEFINE(_z_session_interest_rc, _z_session_interest_rc_t, _z_noop_size, _z_session_interest_rc_drop, - _z_noop_copy) + _z_noop_copy, _z_noop_move) _Z_LIST_DEFINE(_z_session_interest_rc, _z_session_interest_rc_t) typedef enum { @@ -210,7 +214,7 @@ typedef struct { } _z_declare_data_t; void _z_declare_data_clear(_z_declare_data_t *data); -_Z_ELEM_DEFINE(_z_declare_data, _z_declare_data_t, _z_noop_size, _z_declare_data_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_declare_data, _z_declare_data_t, _z_noop_size, _z_declare_data_clear, _z_noop_copy, _z_noop_move) _Z_LIST_DEFINE(_z_declare_data, _z_declare_data_t) #ifdef __cplusplus diff --git a/include/zenoh-pico/session/subscription.h b/include/zenoh-pico/session/subscription.h index 59c942554..b70871b30 100644 --- a/include/zenoh-pico/session/subscription.h +++ b/include/zenoh-pico/session/subscription.h @@ -16,36 +16,57 @@ #define INCLUDE_ZENOH_PICO_SESSION_SUBSCRIPTION_H #include "zenoh-pico/net/encoding.h" -#include "zenoh-pico/net/session.h" +#include "zenoh-pico/protocol/core.h" +#include "zenoh-pico/session/session.h" #ifdef __cplusplus extern "C" { #endif +// Forward declaration to avoid cyclical include +typedef struct _z_session_t _z_session_t; + +// Subscription infos +typedef struct { + _z_closure_sample_callback_t callback; + void *arg; +} _z_subscription_infos_t; + +_Z_ELEM_DEFINE(_z_subscription_infos, _z_subscription_infos_t, _z_noop_size, _z_noop_clear, _z_noop_copy, _z_noop_move) +_Z_SVEC_DEFINE(_z_subscription_infos, _z_subscription_infos_t) + +typedef struct { + _z_keyexpr_t ke_in; + _z_keyexpr_t ke_out; + _z_subscription_infos_svec_t infos; + size_t sub_nb; +} _z_subscription_cache_t; + /*------------------ Subscription ------------------*/ -z_result_t _z_trigger_subscriptions_put(_z_session_t *zn, const _z_keyexpr_t keyexpr, const _z_bytes_t payload, +z_result_t _z_trigger_subscriptions_put(_z_session_t *zn, _z_keyexpr_t *keyexpr, _z_bytes_t *payload, _z_encoding_t *encoding, const _z_timestamp_t *timestamp, const _z_n_qos_t qos, - const _z_bytes_t attachment, z_reliability_t reliability); + _z_bytes_t *attachment, z_reliability_t reliability); -z_result_t _z_trigger_subscriptions_del(_z_session_t *zn, const _z_keyexpr_t keyexpr, const _z_timestamp_t *timestamp, - const _z_n_qos_t qos, const _z_bytes_t attachment, z_reliability_t reliability); +z_result_t _z_trigger_subscriptions_del(_z_session_t *zn, _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp, + const _z_n_qos_t qos, _z_bytes_t *attachment, z_reliability_t reliability); -z_result_t _z_trigger_liveliness_subscriptions_declare(_z_session_t *zn, const _z_keyexpr_t keyexpr, +z_result_t _z_trigger_liveliness_subscriptions_declare(_z_session_t *zn, _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp); -z_result_t _z_trigger_liveliness_subscriptions_undeclare(_z_session_t *zn, const _z_keyexpr_t keyexpr, +z_result_t _z_trigger_liveliness_subscriptions_undeclare(_z_session_t *zn, _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp); #if Z_FEATURE_SUBSCRIPTION == 1 -_z_subscription_rc_t *_z_get_subscription_by_id(_z_session_t *zn, _z_subscriber_kind_t kind, const _z_zint_t id); -_z_subscription_rc_list_t *_z_get_subscriptions_by_key(_z_session_t *zn, _z_subscriber_kind_t kind, - const _z_keyexpr_t *keyexpr); +#if Z_FEATURE_RX_CACHE == 1 +void _z_subscription_cache_clear(_z_subscription_cache_t *cache); +#endif + +_z_subscription_rc_t *_z_get_subscription_by_id(_z_session_t *zn, _z_subscriber_kind_t kind, const _z_zint_t id); _z_subscription_rc_t *_z_register_subscription(_z_session_t *zn, _z_subscriber_kind_t kind, _z_subscription_t *sub); -z_result_t _z_trigger_subscriptions_impl(_z_session_t *zn, _z_subscriber_kind_t subscriber_kind, - const _z_keyexpr_t keyexpr, const _z_bytes_t payload, _z_encoding_t *encoding, - const _z_zint_t sample_kind, const _z_timestamp_t *timestamp, - const _z_n_qos_t qos, const _z_bytes_t attachment, +z_result_t _z_trigger_subscriptions_impl(_z_session_t *zn, _z_subscriber_kind_t sub_kind, _z_keyexpr_t *keyexpr, + _z_bytes_t *payload, _z_encoding_t *encoding, const _z_zint_t sample_kind, + const _z_timestamp_t *timestamp, const _z_n_qos_t qos, _z_bytes_t *attachment, z_reliability_t reliability); void _z_unregister_subscription(_z_session_t *zn, _z_subscriber_kind_t kind, _z_subscription_rc_t *sub); void _z_flush_subscriptions(_z_session_t *zn); diff --git a/include/zenoh-pico/session/utils.h b/include/zenoh-pico/session/utils.h index 97d4d11b3..44c48ff91 100644 --- a/include/zenoh-pico/session/utils.h +++ b/include/zenoh-pico/session/utils.h @@ -35,11 +35,14 @@ void _z_session_clear(_z_session_t *zn); z_result_t _z_session_close(_z_session_t *zn, uint8_t reason); z_result_t _z_handle_network_message(_z_session_rc_t *zsrc, _z_zenoh_message_t *z_msg, uint16_t local_peer_id); -z_result_t _z_send_n_msg(_z_session_t *zn, _z_network_message_t *n_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl); -void _zp_session_lock_mutex(_z_session_t *zn); -void _zp_session_unlock_mutex(_z_session_t *zn); +#if Z_FEATURE_MULTI_THREAD == 1 +static inline void _z_session_mutex_lock(_z_session_t *zn) { (void)_z_mutex_lock(&zn->_mutex_inner); } +static inline void _z_session_mutex_unlock(_z_session_t *zn) { (void)_z_mutex_unlock(&zn->_mutex_inner); } +#else +static inline void _z_session_mutex_lock(_z_session_t *zn) { _ZP_UNUSED(zn); } +static inline void _z_session_mutex_unlock(_z_session_t *zn) { _ZP_UNUSED(zn); } +#endif #ifdef __cplusplus } diff --git a/include/zenoh-pico/system/link/raweth.h b/include/zenoh-pico/system/link/raweth.h index f7b106571..b1fa878fe 100644 --- a/include/zenoh-pico/system/link/raweth.h +++ b/include/zenoh-pico/system/link/raweth.h @@ -47,14 +47,15 @@ typedef struct { void _z_raweth_clear_mapping_entry(_zp_raweth_mapping_entry_t *entry); _Z_ELEM_DEFINE(_zp_raweth_mapping, _zp_raweth_mapping_entry_t, _z_noop_size, _z_raweth_clear_mapping_entry, - _z_noop_copy) + _z_noop_copy, _z_noop_move) _Z_ARRAY_DEFINE(_zp_raweth_mapping, _zp_raweth_mapping_entry_t) typedef struct { uint8_t _mac[_ZP_MAC_ADDR_LENGTH]; } _zp_raweth_whitelist_entry_t; -_Z_ELEM_DEFINE(_zp_raweth_whitelist, _zp_raweth_whitelist_entry_t, _z_noop_size, _z_noop_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_zp_raweth_whitelist, _zp_raweth_whitelist_entry_t, _z_noop_size, _z_noop_clear, _z_noop_copy, + _z_noop_move) _Z_ARRAY_DEFINE(_zp_raweth_whitelist, _zp_raweth_whitelist_entry_t) // Ethernet header structure type diff --git a/include/zenoh-pico/transport/common/tx.h b/include/zenoh-pico/transport/common/tx.h index e70c8fc9e..4e452b046 100644 --- a/include/zenoh-pico/transport/common/tx.h +++ b/include/zenoh-pico/transport/common/tx.h @@ -31,8 +31,12 @@ z_result_t __unsafe_z_serialize_zenoh_fragment(_z_wbuf_t *dst, _z_wbuf_t *src, z bool first); /*------------------ Transmission and Reception helpers ------------------*/ +z_result_t _z_transport_tx_send_t_msg(_z_transport_common_t *ztc, const _z_transport_message_t *t_msg); z_result_t _z_send_t_msg(_z_transport_t *zt, const _z_transport_message_t *t_msg); z_result_t _z_link_send_t_msg(const _z_link_t *zl, const _z_transport_message_t *t_msg); +z_result_t _z_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, z_reliability_t reliability, + z_congestion_control_t cong_ctrl); +z_result_t _z_send_n_batch(_z_session_t *zn, z_congestion_control_t cong_ctrl); #ifdef __cplusplus } diff --git a/include/zenoh-pico/transport/manager.h b/include/zenoh-pico/transport/manager.h index 321bed3ac..b22eff975 100644 --- a/include/zenoh-pico/transport/manager.h +++ b/include/zenoh-pico/transport/manager.h @@ -23,7 +23,12 @@ extern "C" { #endif -z_result_t _z_new_transport(_z_transport_t *zt, _z_id_t *bs, _z_string_t *locator, z_whatami_t mode); +enum _z_peer_op_e { + _Z_PEER_OP_OPEN = 0, + _Z_PEER_OP_LISTEN = 1, +}; + +z_result_t _z_new_transport(_z_transport_t *zt, _z_id_t *bs, _z_string_t *locator, z_whatami_t mode, int peer_op); void _z_free_transport(_z_transport_t **zt); #ifdef __cplusplus diff --git a/include/zenoh-pico/transport/multicast/rx.h b/include/zenoh-pico/transport/multicast/rx.h index c16ca82d4..f7a81e00a 100644 --- a/include/zenoh-pico/transport/multicast/rx.h +++ b/include/zenoh-pico/transport/multicast/rx.h @@ -24,6 +24,7 @@ extern "C" { z_result_t _z_multicast_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_slice_t *addr); z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_slice_t *addr); +z_result_t _z_multicast_update_rx_buffer(_z_transport_multicast_t *ztm); #ifdef __cplusplus } diff --git a/include/zenoh-pico/transport/multicast/transport.h b/include/zenoh-pico/transport/multicast/transport.h index ef8b73994..3e61f4bc1 100644 --- a/include/zenoh-pico/transport/multicast/transport.h +++ b/include/zenoh-pico/transport/multicast/transport.h @@ -31,8 +31,15 @@ z_result_t _z_multicast_send_close(_z_transport_multicast_t *ztm, uint8_t reason z_result_t _z_multicast_transport_close(_z_transport_multicast_t *ztm, uint8_t reason); void _z_multicast_transport_clear(_z_transport_t *zt); +#if (Z_FEATURE_MULTICAST_TRANSPORT == 1 || Z_FEATURE_RAWETH_TRANSPORT == 1) && Z_FEATURE_MULTI_THREAD == 1 +static inline void _z_multicast_peer_mutex_lock(_z_transport_multicast_t *ztm) { _z_mutex_lock(&ztm->_mutex_peer); } +static inline void _z_multicast_peer_mutex_unlock(_z_transport_multicast_t *ztm) { _z_mutex_unlock(&ztm->_mutex_peer); } +#else +static inline void _z_multicast_peer_mutex_lock(_z_transport_multicast_t *ztm) { _ZP_UNUSED(ztm); } +static inline void _z_multicast_peer_mutex_unlock(_z_transport_multicast_t *ztm) { _ZP_UNUSED(ztm); } +#endif // (Z_FEATURE_MULTICAST_TRANSPORT == 1 || Z_FEATURE_RAWETH_TRANSPORT == 1) && Z_FEATURE_MULTI_THREAD == 1 + #ifdef __cplusplus } #endif - #endif /* ZENOH_PICO_MULTICAST_TRANSPORT_H */ diff --git a/include/zenoh-pico/transport/multicast/tx.h b/include/zenoh-pico/transport/multicast/tx.h deleted file mode 100644 index 903fc80cd..000000000 --- a/include/zenoh-pico/transport/multicast/tx.h +++ /dev/null @@ -1,33 +0,0 @@ -// -// 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, -// - -#ifndef ZENOH_PICO_MULTICAST_TX_H -#define ZENOH_PICO_MULTICAST_TX_H - -#include "zenoh-pico/net/session.h" -#include "zenoh-pico/transport/transport.h" - -#ifdef __cplusplus -extern "C" { -#endif - -z_result_t _z_multicast_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl); -z_result_t _z_multicast_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg); - -#ifdef __cplusplus -} -#endif - -#endif /* ZENOH_PICO_MULTICAST_TX_H */ diff --git a/include/zenoh-pico/transport/raweth/rx.h b/include/zenoh-pico/transport/raweth/rx.h index 5ebcfb0e1..19901b68f 100644 --- a/include/zenoh-pico/transport/raweth/rx.h +++ b/include/zenoh-pico/transport/raweth/rx.h @@ -23,6 +23,7 @@ extern "C" { z_result_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_slice_t *addr); z_result_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_slice_t *addr); +z_result_t _z_raweth_update_rx_buff(_z_transport_multicast_t *ztm); #ifdef __cplusplus } diff --git a/include/zenoh-pico/transport/raweth/tx.h b/include/zenoh-pico/transport/raweth/tx.h index 4b191f2de..a338c16f3 100644 --- a/include/zenoh-pico/transport/raweth/tx.h +++ b/include/zenoh-pico/transport/raweth/tx.h @@ -25,7 +25,7 @@ extern "C" { z_result_t _z_raweth_link_send_t_msg(const _z_link_t *zl, const _z_transport_message_t *t_msg); z_result_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, z_congestion_control_t cong_ctrl); -z_result_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg); +z_result_t _z_raweth_send_t_msg(_z_transport_common_t *ztc, const _z_transport_message_t *t_msg); #ifdef __cplusplus } diff --git a/include/zenoh-pico/transport/transport.h b/include/zenoh-pico/transport/transport.h index 02c304ea6..c2f87b6ee 100644 --- a/include/zenoh-pico/transport/transport.h +++ b/include/zenoh-pico/transport/transport.h @@ -29,26 +29,34 @@ extern "C" { #endif -typedef struct { -#if Z_FEATURE_FRAGMENTATION == 1 - // Defragmentation buffers - _z_wbuf_t _dbuf_reliable; - _z_wbuf_t _dbuf_best_effort; -#endif +enum _z_dbuf_state_e { + _Z_DBUF_STATE_NULL = 0, + _Z_DBUF_STATE_INIT = 1, + _Z_DBUF_STATE_OVERFLOW = 2, +}; +enum _z_batching_state_e { + _Z_BATCHING_IDLE = 0, + _Z_BATCHING_ACTIVE = 1, +}; + +typedef struct { _z_id_t _remote_zid; _z_slice_t _remote_addr; _z_conduit_sn_list_t _sn_rx_sns; - // SN numbers _z_zint_t _sn_res; volatile _z_zint_t _lease; volatile _z_zint_t _next_lease; - uint16_t _peer_id; volatile bool _received; #if Z_FEATURE_FRAGMENTATION == 1 + // Defragmentation buffers + uint8_t _state_reliable; + uint8_t _state_best_effort; + _z_wbuf_t _dbuf_reliable; + _z_wbuf_t _dbuf_best_effort; // Patch uint8_t _patch; #endif @@ -59,7 +67,7 @@ void _z_transport_peer_entry_clear(_z_transport_peer_entry_t *src); void _z_transport_peer_entry_copy(_z_transport_peer_entry_t *dst, const _z_transport_peer_entry_t *src); bool _z_transport_peer_entry_eq(const _z_transport_peer_entry_t *left, const _z_transport_peer_entry_t *right); _Z_ELEM_DEFINE(_z_transport_peer_entry, _z_transport_peer_entry_t, _z_transport_peer_entry_size, - _z_transport_peer_entry_clear, _z_transport_peer_entry_copy) + _z_transport_peer_entry_clear, _z_transport_peer_entry_copy, _z_noop_move) _Z_LIST_DEFINE(_z_transport_peer_entry, _z_transport_peer_entry_t) _z_transport_peer_entry_list_t *_z_transport_peer_entry_list_insert(_z_transport_peer_entry_list_t *root, _z_transport_peer_entry_t *entry); @@ -67,98 +75,70 @@ _z_transport_peer_entry_list_t *_z_transport_peer_entry_list_insert(_z_transport // Forward type declaration to avoid cyclical include typedef struct _z_session_rc_t _z_session_rc_ref_t; -// Forward declaration to be used in _zp_f_send_tmsg* -typedef struct _z_transport_multicast_t _z_transport_multicast_t; -// Send function prototype -typedef z_result_t (*_zp_f_send_tmsg)(_z_transport_multicast_t *self, const _z_transport_message_t *t_msg); +#define _Z_RES_POOL_INIT_SIZE 8 // Arbitrary small value typedef struct { - // Session associated to the transport _z_session_rc_ref_t *_session; - -#if Z_FEATURE_MULTI_THREAD == 1 - // TX and RX mutexes - _z_mutex_t _mutex_rx; - _z_mutex_t _mutex_tx; -#endif // Z_FEATURE_MULTI_THREAD == 1 - _z_link_t _link; - -#if Z_FEATURE_FRAGMENTATION == 1 - // Defragmentation buffer - _z_wbuf_t _dbuf_reliable; - _z_wbuf_t _dbuf_best_effort; -#endif - - // Regular Buffers + // TX and RX buffers _z_wbuf_t _wbuf; _z_zbuf_t _zbuf; - - _z_id_t _remote_zid; - // SN numbers _z_zint_t _sn_res; _z_zint_t _sn_tx_reliable; _z_zint_t _sn_tx_best_effort; - _z_zint_t _sn_rx_reliable; - _z_zint_t _sn_rx_best_effort; + _z_arc_slice_svec_t _arc_pool; + _z_network_message_svec_t _msg_pool; volatile _z_zint_t _lease; - + volatile bool _transmitted; #if Z_FEATURE_MULTI_THREAD == 1 + // TX and RX mutexes + _z_mutex_t _mutex_rx; + _z_mutex_t _mutex_tx; + _z_task_t *_read_task; _z_task_t *_lease_task; volatile bool _read_task_running; volatile bool _lease_task_running; -#endif // Z_FEATURE_MULTI_THREAD == 1 +#endif +// Transport batching +#if Z_FEATURE_BATCHING == 1 + uint8_t _batch_state; + size_t _batch_count; +#endif +} _z_transport_common_t; + +// Send function prototype +typedef z_result_t (*_zp_f_send_tmsg)(_z_transport_common_t *self, const _z_transport_message_t *t_msg); +typedef struct { + _z_transport_common_t _common; + _z_id_t _remote_zid; + _z_zint_t _sn_rx_reliable; + _z_zint_t _sn_rx_best_effort; volatile bool _received; - volatile bool _transmitted; #if Z_FEATURE_FRAGMENTATION == 1 + // Defragmentation buffer + uint8_t _state_reliable; + uint8_t _state_best_effort; + _z_wbuf_t _dbuf_reliable; + _z_wbuf_t _dbuf_best_effort; // Patch uint8_t _patch; #endif } _z_transport_unicast_t; typedef struct _z_transport_multicast_t { - // Session associated to the transport - _z_session_rc_ref_t *_session; - -#if Z_FEATURE_MULTI_THREAD == 1 - // TX and RX mutexes - _z_mutex_t _mutex_rx; - _z_mutex_t _mutex_tx; - - // Peer list mutex - _z_mutex_t _mutex_peer; -#endif // Z_FEATURE_MULTI_THREAD == 1 - - _z_link_t _link; - - // TX and RX buffers - _z_wbuf_t _wbuf; - _z_zbuf_t _zbuf; - - // SN initial numbers - _z_zint_t _sn_res; - _z_zint_t _sn_tx_reliable; - _z_zint_t _sn_tx_best_effort; - volatile _z_zint_t _lease; - + _z_transport_common_t _common; // Known valid peers _z_transport_peer_entry_list_t *_peers; - // T message send function _zp_f_send_tmsg _send_f; #if Z_FEATURE_MULTI_THREAD == 1 - _z_task_t *_read_task; - _z_task_t *_lease_task; - volatile bool _read_task_running; - volatile bool _lease_task_running; -#endif // Z_FEATURE_MULTI_THREAD == 1 - - volatile bool _transmitted; + _z_mutex_t _mutex_peer; // Peer list mutex +#endif } _z_transport_multicast_t; typedef struct { @@ -171,7 +151,7 @@ typedef struct { enum { _Z_TRANSPORT_UNICAST_TYPE, _Z_TRANSPORT_MULTICAST_TYPE, _Z_TRANSPORT_RAWETH_TYPE, _Z_TRANSPORT_NONE } _type; } _z_transport_t; -_Z_ELEM_DEFINE(_z_transport, _z_transport_t, _z_noop_size, _z_noop_clear, _z_noop_copy) +_Z_ELEM_DEFINE(_z_transport, _z_transport_t, _z_noop_size, _z_noop_clear, _z_noop_copy, _z_noop_move) _Z_LIST_DEFINE(_z_transport, _z_transport_t) typedef struct { @@ -199,8 +179,35 @@ z_result_t _z_transport_close(_z_transport_t *zt, uint8_t reason); void _z_transport_clear(_z_transport_t *zt); void _z_transport_free(_z_transport_t **zt); +#if Z_FEATURE_BATCHING == 1 +bool _z_transport_start_batching(_z_transport_t *zt); +void _z_transport_stop_batching(_z_transport_t *zt); +#endif + +#if Z_FEATURE_MULTI_THREAD == 1 +static inline z_result_t _z_transport_tx_mutex_lock(_z_transport_common_t *ztc, bool block) { + if (block) { + _z_mutex_lock(&ztc->_mutex_tx); + return _Z_RES_OK; + } else { + return _z_mutex_try_lock(&ztc->_mutex_tx); + } +} +static inline void _z_transport_tx_mutex_unlock(_z_transport_common_t *ztc) { _z_mutex_unlock(&ztc->_mutex_tx); } +static inline void _z_transport_rx_mutex_lock(_z_transport_common_t *ztc) { _z_mutex_lock(&ztc->_mutex_rx); } +static inline void _z_transport_rx_mutex_unlock(_z_transport_common_t *ztc) { _z_mutex_unlock(&ztc->_mutex_rx); } +#else +static inline z_result_t _z_transport_tx_mutex_lock(_z_transport_common_t *ztc, bool block) { + _ZP_UNUSED(ztc); + _ZP_UNUSED(block); + return _Z_RES_OK; +} +static inline void _z_transport_tx_mutex_unlock(_z_transport_common_t *ztc) { _ZP_UNUSED(ztc); } +static inline void _z_transport_rx_mutex_lock(_z_transport_common_t *ztc) { _ZP_UNUSED(ztc); } +static inline void _z_transport_rx_mutex_unlock(_z_transport_common_t *ztc) { _ZP_UNUSED(ztc); } +#endif // Z_FEATURE_MULTI_THREAD == 1 + #ifdef __cplusplus } #endif - #endif /* INCLUDE_ZENOH_PICO_TRANSPORT_TRANSPORT_H */ diff --git a/include/zenoh-pico/transport/unicast/rx.h b/include/zenoh-pico/transport/unicast/rx.h index 6e948dc73..8052ebd88 100644 --- a/include/zenoh-pico/transport/unicast/rx.h +++ b/include/zenoh-pico/transport/unicast/rx.h @@ -24,6 +24,7 @@ extern "C" { z_result_t _z_unicast_recv_t_msg(_z_transport_unicast_t *ztu, _z_transport_message_t *t_msg); z_result_t _z_unicast_recv_t_msg_na(_z_transport_unicast_t *ztu, _z_transport_message_t *t_msg); z_result_t _z_unicast_handle_transport_message(_z_transport_unicast_t *ztu, _z_transport_message_t *t_msg); +z_result_t _z_unicast_update_rx_buffer(_z_transport_unicast_t *ztu); #ifdef __cplusplus } diff --git a/include/zenoh-pico/transport/unicast/transport.h b/include/zenoh-pico/transport/unicast/transport.h index dc39cac54..3ca322c42 100644 --- a/include/zenoh-pico/transport/unicast/transport.h +++ b/include/zenoh-pico/transport/unicast/transport.h @@ -26,7 +26,7 @@ z_result_t _z_unicast_transport_create(_z_transport_t *zt, _z_link_t *zl, z_result_t _z_unicast_open_client(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, const _z_id_t *local_zid); z_result_t _z_unicast_open_peer(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, - const _z_id_t *local_zid); + const _z_id_t *local_zid, int peer_op); z_result_t _z_unicast_send_close(_z_transport_unicast_t *ztu, uint8_t reason, bool link_only); z_result_t _z_unicast_transport_close(_z_transport_unicast_t *ztu, uint8_t reason); void _z_unicast_transport_clear(_z_transport_t *zt); diff --git a/include/zenoh-pico/transport/unicast/tx.h b/include/zenoh-pico/transport/unicast/tx.h deleted file mode 100644 index ade38faf0..000000000 --- a/include/zenoh-pico/transport/unicast/tx.h +++ /dev/null @@ -1,33 +0,0 @@ -// -// 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, -// - -#ifndef ZENOH_PICO_UNICAST_TX_H -#define ZENOH_PICO_UNICAST_TX_H - -#include "zenoh-pico/net/session.h" -#include "zenoh-pico/transport/transport.h" - -#ifdef __cplusplus -extern "C" { -#endif - -z_result_t _z_unicast_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl); -z_result_t _z_unicast_send_t_msg(_z_transport_unicast_t *ztu, const _z_transport_message_t *t_msg); - -#ifdef __cplusplus -} -#endif - -#endif /* ZENOH_PICO_TRANSPORT_LINK_TX_H */ diff --git a/src/api/api.c b/src/api/api.c index 8ff30c7cf..1e6d13634 100644 --- a/src/api/api.c +++ b/src/api/api.c @@ -39,6 +39,7 @@ #include "zenoh-pico/session/utils.h" #include "zenoh-pico/system/platform.h" #include "zenoh-pico/system/platform_common.h" +#include "zenoh-pico/transport/common/tx.h" #include "zenoh-pico/transport/multicast.h" #include "zenoh-pico/transport/unicast.h" #include "zenoh-pico/utils/endianness.h" @@ -64,8 +65,8 @@ _z_string_svec_t _z_string_array_null(void) { return _z_string_svec_make(0); } void z_string_array_new(z_owned_string_array_t *a) { a->_val = _z_string_array_null(); } size_t z_string_array_push_by_alias(z_loaned_string_array_t *a, const z_loaned_string_t *value) { - _z_string_t str = _z_string_alias(value); - _z_string_svec_append(a, &str); + _z_string_t str = _z_string_alias(*value); + _z_string_svec_append(a, &str, true); return _z_string_svec_len(a); } @@ -73,7 +74,7 @@ size_t z_string_array_push_by_alias(z_loaned_string_array_t *a, const z_loaned_s size_t z_string_array_push_by_copy(z_loaned_string_array_t *a, const z_loaned_string_t *value) { _z_string_t str; _z_string_copy(&str, value); - _z_string_svec_append(a, &str); + _z_string_svec_append(a, &str, true); return _z_string_svec_len(a); } @@ -130,7 +131,7 @@ void z_view_keyexpr_from_substr_unchecked(z_view_keyexpr_t *keyexpr, const char } z_result_t z_keyexpr_as_view_string(const z_loaned_keyexpr_t *keyexpr, z_view_string_t *s) { - s->_val = _z_string_alias(&keyexpr->_suffix); + s->_val = _z_string_alias(keyexpr->_suffix); return _Z_RES_OK; } @@ -243,12 +244,12 @@ const uint8_t *z_slice_data(const z_loaned_slice_t *slice) { return slice->start size_t z_slice_len(const z_loaned_slice_t *slice) { return slice->len; } -void z_slice_empty(z_owned_slice_t *slice) { slice->_val = _z_slice_empty(); } +void z_slice_empty(z_owned_slice_t *slice) { slice->_val = _z_slice_null(); } bool z_slice_is_empty(const z_loaned_slice_t *slice) { return _z_slice_is_empty(slice); } z_result_t z_bytes_to_slice(const z_loaned_bytes_t *bytes, z_owned_slice_t *dst) { - dst->_val = _z_slice_empty(); + dst->_val = _z_slice_null(); return _z_bytes_to_slice(bytes, &dst->_val); } @@ -385,6 +386,7 @@ z_result_t z_timestamp_new(z_timestamp_t *ts, const z_loaned_session_t *zs) { *ts = _z_timestamp_null(); _z_time_since_epoch t; _Z_RETURN_IF_ERR(_z_get_time_since_epoch(&t)); + ts->valid = true; ts->time = _z_timestamp_ntp64_from_time(t.secs, t.nanos); ts->id = _Z_RC_IN_VAL(zs)->_local_zid; return _Z_RES_OK; @@ -415,14 +417,15 @@ z_query_consolidation_t z_query_consolidation_none(void) { z_query_consolidation_t z_query_consolidation_default(void) { return z_query_consolidation_auto(); } void z_query_parameters(const z_loaned_query_t *query, z_view_string_t *parameters) { - parameters->_val = _z_string_alias_str(_Z_RC_IN_VAL(query)->_parameters); + parameters->_val = _z_string_alias(_Z_RC_IN_VAL(query)->_parameters); } -const z_loaned_bytes_t *z_query_attachment(const z_loaned_query_t *query) { return &_Z_RC_IN_VAL(query)->attachment; } +const z_loaned_bytes_t *z_query_attachment(const z_loaned_query_t *query) { return &_Z_RC_IN_VAL(query)->_attachment; } const z_loaned_keyexpr_t *z_query_keyexpr(const z_loaned_query_t *query) { return &_Z_RC_IN_VAL(query)->_key; } const z_loaned_bytes_t *z_query_payload(const z_loaned_query_t *query) { return &_Z_RC_IN_VAL(query)->_value.payload; } + const z_loaned_encoding_t *z_query_encoding(const z_loaned_query_t *query) { return &_Z_RC_IN_VAL(query)->_value.encoding; } @@ -468,16 +471,13 @@ _Z_OWNED_FUNCTIONS_VALUE_IMPL(_z_config_t, config, _z_config_check, _z_config_nu _Z_OWNED_FUNCTIONS_VALUE_IMPL(_z_string_t, string, _z_string_check, _z_string_null, _z_string_copy, _z_string_clear) -bool _z_value_check(const _z_value_t *value) { - return _z_encoding_check(&value->encoding) || _z_bytes_check(&value->payload); -} _Z_OWNED_FUNCTIONS_VALUE_IMPL(_z_value_t, reply_err, _z_value_check, _z_value_null, _z_value_copy, _z_value_clear) _Z_OWNED_FUNCTIONS_VALUE_IMPL(_z_keyexpr_t, keyexpr, _z_keyexpr_check, _z_keyexpr_null, _z_keyexpr_copy, _z_keyexpr_clear) _Z_VIEW_FUNCTIONS_IMPL(_z_keyexpr_t, keyexpr, _z_keyexpr_check, _z_keyexpr_null) _Z_VIEW_FUNCTIONS_IMPL(_z_string_t, string, _z_string_check, _z_string_null) -_Z_VIEW_FUNCTIONS_IMPL(_z_slice_t, slice, _z_slice_check, _z_slice_empty) +_Z_VIEW_FUNCTIONS_IMPL(_z_slice_t, slice, _z_slice_check, _z_slice_null) _Z_OWNED_FUNCTIONS_VALUE_IMPL(_z_hello_t, hello, _z_hello_check, _z_hello_null, _z_hello_copy, _z_hello_clear) @@ -515,12 +515,11 @@ z_result_t z_whatami_to_view_string(z_whatami_t whatami, z_view_string_t *str_ou bool _z_string_array_check(const _z_string_svec_t *val) { return !_z_string_svec_is_empty(val); } z_result_t _z_string_array_copy(_z_string_svec_t *dst, const _z_string_svec_t *src) { - _z_string_svec_copy(dst, src); - return dst->_len == src->_len ? _Z_RES_OK : _Z_ERR_SYSTEM_OUT_OF_MEMORY; + return _z_string_svec_copy(dst, src, true); } _Z_OWNED_FUNCTIONS_VALUE_IMPL(_z_string_svec_t, string_array, _z_string_array_check, _z_string_array_null, _z_string_array_copy, _z_string_svec_clear) -_Z_OWNED_FUNCTIONS_VALUE_IMPL(_z_slice_t, slice, _z_slice_check, _z_slice_empty, _z_slice_copy, _z_slice_clear) +_Z_OWNED_FUNCTIONS_VALUE_IMPL(_z_slice_t, slice, _z_slice_check, _z_slice_null, _z_slice_copy, _z_slice_clear) _Z_OWNED_FUNCTIONS_VALUE_IMPL(_z_bytes_t, bytes, _z_bytes_check, _z_bytes_null, _z_bytes_copy, _z_bytes_drop) _Z_OWNED_FUNCTIONS_VALUE_NO_COPY_IMPL(_z_bytes_writer_t, bytes_writer, _z_bytes_writer_check, _z_bytes_writer_empty, _z_bytes_writer_clear) @@ -821,18 +820,22 @@ z_result_t z_put(const z_loaned_session_t *zs, const z_loaned_keyexpr_t *keyexpr reliability = opt.reliability; #endif + _z_bytes_t payload_bytes = _z_bytes_from_owned_bytes(&payload->_this); + _z_bytes_t attachment_bytes = _z_bytes_from_owned_bytes(&opt.attachment->_this); _z_keyexpr_t keyexpr_aliased = _z_keyexpr_alias_from_user_defined(*keyexpr, true); - ret = _z_write(_Z_RC_IN_VAL(zs), keyexpr_aliased, _z_bytes_from_owned_bytes(&payload->_this), + ret = _z_write(_Z_RC_IN_VAL(zs), keyexpr_aliased, payload_bytes, opt.encoding == NULL ? NULL : &opt.encoding->_this._val, Z_SAMPLE_KIND_PUT, opt.congestion_control, - opt.priority, opt.is_express, opt.timestamp, _z_bytes_from_owned_bytes(&opt.attachment->_this), - reliability); + opt.priority, opt.is_express, opt.timestamp, attachment_bytes, reliability); - // Trigger subscriptions + // Trigger local subscriptions +#if Z_FEATURE_LOCAL_SUBSCRIBER == 1 + _z_timestamp_t local_timestamp = (opt.timestamp != NULL) ? *opt.timestamp : _z_timestamp_null(); + _z_encoding_t local_encoding = (opt.encoding != NULL) ? &opt.encoding->_this._val : _z_encoding_null(); _z_trigger_subscriptions_put( - _Z_RC_IN_VAL(zs), keyexpr_aliased, _z_bytes_from_owned_bytes(&payload->_this), - opt.encoding == NULL ? NULL : &opt.encoding->_this._val, opt.timestamp, + _Z_RC_IN_VAL(zs), &keyexpr_aliased, &payload_bytes, &local_encoding, , &local_timestamp, _z_n_qos_make(opt.is_express, opt.congestion_control == Z_CONGESTION_CONTROL_BLOCK, opt.priority), - _z_bytes_from_owned_bytes(&opt.attachment->_this), reliability); + &attachment_bytes, reliability); +#endif // Clean-up z_encoding_drop(opt.encoding); z_bytes_drop(opt.attachment); @@ -949,28 +952,45 @@ z_result_t z_publisher_put(const z_loaned_publisher_t *pub, z_moved_bytes_t *pay // Remove potentially redundant ke suffix _z_keyexpr_t pub_keyexpr = _z_keyexpr_alias_from_user_defined(pub->_key, true); + _z_session_t *session = NULL; +#if Z_FEATURE_PUBLISHER_SESSION_CHECK == 1 // Try to upgrade session rc _z_session_rc_t sess_rc = _z_session_weak_upgrade_if_open(&pub->_zn); - if (!_Z_RC_IS_NULL(&sess_rc)) { + session = _Z_RC_IN_VAL(&sess_rc); + } else { + ret = _Z_ERR_SESSION_CLOSED; + } +#else + session = _Z_RC_IN_VAL(&pub->_zn); +#endif + + if (session != NULL) { + _z_bytes_t payload_bytes = _z_bytes_from_owned_bytes(&payload->_this); + _z_bytes_t attachment_bytes = _z_bytes_from_owned_bytes(&opt.attachment->_this); + // Check if write filter is active before writing if (!_z_write_filter_active(pub)) { // Write value - ret = _z_write(_Z_RC_IN_VAL(&sess_rc), pub_keyexpr, _z_bytes_from_owned_bytes(&payload->_this), &encoding, - Z_SAMPLE_KIND_PUT, pub->_congestion_control, pub->_priority, pub->_is_express, opt.timestamp, - _z_bytes_from_owned_bytes(&opt.attachment->_this), reliability); + ret = _z_write(session, pub_keyexpr, payload_bytes, &encoding, Z_SAMPLE_KIND_PUT, pub->_congestion_control, + pub->_priority, pub->_is_express, opt.timestamp, attachment_bytes, reliability); } - // Trigger subscriptions + // Trigger local subscriptions +#if Z_FEATURE_LOCAL_SUBSCRIBER == 1 + _z_timestamp_t local_timestamp = (opt.timestamp != NULL) ? *opt.timestamp : _z_timestamp_null(); _z_trigger_subscriptions_put( - _Z_RC_IN_VAL(&sess_rc), pub_keyexpr, _z_bytes_from_owned_bytes(&payload->_this), &encoding, opt.timestamp, + session, &pub_keyexpr, &payload_bytes, &encoding, &local_timestamp, _z_n_qos_make(pub->_is_express, pub->_congestion_control == Z_CONGESTION_CONTROL_BLOCK, pub->_priority), - _z_bytes_from_owned_bytes(&opt.attachment->_this), reliability); - - _z_session_rc_drop(&sess_rc); + &attachment_bytes, reliability); +#endif } else { ret = _Z_ERR_SESSION_CLOSED; } +#if Z_FEATURE_PUBLISHER_SESSION_CHECK == 1 + _z_session_rc_drop(&sess_rc); +#endif + // Clean-up _z_encoding_clear(&encoding); z_bytes_drop(opt.attachment); @@ -992,16 +1012,27 @@ z_result_t z_publisher_delete(const z_loaned_publisher_t *pub, const z_publisher // Remove potentially redundant ke suffix _z_keyexpr_t pub_keyexpr = _z_keyexpr_alias_from_user_defined(pub->_key, true); + _z_session_t *session = NULL; +#if Z_FEATURE_PUBLISHER_SESSION_CHECK == 1 // Try to upgrade session rc _z_session_rc_t sess_rc = _z_session_weak_upgrade_if_open(&pub->_zn); - if (_Z_RC_IS_NULL(&sess_rc)) { + if (!_Z_RC_IS_NULL(&sess_rc)) { + session = _Z_RC_IN_VAL(&sess_rc); + } else { return _Z_ERR_SESSION_CLOSED; } - z_result_t ret = _z_write(_Z_RC_IN_VAL(&sess_rc), pub_keyexpr, _z_bytes_null(), NULL, Z_SAMPLE_KIND_DELETE, - pub->_congestion_control, pub->_priority, pub->_is_express, opt.timestamp, - _z_bytes_null(), reliability); +#else + session = _Z_RC_IN_VAL(&pub->_zn); +#endif + + z_result_t ret = + _z_write(session, pub_keyexpr, _z_bytes_null(), NULL, Z_SAMPLE_KIND_DELETE, pub->_congestion_control, + pub->_priority, pub->_is_express, opt.timestamp, _z_bytes_null(), reliability); + +#if Z_FEATURE_PUBLISHER_SESSION_CHECK == 1 // Clean up _z_session_rc_drop(&sess_rc); +#endif return ret; } @@ -1045,14 +1076,7 @@ z_result_t z_get(const z_loaned_session_t *zs, const z_loaned_keyexpr_t *keyexpr z_get_options_t opt; z_get_options_default(&opt); if (options != NULL) { - opt.consolidation = options->consolidation; - opt.target = options->target; - opt.encoding = options->encoding; - opt.payload = options->payload; - opt.attachment = options->attachment; - opt.congestion_control = options->congestion_control; - opt.priority = options->priority; - opt.is_express = options->is_express; + opt = *options; } if (opt.consolidation.mode == Z_CONSOLIDATION_MODE_AUTO) { @@ -1142,7 +1166,7 @@ z_result_t z_declare_queryable(const z_loaned_session_t *zs, z_owned_queryable_t z_queryable_options_t opt; z_queryable_options_default(&opt); if (options != NULL) { - opt.complete = options->complete; + opt = *options; } queryable->_val = @@ -1249,7 +1273,6 @@ z_result_t z_query_reply_err(const z_loaned_query_t *query, z_moved_bytes_t *pay // Set value _z_value_t value = {.payload = _z_bytes_from_owned_bytes(&payload->_this), .encoding = _z_encoding_from_owned(&opts.encoding->_this)}; - z_result_t ret = _z_send_reply_err(_Z_RC_IN_VAL(query), &sess_rc, value); _z_session_rc_drop(&sess_rc); z_bytes_drop(payload); @@ -1410,6 +1433,37 @@ const z_loaned_keyexpr_t *z_subscriber_keyexpr(const z_loaned_subscriber_t *sub) } #endif +#ifdef Z_FEATURE_UNSTABLE_API +#if Z_FEATURE_BATCHING == 1 +z_result_t zp_batch_start(const z_loaned_session_t *zs) { + if (_Z_RC_IS_NULL(zs)) { + return _Z_ERR_SESSION_CLOSED; + } + _z_session_t *session = _Z_RC_IN_VAL(zs); + return _z_transport_start_batching(&session->_tp) ? _Z_RES_OK : _Z_ERR_GENERIC; +} + +z_result_t zp_batch_flush(const z_loaned_session_t *zs) { + _z_session_t *session = _Z_RC_IN_VAL(zs); + if (_Z_RC_IS_NULL(zs)) { + return _Z_ERR_SESSION_CLOSED; + } + // Send current batch + return _z_send_n_batch(session, Z_CONGESTION_CONTROL_DEFAULT); +} + +z_result_t zp_batch_stop(const z_loaned_session_t *zs) { + _z_session_t *session = _Z_RC_IN_VAL(zs); + if (_Z_RC_IS_NULL(zs)) { + return _Z_ERR_SESSION_CLOSED; + } + _z_transport_stop_batching(&session->_tp); + // Send remaining batch + return _z_send_n_batch(session, Z_CONGESTION_CONTROL_DEFAULT); +} +#endif +#endif + /**************** Tasks ****************/ void zp_task_read_options_default(zp_task_read_options_t *options) { #if Z_FEATURE_MULTI_THREAD == 1 @@ -1425,7 +1479,7 @@ z_result_t zp_start_read_task(z_loaned_session_t *zs, const zp_task_read_options zp_task_read_options_t opt; zp_task_read_options_default(&opt); if (options != NULL) { - opt.task_attributes = options->task_attributes; + opt = *options; } return _zp_start_read_task(_Z_RC_IN_VAL(zs), opt.task_attributes); #else @@ -1457,7 +1511,7 @@ z_result_t zp_start_lease_task(z_loaned_session_t *zs, const zp_task_lease_optio zp_task_lease_options_t opt; zp_task_lease_options_default(&opt); if (options != NULL) { - opt.task_attributes = options->task_attributes; + opt = *options; } return _zp_start_lease_task(_Z_RC_IN_VAL(zs), opt.task_attributes); #else diff --git a/src/api/liveliness.c b/src/api/liveliness.c index 7cc7f74b3..7c94f6fd2 100644 --- a/src/api/liveliness.c +++ b/src/api/liveliness.c @@ -67,7 +67,7 @@ z_result_t z_liveliness_declare_token(const z_loaned_session_t *zs, z_owned_live _z_keyexpr_t key = _z_update_keyexpr_to_declared(_Z_RC_IN_VAL(zs), *keyexpr); - return _z_declare_liveliness_token(zs, &token->_val, key); + return _z_declare_liveliness_token(zs, &token->_val, &key); } z_result_t z_liveliness_undeclare_token(z_moved_liveliness_token_t *token) { @@ -97,7 +97,7 @@ z_result_t z_liveliness_declare_subscriber(const z_loaned_session_t *zs, z_owned _z_keyexpr_t key = _z_update_keyexpr_to_declared(_Z_RC_IN_VAL(zs), *keyexpr); - _z_subscriber_t int_sub = _z_declare_liveliness_subscriber(zs, key, callback->_this._val.call, + _z_subscriber_t int_sub = _z_declare_liveliness_subscriber(zs, &key, callback->_this._val.call, callback->_this._val.drop, opt.history, ctx); z_internal_closure_sample_null(&callback->_this); @@ -108,7 +108,7 @@ z_result_t z_liveliness_declare_subscriber(const z_loaned_session_t *zs, z_owned } if (opt.history) { - z_result_t ret = _z_liveliness_subscription_trigger_history(_Z_RC_IN_VAL(zs), *keyexpr); + z_result_t ret = _z_liveliness_subscription_trigger_history(_Z_RC_IN_VAL(zs), keyexpr); if (ret != _Z_RES_OK) { return ret; } @@ -140,8 +140,8 @@ z_result_t z_liveliness_get(const z_loaned_session_t *zs, const z_loaned_keyexpr opt = *options; } - _z_keyexpr_t ke = _z_keyexpr_duplicate(*keyexpr); - ret = _z_liveliness_query(_Z_RC_IN_VAL(zs), ke, callback->_this._val.call, callback->_this._val.drop, ctx, + _z_keyexpr_t ke = _z_keyexpr_alias_from_user_defined(*keyexpr, true); + ret = _z_liveliness_query(_Z_RC_IN_VAL(zs), &ke, callback->_this._val.call, callback->_this._val.drop, ctx, opt.timeout_ms); z_internal_closure_reply_null( diff --git a/src/collections/arc_slice.c b/src/collections/arc_slice.c index 01f7d8038..28603aff9 100644 --- a/src/collections/arc_slice.c +++ b/src/collections/arc_slice.c @@ -19,19 +19,11 @@ #include #include -_z_arc_slice_t _z_arc_slice_empty(void) { - _z_arc_slice_t s; - s.len = 0; - s.start = 0; - s.slice = _z_slice_rc_null(); - return s; -} - _z_arc_slice_t _z_arc_slice_wrap(_z_slice_t s, size_t offset, size_t len) { assert(offset + len <= s.len); _z_arc_slice_t arc_s; - arc_s.slice = _z_slice_rc_new_from_val(&s); + arc_s.slice = _z_slice_simple_rc_new_from_val(&s); if (_Z_RC_IS_NULL(&arc_s.slice)) { return _z_arc_slice_empty(); } @@ -40,6 +32,15 @@ _z_arc_slice_t _z_arc_slice_wrap(_z_slice_t s, size_t offset, size_t len) { return arc_s; } +_z_arc_slice_t _z_arc_slice_wrap_slice_rc(_z_slice_simple_rc_t* slice_rc, size_t offset, size_t len) { + assert(offset + len <= _Z_RC_IN_VAL(slice_rc)->len); + _z_arc_slice_t arc_s; + arc_s.slice = _z_slice_simple_rc_clone(slice_rc); + arc_s.len = len; + arc_s.start = offset; + return arc_s; +} + _z_arc_slice_t _z_arc_slice_get_subslice(const _z_arc_slice_t* s, size_t offset, size_t len) { assert(offset + len <= s->len); assert(!_Z_RC_IS_NULL(&s->slice) || (len == 0 && offset == 0)); @@ -48,20 +49,16 @@ _z_arc_slice_t _z_arc_slice_get_subslice(const _z_arc_slice_t* s, size_t offset, return _z_arc_slice_empty(); } _z_arc_slice_t out; - out.slice = _z_slice_rc_clone(&s->slice); + out.slice = _z_slice_simple_rc_clone(&s->slice); out.len = len; out.start = s->start + offset; return out; } -size_t _z_arc_slice_len(const _z_arc_slice_t* s) { return s->len; } - -bool _z_arc_slice_is_empty(const _z_arc_slice_t* s) { return _z_arc_slice_len(s) == 0; } - const uint8_t* _z_arc_slice_data(const _z_arc_slice_t* s) { return _Z_RC_IN_VAL(&s->slice)->start + s->start; } z_result_t _z_arc_slice_copy(_z_arc_slice_t* dst, const _z_arc_slice_t* src) { - _z_slice_rc_copy(&dst->slice, &src->slice); + _z_slice_simple_rc_copy(&dst->slice, &src->slice); dst->len = src->len; dst->start = src->start; return _Z_RES_OK; @@ -73,12 +70,12 @@ z_result_t _z_arc_slice_move(_z_arc_slice_t* dst, _z_arc_slice_t* src) { dst->start = src->start; src->len = 0; src->start = 0; - src->slice = _z_slice_rc_null(); + src->slice = _z_slice_simple_rc_null(); return _Z_RES_OK; } z_result_t _z_arc_slice_drop(_z_arc_slice_t* s) { - _z_slice_rc_drop(&s->slice); - *s = _z_arc_slice_empty(); + _z_slice_simple_rc_drop(&s->slice); + s->len = 0; return _Z_RES_OK; } diff --git a/src/collections/bytes.c b/src/collections/bytes.c index 698e4bb5a..5f8ab571c 100644 --- a/src/collections/bytes.c +++ b/src/collections/bytes.c @@ -27,19 +27,14 @@ /*-------- Bytes --------*/ bool _z_bytes_check(const _z_bytes_t *bytes) { return !_z_bytes_is_empty(bytes); } -_z_bytes_t _z_bytes_null(void) { - _z_bytes_t b; - b._slices = _z_arc_slice_svec_make(0); - return b; +z_result_t _z_bytes_copy(_z_bytes_t *dst, const _z_bytes_t *src) { + return _z_arc_slice_svec_copy(&dst->_slices, &src->_slices, true); } -z_result_t _z_bytes_copy(_z_bytes_t *dst, const _z_bytes_t *src) { - _z_arc_slice_svec_copy(&dst->_slices, &src->_slices); - if (_z_arc_slice_svec_len(&dst->_slices) == _z_arc_slice_svec_len(&src->_slices)) { - return _Z_RES_OK; - } else { - return _Z_ERR_SYSTEM_OUT_OF_MEMORY; - } +_z_bytes_t _z_bytes_alias(const _z_bytes_t src) { + _z_bytes_t dst; + dst._slices = src._slices; + return dst; } _z_bytes_t _z_bytes_duplicate(const _z_bytes_t *src) { @@ -104,7 +99,7 @@ z_result_t _z_bytes_from_slice(_z_bytes_t *b, _z_slice_t s) { *b = _z_bytes_null(); _z_arc_slice_t arc_s = _z_arc_slice_wrap(s, 0, s.len); if (_z_arc_slice_len(&arc_s) != s.len) return _Z_ERR_SYSTEM_OUT_OF_MEMORY; - return _z_arc_slice_svec_append(&b->_slices, &arc_s) ? _Z_RES_OK : _Z_ERR_SYSTEM_OUT_OF_MEMORY; + return _z_arc_slice_svec_append(&b->_slices, &arc_s, true); } z_result_t _z_bytes_from_buf(_z_bytes_t *b, const uint8_t *src, size_t len) { @@ -136,9 +131,12 @@ z_result_t _z_bytes_to_slice(const _z_bytes_t *bytes, _z_slice_t *s) { } z_result_t _z_bytes_append_slice(_z_bytes_t *dst, _z_arc_slice_t *s) { - _Z_CLEAN_RETURN_IF_ERR(_z_arc_slice_svec_append(&dst->_slices, s) ? _Z_RES_OK : _Z_ERR_SYSTEM_OUT_OF_MEMORY, - _z_arc_slice_drop(s)); - return _Z_RES_OK; + z_result_t ret = _Z_RES_OK; + ret = _z_arc_slice_svec_append(&dst->_slices, s, true); + if (ret != _Z_RES_OK) { + _z_arc_slice_drop(s); + } + return ret; } z_result_t _z_bytes_append_bytes(_z_bytes_t *dst, _z_bytes_t *src) { @@ -161,11 +159,11 @@ _z_slice_t _z_bytes_try_get_contiguous(const _z_bytes_t *bs) { _z_arc_slice_t *arc_s = _z_bytes_get_slice(bs, 0); return _z_slice_alias_buf(_z_arc_slice_data(arc_s), _z_arc_slice_len(arc_s)); } - return _z_slice_empty(); + return _z_slice_null(); } void _z_bytes_move(_z_bytes_t *dst, _z_bytes_t *src) { - dst->_slices = src->_slices; + *dst = *src; *src = _z_bytes_null(); } diff --git a/src/collections/refcount.c b/src/collections/refcount.c index 7a44ab4e6..9a8681b5f 100644 --- a/src/collections/refcount.c +++ b/src/collections/refcount.c @@ -49,9 +49,9 @@ // c11 atomic variant #define _ZP_RC_CNT_TYPE _z_atomic(unsigned int) -#define _ZP_RC_OP_INIT_CNT(p) \ - _z_atomic_store_explicit(&(p)->_strong_cnt, (unsigned int)1, _z_memory_order_relaxed); \ - _z_atomic_store_explicit(&(p)->_weak_cnt, (unsigned int)1, _z_memory_order_relaxed); +#define _ZP_RC_OP_INIT_STRONG_CNT(p) \ + _z_atomic_store_explicit(&(p)->_strong_cnt, (unsigned int)1, _z_memory_order_relaxed); +#define _ZP_RC_OP_INIT_WEAK_CNT(p) _z_atomic_store_explicit(&(p)->_weak_cnt, (unsigned int)1, _z_memory_order_relaxed); #define _ZP_RC_OP_INCR_STRONG_CNT(p) \ _z_atomic_fetch_add_explicit(&(p)->_strong_cnt, (unsigned int)1, _z_memory_order_relaxed); #define _ZP_RC_OP_INCR_AND_CMP_WEAK(p, x) \ @@ -83,10 +83,11 @@ // c99 gcc sync builtin variant #define _ZP_RC_CNT_TYPE unsigned int -#define _ZP_RC_OP_INIT_CNT(p) \ +#define _ZP_RC_OP_INIT_STRONG_CNT(p) \ __sync_fetch_and_and(&(p)->_strong_cnt, (unsigned int)0); \ - __sync_fetch_and_add(&(p)->_strong_cnt, (unsigned int)1); \ - __sync_fetch_and_and(&(p)->_weak_cnt, (unsigned int)0); \ + __sync_fetch_and_add(&(p)->_strong_cnt, (unsigned int)1); +#define _ZP_RC_OP_INIT_WEAK_CNT(p) \ + __sync_fetch_and_and(&(p)->_weak_cnt, (unsigned int)0); \ __sync_fetch_and_add(&(p)->_weak_cnt, (unsigned int)1); #define _ZP_RC_OP_INCR_STRONG_CNT(p) __sync_fetch_and_add(&(p)->_strong_cnt, (unsigned int)1); #define _ZP_RC_OP_INCR_AND_CMP_WEAK(p, x) __sync_fetch_and_add(&(p)->_weak_cnt, (unsigned int)1) >= x @@ -117,7 +118,8 @@ // None variant #error "Multi-thread refcount in C99 only exists for GCC, use GCC or C11 or deactivate multi-thread" #define _ZP_RC_CNT_TYPE unsigned int -#define _ZP_RC_OP_INIT_CNT(p) +#define _ZP_RC_OP_INIT_STRONG_CNT(p) +#define _ZP_RC_OP_INIT_WEAK_CNT(p) #define _ZP_RC_OP_INCR_STRONG_CNT(p) #define _ZP_RC_OP_INCR_AND_CMP_WEAK(p, x) (x == 0) #define _ZP_RC_OP_DECR_AND_CMP_STRONG(p, x) (x == 0) @@ -136,9 +138,8 @@ // Single thread variant #define _ZP_RC_CNT_TYPE unsigned int -#define _ZP_RC_OP_INIT_CNT(p) \ - (p)->_strong_cnt = (unsigned int)1; \ - (p)->_weak_cnt = (unsigned int)1; +#define _ZP_RC_OP_INIT_STRONG_CNT(p) (p)->_strong_cnt = (unsigned int)1; +#define _ZP_RC_OP_INIT_WEAK_CNT(p) (p)->_weak_cnt = (unsigned int)1; #define _ZP_RC_OP_INCR_STRONG_CNT(p) p->_strong_cnt++; #define _ZP_RC_OP_INCR_AND_CMP_WEAK(p, x) p->_weak_cnt++ >= x #define _ZP_RC_OP_DECR_AND_CMP_STRONG(p, x) p->_strong_cnt-- > (unsigned int)x @@ -170,7 +171,8 @@ z_result_t _z_rc_init(void** cnt) { if ((*cnt) == NULL) { return _Z_ERR_SYSTEM_OUT_OF_MEMORY; } - _ZP_RC_OP_INIT_CNT((_z_inner_rc_t*)*cnt) + _ZP_RC_OP_INIT_STRONG_CNT((_z_inner_rc_t*)*cnt) + _ZP_RC_OP_INIT_WEAK_CNT((_z_inner_rc_t*)*cnt) return _Z_RES_OK; } @@ -220,3 +222,33 @@ z_result_t _z_rc_weak_upgrade(void* cnt) { return _upgrade((_z_inner_rc_t*)cnt); size_t _z_rc_weak_count(void* cnt) { return ((_z_inner_rc_t*)cnt)->_weak_cnt; } size_t _z_rc_strong_count(void* cnt) { return ((_z_inner_rc_t*)cnt)->_strong_cnt; } + +typedef struct { + _ZP_RC_CNT_TYPE _strong_cnt; +} _z_inner_simple_rc_t; + +z_result_t _z_simple_rc_init(void** cnt) { + *cnt = z_malloc(sizeof(_z_inner_simple_rc_t)); + if ((*cnt) == NULL) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + _ZP_RC_OP_INIT_STRONG_CNT((_z_inner_simple_rc_t*)*cnt) + return _Z_RES_OK; +} + +z_result_t _z_simple_rc_increase(void* cnt) { + _z_inner_simple_rc_t* c = (_z_inner_simple_rc_t*)cnt; + _ZP_RC_OP_INCR_STRONG_CNT(c); + return _Z_RES_OK; +} + +bool _z_simple_rc_decrease(void** cnt) { + _z_inner_simple_rc_t* c = (_z_inner_simple_rc_t*)*cnt; + if (_ZP_RC_OP_DECR_AND_CMP_STRONG(c, 1)) { + return false; + } + z_free(*cnt); + return true; +} + +size_t _z_simple_rc_strong_count(void* cnt) { return ((_z_inner_simple_rc_t*)cnt)->_strong_cnt; } diff --git a/src/collections/slice.c b/src/collections/slice.c index c6fe1171a..750c10f86 100644 --- a/src/collections/slice.c +++ b/src/collections/slice.c @@ -28,14 +28,6 @@ void _z_default_deleter(void *data, void *context) { z_free(data); } -_z_delete_context_t _z_delete_context_null(void) { return _z_delete_context_create(NULL, NULL); } - -bool _z_delete_context_is_null(const _z_delete_context_t *c) { return c->deleter == NULL; } - -_z_delete_context_t _z_delete_context_create(void (*deleter)(void *data, void *context), void *context) { - return (_z_delete_context_t){.deleter = deleter, .context = context}; -} - _z_delete_context_t _z_delete_context_default(void) { return _z_delete_context_create(_z_default_deleter, NULL); } void _z_delete_context_delete(_z_delete_context_t *c, void *data) { @@ -45,27 +37,16 @@ void _z_delete_context_delete(_z_delete_context_t *c, void *data) { } /*-------- Slice --------*/ -_z_slice_t _z_slice_empty(void) { - return (_z_slice_t){.start = NULL, .len = 0, ._delete_context = _z_delete_context_null()}; -} - z_result_t _z_slice_init(_z_slice_t *bs, size_t capacity) { - z_result_t ret = _Z_RES_OK; - - bs->start = capacity == 0 ? NULL : (uint8_t *)z_malloc(capacity); - if (bs->start != NULL) { - bs->len = capacity; - bs->_delete_context = _z_delete_context_default(); - } else { + bs->start = (uint8_t *)z_malloc(capacity); + if (bs->start == NULL) { bs->len = 0; bs->_delete_context = _z_delete_context_null(); + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; } - - if (bs->len != capacity) { - ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; - } - - return ret; + bs->len = capacity; + bs->_delete_context = _z_delete_context_default(); + return _Z_RES_OK; } _z_slice_t _z_slice_make(size_t capacity) { @@ -82,11 +63,6 @@ _z_slice_t _z_slice_from_buf_custom_deleter(const uint8_t *p, size_t len, _z_del return bs; } -_z_slice_t _z_slice_alias(const _z_slice_t *bs) { - _z_slice_t alias = {.len = bs->len, .start = bs->start, ._delete_context = _z_delete_context_null()}; - return alias; -} - _z_slice_t _z_slice_alias_buf(const uint8_t *p, size_t len) { return _z_slice_from_buf_custom_deleter(p, len, _z_delete_context_null()); } @@ -96,14 +72,8 @@ _z_slice_t _z_slice_copy_from_buf(const uint8_t *p, size_t len) { return _z_slice_duplicate(&bs); } -void _z_slice_reset(_z_slice_t *bs) { - bs->start = NULL; - bs->len = 0; - bs->_delete_context = _z_delete_context_null(); -} - void _z_slice_clear(_z_slice_t *bs) { - if ((bs->start != NULL)) { + if (bs->start != NULL) { _z_delete_context_delete(&bs->_delete_context, (void *)bs->start); } _z_slice_reset(bs); @@ -141,24 +111,19 @@ z_result_t _z_slice_n_copy(_z_slice_t *dst, const _z_slice_t *src, size_t offset } void _z_slice_move(_z_slice_t *dst, _z_slice_t *src) { - dst->start = src->start; - dst->len = src->len; - dst->_delete_context = src->_delete_context; - + *dst = *src; _z_slice_reset(src); } _z_slice_t _z_slice_duplicate(const _z_slice_t *src) { - _z_slice_t dst = _z_slice_empty(); + _z_slice_t dst = _z_slice_null(); _z_slice_copy(&dst, src); return dst; } -bool _z_slice_is_empty(const _z_slice_t *bs) { return bs->len == 0; } - _z_slice_t _z_slice_steal(_z_slice_t *b) { _z_slice_t ret = *b; - *b = _z_slice_empty(); + *b = _z_slice_null(); return ret; } bool _z_slice_eq(const _z_slice_t *left, const _z_slice_t *right) { diff --git a/src/collections/string.c b/src/collections/string.c index e734ee79a..236b85077 100644 --- a/src/collections/string.c +++ b/src/collections/string.c @@ -17,16 +17,10 @@ #include #include +#include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/pointers.h" /*-------- string --------*/ -_z_string_t _z_string_null(void) { - _z_string_t s = {._slice = _z_slice_empty()}; - return s; -} - -bool _z_string_check(const _z_string_t *value) { return !_z_slice_is_empty(&value->_slice); } - _z_string_t _z_string_copy_from_str(const char *value) { _z_string_t s; s._slice = _z_slice_copy_from_buf((uint8_t *)value, strlen(value)); @@ -39,6 +33,12 @@ _z_string_t _z_string_copy_from_substr(const char *value, size_t len) { return s; } +_z_string_t _z_string_alias_slice(const _z_slice_t *slice) { + _z_string_t s; + s._slice = _z_slice_alias(*slice); + return s; +} + _z_string_t _z_string_alias_str(const char *value) { _z_string_t s; s._slice = _z_slice_alias_buf((const uint8_t *)(value), strlen(value)); @@ -77,7 +77,7 @@ z_result_t _z_string_copy_substring(_z_string_t *dst, const _z_string_t *src, si return _z_slice_n_copy(&dst->_slice, &src->_slice, offset, len); } -void _z_string_move(_z_string_t *dst, _z_string_t *src) { *dst = _z_string_steal(src); } +void _z_string_move(_z_string_t *dst, _z_string_t *src) { _z_slice_move(&dst->_slice, &src->_slice); } _z_string_t _z_string_steal(_z_string_t *str) { _z_string_t ret; @@ -85,11 +85,6 @@ _z_string_t _z_string_steal(_z_string_t *str) { return ret; } -_z_string_t _z_string_alias(const _z_string_t *str) { - _z_string_t alias = {._slice = _z_slice_alias(&str->_slice)}; - return alias; -} - void _z_string_move_str(_z_string_t *dst, char *src) { *dst = _z_string_alias_str(src); } void _z_string_reset(_z_string_t *str) { _z_slice_reset(&str->_slice); } @@ -132,10 +127,10 @@ _z_string_t _z_string_convert_bytes_le(const _z_slice_t *bs) { } _z_string_t _z_string_preallocate(size_t len) { - _z_string_t s = _z_string_null(); - _z_slice_init(&s._slice, len); - if (_z_slice_is_empty(&s._slice)) { - return _z_string_null(); + _z_string_t s; + // As long as _z_string_t is only a slice, no need to do anything more + if (_z_slice_init(&s._slice, len) != _Z_RES_OK) { + _Z_ERROR("String allocation failed"); } return s; } diff --git a/src/collections/vec.c b/src/collections/vec.c index 6556d6b88..c03f18f09 100644 --- a/src/collections/vec.c +++ b/src/collections/vec.c @@ -20,12 +20,12 @@ /*-------- vec --------*/ _z_vec_t _z_vec_make(size_t capacity) { - _z_vec_t v = {._capacity = capacity, ._len = 0, ._val = NULL}; + _z_vec_t v = {0}; if (capacity != 0) { v._val = (void **)z_malloc(sizeof(void *) * capacity); - } - if (v._val != NULL) { - v._capacity = capacity; + if (v._val != NULL) { + v._capacity = capacity; + } } return v; } @@ -41,6 +41,11 @@ void _z_vec_copy(_z_vec_t *dst, const _z_vec_t *src, z_element_clone_f d_f) { } } +void _z_vec_move(_z_vec_t *dst, _z_vec_t *src) { + *dst = *src; + *src = _z_vec_null(); +} + void _z_vec_reset(_z_vec_t *v, z_element_free_f free_f) { for (size_t i = 0; i < v->_len; i++) { free_f(&v->_val[i]); @@ -130,7 +135,7 @@ void _z_vec_remove(_z_vec_t *v, size_t pos, z_element_free_f free_f) { /*-------- svec --------*/ _z_svec_t _z_svec_make(size_t capacity, size_t element_size) { - _z_svec_t v = {._capacity = 0, ._len = 0, ._val = NULL}; + _z_svec_t v = _z_svec_null(); if (capacity != 0) { v._val = z_malloc(element_size * capacity); } @@ -140,40 +145,51 @@ _z_svec_t _z_svec_make(size_t capacity, size_t element_size) { return v; } -void __z_svec_copy_inner(void *dst, const void *src, z_element_copy_f copy, size_t num_elements, size_t element_size) { - if (copy == NULL) { - memcpy(dst, src, num_elements * element_size); - } else { - size_t offset = 0; - for (size_t i = 0; i < num_elements; i++) { - copy((uint8_t *)dst + offset, (uint8_t *)src + offset); - offset += element_size; - } - } +void _z_svec_init(_z_svec_t *v, size_t offset, size_t element_size) { + assert(offset <= v->_capacity); + void *start = _z_svec_get_mut(v, offset, element_size); + memset(start, 0, (v->_capacity - offset) * element_size); } -void __z_svec_move_inner(void *dst, void *src, z_element_move_f move, size_t num_elements, size_t element_size) { - if (move == NULL) { - memcpy(dst, src, num_elements * element_size); - } else { +static inline void __z_svec_move_inner(void *dst, void *src, z_element_move_f move, size_t num_elements, + size_t element_size, bool use_elem_f) { + if (use_elem_f) { size_t offset = 0; for (size_t i = 0; i < num_elements; i++) { move((uint8_t *)dst + offset, (uint8_t *)src + offset); offset += element_size; } + } else { + memmove(dst, src, num_elements * element_size); } } -bool _z_svec_copy(_z_svec_t *dst, const _z_svec_t *src, z_element_copy_f copy, size_t element_size) { +void _z_svec_move(_z_svec_t *dst, _z_svec_t *src) { + *dst = *src; + *src = _z_svec_null(); +} + +z_result_t _z_svec_copy(_z_svec_t *dst, const _z_svec_t *src, z_element_copy_f copy, size_t element_size, + bool use_elem_f) { dst->_capacity = 0; dst->_len = 0; dst->_val = z_malloc(element_size * src->_capacity); - if (dst->_val != NULL) { - dst->_capacity = src->_capacity; - dst->_len = src->_len; - __z_svec_copy_inner(dst->_val, src->_val, copy, src->_len, element_size); + if (dst->_val == NULL) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; } - return dst->_len == src->_len; + dst->_capacity = src->_capacity; + dst->_len = src->_len; + // Copy data to new vector + if (use_elem_f) { + size_t offset = 0; + for (size_t i = 0; i < src->_len; i++) { + copy((uint8_t *)dst->_val + offset, (uint8_t *)src->_val + offset); + offset += element_size; + } + } else { + memcpy(dst->_val, src->_val, src->_len * element_size); + } + return _Z_RES_OK; } void _z_svec_reset(_z_svec_t *v, z_element_clear_f clear, size_t element_size) { @@ -189,15 +205,15 @@ void _z_svec_reset(_z_svec_t *v, z_element_clear_f clear, size_t element_size) { void _z_svec_clear(_z_svec_t *v, z_element_clear_f clear_f, size_t element_size) { _z_svec_reset(v, clear_f, element_size); - _z_svec_release(v); + if (!v->_aliased) { + _z_svec_release(v); + } } void _z_svec_release(_z_svec_t *v) { z_free(v->_val); - v->_val = NULL; - v->_capacity = 0; - v->_len = 0; + v->_val = NULL; } void _z_svec_free(_z_svec_t **v, z_element_clear_f clear, size_t element_size) { @@ -215,29 +231,32 @@ size_t _z_svec_len(const _z_svec_t *v) { return v->_len; } bool _z_svec_is_empty(const _z_svec_t *v) { return v->_len == 0; } -bool _z_svec_append(_z_svec_t *v, const void *e, z_element_move_f move, size_t element_size) { - if (v->_len == v->_capacity) { - // Allocate a new vector - size_t _capacity = v->_capacity == 0 ? 1 : (v->_capacity << 1); - void *_val = (void *)z_malloc(_capacity * element_size); - if (_val != NULL) { - __z_svec_move_inner(_val, v->_val, move, v->_len, element_size); - // Free the old data - z_free(v->_val); +z_result_t _z_svec_expand(_z_svec_t *v, z_element_move_f move, size_t element_size, bool use_elem_f) { + // Allocate a new vector + size_t _capacity = v->_capacity << 1; + void *_val = (void *)z_malloc(_capacity * element_size); + if (_val == NULL) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + // Move and clear old data + __z_svec_move_inner(_val, v->_val, move, v->_len, element_size, use_elem_f); + z_free(v->_val); + // Update the current vector + v->_val = _val; + v->_capacity = _capacity; + return _Z_RES_OK; +} - // Update the current vector - v->_val = _val; - v->_capacity = _capacity; - memcpy((uint8_t *)v->_val + v->_len * element_size, e, element_size); - v->_len++; - } else { - return false; - } - } else { - memcpy((uint8_t *)v->_val + v->_len * element_size, e, element_size); - v->_len++; +z_result_t _z_svec_append(_z_svec_t *v, const void *e, z_element_move_f move, size_t element_size, bool use_elem_f) { + if (v->_capacity == 0) { + *v = _z_svec_make(1, element_size); + } else if (v->_len == v->_capacity) { + _Z_RETURN_IF_ERR(_z_svec_expand(v, move, element_size, use_elem_f)); } - return true; + // Append element + memcpy((uint8_t *)v->_val + v->_len * element_size, e, element_size); + v->_len++; + return _Z_RES_OK; } void *_z_svec_get(const _z_svec_t *v, size_t i, size_t element_size) { @@ -245,17 +264,20 @@ void *_z_svec_get(const _z_svec_t *v, size_t i, size_t element_size) { return (uint8_t *)v->_val + i * element_size; } +void *_z_svec_get_mut(_z_svec_t *v, size_t i, size_t element_size) { return (uint8_t *)v->_val + i * element_size; } + void _z_svec_set(_z_svec_t *v, size_t i, void *e, z_element_clear_f clear, size_t element_size) { assert(i < v->_len); clear((uint8_t *)v->_val + i * element_size); memcpy((uint8_t *)v->_val + i * element_size, e, element_size); } -void _z_svec_remove(_z_svec_t *v, size_t pos, z_element_clear_f clear, z_element_move_f move, size_t element_size) { +void _z_svec_remove(_z_svec_t *v, size_t pos, z_element_clear_f clear, z_element_move_f move, size_t element_size, + bool use_elem_f) { assert(pos < v->_len); clear((uint8_t *)v->_val + pos * element_size); __z_svec_move_inner((uint8_t *)v->_val + pos * element_size, (uint8_t *)v->_val + (pos + 1) * element_size, move, - (v->_len - pos - 1) * element_size, element_size); + (v->_len - pos - 1) * element_size, element_size, use_elem_f); v->_len--; } diff --git a/src/link/link.c b/src/link/link.c index 197d68212..8c5ffb312 100644 --- a/src/link/link.c +++ b/src/link/link.c @@ -27,13 +27,10 @@ z_result_t _z_open_link(_z_link_t *zl, _z_string_t *locator) { _z_endpoint_t ep; ret = _z_endpoint_from_string(&ep, locator); if (ret == _Z_RES_OK) { - // TODO[peer]: when peer unicast mode is supported, this must be revisited // Create transport link -#if Z_FEATURE_LINK_TCP == 1 if (_z_endpoint_tcp_valid(&ep) == _Z_RES_OK) { ret = _z_new_link_tcp(zl, &ep); } else -#endif #if Z_FEATURE_LINK_UDP_UNICAST == 1 if (_z_endpoint_udp_unicast_valid(&ep) == _Z_RES_OK) { ret = _z_new_link_udp_unicast(zl, ep); @@ -80,10 +77,12 @@ z_result_t _z_listen_link(_z_link_t *zl, _z_string_t *locator) { _z_endpoint_t ep; ret = _z_endpoint_from_string(&ep, locator); if (ret == _Z_RES_OK) { - // TODO[peer]: when peer unicast mode is supported, this must be revisited // Create transport link + if (_z_endpoint_tcp_valid(&ep) == _Z_RES_OK) { + ret = _z_new_link_tcp(zl, &ep); + } else #if Z_FEATURE_LINK_UDP_MULTICAST == 1 - if (_z_endpoint_udp_multicast_valid(&ep) == _Z_RES_OK) { + if (_z_endpoint_udp_multicast_valid(&ep) == _Z_RES_OK) { ret = _z_new_link_udp_multicast(zl, ep); } else #endif @@ -153,17 +152,8 @@ size_t _z_link_recv_exact_zbuf(const _z_link_t *link, _z_zbuf_t *zbf, size_t len z_result_t _z_link_send_wbuf(const _z_link_t *link, const _z_wbuf_t *wbf) { z_result_t ret = _Z_RES_OK; - bool link_is_streamed = false; - - switch (link->_cap._flow) { - case Z_LINK_CAP_FLOW_STREAM: - link_is_streamed = true; - break; - case Z_LINK_CAP_FLOW_DATAGRAM: - default: - link_is_streamed = false; - break; - } + bool link_is_streamed = link->_cap._flow == Z_LINK_CAP_FLOW_STREAM; + for (size_t i = 0; (i < _z_wbuf_len_iosli(wbf)) && (ret == _Z_RES_OK); i++) { _z_slice_t bs = _z_iosli_to_bytes(_z_wbuf_get_iosli(wbf, i)); size_t n = bs.len; diff --git a/src/link/multicast/bt.c b/src/link/multicast/bt.c index 0b26d3643..84216e9bb 100644 --- a/src/link/multicast/bt.c +++ b/src/link/multicast/bt.c @@ -104,8 +104,8 @@ size_t _z_f_link_write_all_bt(const _z_link_t *self, const uint8_t *ptr, size_t size_t _z_f_link_read_bt(const _z_link_t *self, uint8_t *ptr, size_t len, _z_slice_t *addr) { size_t rb = _z_read_bt(self->_socket._bt._sock, ptr, len); if ((rb > (size_t)0) && (addr != NULL)) { - *addr = _z_slice_make(strlen(self->_socket._bt._gname)); - (void)memcpy((uint8_t *)addr->start, self->_socket._bt._gname, addr->len); + addr->len = strlen(self->_socket._bt._gname); + (void)memcpy((uint8_t *)addr->start, self->_socket._bt._gname, strlen(self->_socket._bt._gname)); } return rb; @@ -114,8 +114,8 @@ size_t _z_f_link_read_bt(const _z_link_t *self, uint8_t *ptr, size_t len, _z_sli size_t _z_f_link_read_exact_bt(const _z_link_t *self, uint8_t *ptr, size_t len, _z_slice_t *addr) { size_t rb = _z_read_exact_bt(self->_socket._bt._sock, ptr, len); if ((rb == len) && (addr != NULL)) { - *addr = _z_slice_make(strlen(self->_socket._bt._gname)); - (void)memcpy((uint8_t *)addr->start, self->_socket._bt._gname, addr->len); + addr->len = strlen(self->_socket._bt._gname); + (void)memcpy((uint8_t *)addr->start, self->_socket._bt._gname, strlen(self->_socket._bt._gname)); } return rb; diff --git a/src/link/unicast/tcp.c b/src/link/unicast/tcp.c index d55cf7c31..232b7ffdc 100644 --- a/src/link/unicast/tcp.c +++ b/src/link/unicast/tcp.c @@ -188,4 +188,15 @@ z_result_t _z_new_link_tcp(_z_link_t *zl, _z_endpoint_t *endpoint) { return ret; } +#else +z_result_t _z_endpoint_tcp_valid(_z_endpoint_t *endpoint) { + _ZP_UNUSED(endpoint); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} + +z_result_t _z_new_link_tcp(_z_link_t *zl, _z_endpoint_t *endpoint) { + _ZP_UNUSED(zl); + _ZP_UNUSED(endpoint); + return _Z_ERR_TRANSPORT_NOT_AVAILABLE; +} #endif diff --git a/src/net/encoding.c b/src/net/encoding.c index 5926f4657..49d2e4794 100644 --- a/src/net/encoding.c +++ b/src/net/encoding.c @@ -16,9 +16,15 @@ #include #include "zenoh-pico/api/constants.h" +#include "zenoh-pico/collections/string.h" #include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/result.h" +_z_encoding_t _z_encoding_wrap(uint16_t id, const char *schema) { + return (_z_encoding_t){.id = id, + .schema = (schema == NULL) ? _z_string_null() : _z_string_alias_str((char *)schema)}; +} + z_result_t _z_encoding_make(_z_encoding_t *encoding, uint16_t id, const char *schema, size_t len) { encoding->id = id; // Clone schema @@ -33,30 +39,41 @@ z_result_t _z_encoding_make(_z_encoding_t *encoding, uint16_t id, const char *sc return _Z_RES_OK; } -_z_encoding_t _z_encoding_wrap(uint16_t id, const char *schema) { - return (_z_encoding_t){.id = id, - .schema = (schema == NULL) ? _z_string_null() : _z_string_alias_str((char *)schema)}; -} - -_z_encoding_t _z_encoding_null(void) { return _z_encoding_wrap(_Z_ENCODING_ID_DEFAULT, NULL); } - -void _z_encoding_clear(_z_encoding_t *encoding) { _z_string_clear(&encoding->schema); } - -bool _z_encoding_check(const _z_encoding_t *encoding) { - return ((encoding->id != _Z_ENCODING_ID_DEFAULT) || _z_string_check(&encoding->schema)); +void _z_encoding_clear(_z_encoding_t *encoding) { + if (_z_string_check(&encoding->schema)) { + _z_string_clear(&encoding->schema); + } } z_result_t _z_encoding_copy(_z_encoding_t *dst, const _z_encoding_t *src) { - *dst = _z_encoding_null(); - _Z_RETURN_IF_ERR(_z_string_copy(&dst->schema, &src->schema)); dst->id = src->id; + if (_z_string_check(&src->schema)) { + _Z_RETURN_IF_ERR(_z_string_copy(&dst->schema, &src->schema)); + } else { + dst->schema = _z_string_null(); + } return _Z_RES_OK; } +_z_encoding_t _z_encoding_alias(_z_encoding_t src) { + _z_encoding_t dst; + dst.id = src.id; + if (_z_string_check(&src.schema)) { + dst.schema = _z_string_alias(src.schema); + } else { + dst.schema = _z_string_null(); + } + return dst; +} + void _z_encoding_move(_z_encoding_t *dst, _z_encoding_t *src) { dst->id = src->id; src->id = _Z_ENCODING_ID_DEFAULT; - _z_string_move(&dst->schema, &src->schema); + if (_z_string_check(&src->schema)) { + _z_string_move(&dst->schema, &src->schema); + } else { + dst->schema = _z_string_null(); + } } _z_encoding_t _z_encoding_steal(_z_encoding_t *val) { diff --git a/src/net/filtering.c b/src/net/filtering.c index d3be65236..022c6c798 100644 --- a/src/net/filtering.c +++ b/src/net/filtering.c @@ -96,9 +96,10 @@ z_result_t _z_write_filter_create(_z_publisher_t *pub) { z_result_t _z_write_filter_destroy(_z_publisher_t *pub) { if (pub->_filter.ctx != NULL) { - _Z_RETURN_IF_ERR(_z_remove_interest(_Z_RC_IN_VAL(&pub->_zn), pub->_filter._interest_id)); + z_result_t res = _z_remove_interest(_Z_RC_IN_VAL(&pub->_zn), pub->_filter._interest_id); z_free(pub->_filter.ctx); pub->_filter.ctx = NULL; + return res; } return _Z_RES_OK; } diff --git a/src/net/liveliness.c b/src/net/liveliness.c index 872c4d9c7..5348bcbb1 100644 --- a/src/net/liveliness.c +++ b/src/net/liveliness.c @@ -21,6 +21,7 @@ #include "zenoh-pico/session/session.h" #include "zenoh-pico/session/subscription.h" #include "zenoh-pico/session/utils.h" +#include "zenoh-pico/transport/common/tx.h" #include "zenoh-pico/utils/result.h" #if Z_FEATURE_LIVELINESS == 1 @@ -28,12 +29,12 @@ /**************** Liveliness Token ****************/ z_result_t _z_declare_liveliness_token(const _z_session_rc_t *zn, _z_liveliness_token_t *ret_token, - _z_keyexpr_t keyexpr) { + _z_keyexpr_t *keyexpr) { z_result_t ret; uint32_t id = _z_get_entity_id(_Z_RC_IN_VAL(zn)); - _z_declaration_t declaration = _z_make_decl_token(&keyexpr, id); + _z_declaration_t declaration = _z_make_decl_token(keyexpr, id); _z_network_message_t n_msg = _z_n_msg_make_declare(declaration, false, 0); ret = _z_send_n_msg(_Z_RC_IN_VAL(zn), &n_msg, Z_RELIABILITY_RELIABLE, Z_CONGESTION_CONTROL_BLOCK); _z_n_msg_clear(&n_msg); @@ -41,7 +42,7 @@ z_result_t _z_declare_liveliness_token(const _z_session_rc_t *zn, _z_liveliness_ _z_liveliness_register_token(_Z_RC_IN_VAL(zn), id, keyexpr); ret_token->_id = id; - _z_keyexpr_move(&ret_token->_key, &keyexpr); + _z_keyexpr_move(&ret_token->_key, keyexpr); ret_token->_zn = _z_session_rc_clone_as_weak(zn); return ret; } @@ -66,13 +67,13 @@ z_result_t _z_undeclare_liveliness_token(_z_liveliness_token_t *token) { /**************** Liveliness Subscriber ****************/ #if Z_FEATURE_SUBSCRIPTION == 1 -_z_subscriber_t _z_declare_liveliness_subscriber(const _z_session_rc_t *zn, _z_keyexpr_t keyexpr, +_z_subscriber_t _z_declare_liveliness_subscriber(const _z_session_rc_t *zn, _z_keyexpr_t *keyexpr, _z_closure_sample_callback_t callback, _z_drop_handler_t dropper, bool history, void *arg) { _z_subscription_t s; s._id = _z_get_entity_id(_Z_RC_IN_VAL(zn)); - s._key_id = keyexpr._id; - s._key = _z_get_expanded_key_from_key(_Z_RC_IN_VAL(zn), &keyexpr); + s._key_id = keyexpr->_id; + s._key = _z_get_expanded_key_from_key(_Z_RC_IN_VAL(zn), keyexpr); s._callback = callback; s._dropper = dropper; s._arg = arg; @@ -88,7 +89,7 @@ _z_subscriber_t _z_declare_liveliness_subscriber(const _z_session_rc_t *zn, _z_k // Build the declare message to send on the wire uint8_t mode = history ? (_Z_INTEREST_FLAG_CURRENT | _Z_INTEREST_FLAG_FUTURE) : _Z_INTEREST_FLAG_FUTURE; _z_interest_t interest = _z_make_interest( - &keyexpr, s._id, _Z_INTEREST_FLAG_KEYEXPRS | _Z_INTEREST_FLAG_TOKENS | _Z_INTEREST_FLAG_RESTRICTED | mode); + keyexpr, s._id, _Z_INTEREST_FLAG_KEYEXPRS | _Z_INTEREST_FLAG_TOKENS | _Z_INTEREST_FLAG_RESTRICTED | mode); _z_network_message_t n_msg = _z_n_msg_make_interest(interest); if (_z_send_n_msg(_Z_RC_IN_VAL(zn), &n_msg, Z_RELIABILITY_RELIABLE, Z_CONGESTION_CONTROL_BLOCK) != _Z_RES_OK) { @@ -131,7 +132,7 @@ z_result_t _z_undeclare_liveliness_subscriber(_z_subscriber_t *sub) { /**************** Liveliness Query ****************/ #if Z_FEATURE_QUERY == 1 -z_result_t _z_liveliness_query(_z_session_t *zn, _z_keyexpr_t keyexpr, _z_closure_reply_callback_t callback, +z_result_t _z_liveliness_query(_z_session_t *zn, const _z_keyexpr_t *keyexpr, _z_closure_reply_callback_t callback, _z_drop_handler_t dropper, void *arg, uint64_t timeout_ms) { z_result_t ret = _Z_RES_OK; @@ -140,7 +141,7 @@ z_result_t _z_liveliness_query(_z_session_t *zn, _z_keyexpr_t keyexpr, _z_closur (_z_liveliness_pending_query_t *)z_malloc(sizeof(_z_liveliness_pending_query_t)); if (pq != NULL) { uint32_t id = _z_liveliness_get_query_id(zn); - pq->_key = _z_get_expanded_key_from_key(zn, &keyexpr); + pq->_key = _z_get_expanded_key_from_key(zn, keyexpr); pq->_callback = callback; pq->_dropper = dropper; pq->_arg = arg; @@ -148,8 +149,8 @@ z_result_t _z_liveliness_query(_z_session_t *zn, _z_keyexpr_t keyexpr, _z_closur ret = _z_liveliness_register_pending_query(zn, id, pq); if (ret == _Z_RES_OK) { _ZP_UNUSED(timeout_ms); // Current interest in pico don't support timeout - - _z_interest_t interest = _z_make_interest(&keyexpr, id, + _z_keyexpr_t key = _z_keyexpr_alias(*keyexpr); + _z_interest_t interest = _z_make_interest(&key, id, _Z_INTEREST_FLAG_KEYEXPRS | _Z_INTEREST_FLAG_TOKENS | _Z_INTEREST_FLAG_RESTRICTED | _Z_INTEREST_FLAG_CURRENT); diff --git a/src/net/primitives.c b/src/net/primitives.c index 13a78eea8..d1909188f 100644 --- a/src/net/primitives.c +++ b/src/net/primitives.c @@ -34,6 +34,7 @@ #include "zenoh-pico/session/session.h" #include "zenoh-pico/session/subscription.h" #include "zenoh-pico/session/utils.h" +#include "zenoh-pico/transport/common/tx.h" #include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/result.h" @@ -130,7 +131,7 @@ _z_publisher_t _z_declare_publisher(const _z_session_rc_t *zn, _z_keyexpr_t keye // Allocate publisher _z_publisher_t ret; // Fill publisher - ret._key = _z_keyexpr_duplicate(keyexpr); + ret._key = _z_keyexpr_duplicate(&keyexpr); ret._id = _z_get_entity_id(_Z_RC_IN_VAL(zn)); ret._congestion_control = congestion_control; ret._priority = priority; @@ -492,6 +493,8 @@ z_result_t _z_query(_z_session_t *zn, _z_keyexpr_t keyexpr, const char *paramete pq->_dropper = dropper; pq->_pending_replies = NULL; pq->_arg = arg; + pq->_timeout = timeout_ms; + pq->_start_time = z_clock_now(); ret = _z_register_pending_query(zn, pq); // Add the pending query to the current session if (ret == _Z_RES_OK) { diff --git a/src/net/publish.c b/src/net/publish.c index d81893d0b..13900de39 100644 --- a/src/net/publish.c +++ b/src/net/publish.c @@ -35,17 +35,4 @@ void _z_publisher_free(_z_publisher_t **pub) { } } -bool _z_publisher_check(const _z_publisher_t *publisher) { return !_Z_RC_IS_NULL(&publisher->_zn); } -_z_publisher_t _z_publisher_null(void) { - return (_z_publisher_t){._congestion_control = Z_CONGESTION_CONTROL_DEFAULT, - ._id = 0, - ._key = _z_keyexpr_null(), - ._priority = Z_PRIORITY_DEFAULT, - ._zn = _z_session_weak_null(), - ._encoding = _z_encoding_null(), -#if Z_FEATURE_INTEREST == 1 - ._filter = (_z_write_filter_t){._interest_id = 0, .ctx = NULL} -#endif - }; -} #endif diff --git a/src/net/query.c b/src/net/query.c index 75bde3f41..817e94a59 100644 --- a/src/net/query.c +++ b/src/net/query.c @@ -13,65 +13,39 @@ #include "zenoh-pico/net/query.h" +#include "zenoh-pico/net/session.h" #include "zenoh-pico/session/utils.h" +#include "zenoh-pico/transport/common/tx.h" #include "zenoh-pico/utils/logging.h" -_z_query_t _z_query_null(void) { - return (_z_query_t){ - ._anyke = false, - ._key = _z_keyexpr_null(), - ._parameters = NULL, - ._request_id = 0, - ._value = _z_value_null(), - .attachment = _z_bytes_null(), - ._zn = _z_session_weak_null(), - }; -} - -void _z_query_clear_inner(_z_query_t *q) { +static void _z_query_clear_inner(_z_query_t *q) { _z_keyexpr_clear(&q->_key); _z_value_clear(&q->_value); - _z_bytes_drop(&q->attachment); - z_free(q->_parameters); + _z_bytes_drop(&q->_attachment); + _z_string_clear(&q->_parameters); _z_session_weak_drop(&q->_zn); } -void _z_query_clear(_z_query_t *q) { +z_result_t _z_query_send_reply_final(_z_query_t *q) { // Try to upgrade session weak to rc _z_session_rc_t sess_rc = _z_session_weak_upgrade_if_open(&q->_zn); - if (!_Z_RC_IS_NULL(&sess_rc)) { - // Send REPLY_FINAL message - _z_zenoh_message_t z_msg = _z_n_msg_make_response_final(q->_request_id); - if (_z_send_n_msg(_Z_RC_IN_VAL(&q->_zn), &z_msg, Z_RELIABILITY_RELIABLE, Z_CONGESTION_CONTROL_BLOCK) != - _Z_RES_OK) { - _Z_ERROR("Query send REPLY_FINAL transport failure !"); - } - _z_msg_clear(&z_msg); - _z_session_rc_drop(&sess_rc); + if (_Z_RC_IS_NULL(&sess_rc)) { + return _Z_ERR_TRANSPORT_TX_FAILED; } - // Clean up memory - _z_query_clear_inner(q); + _z_zenoh_message_t z_msg = _z_n_msg_make_response_final(q->_request_id); + z_result_t ret = _z_send_n_msg(_Z_RC_IN_VAL(&sess_rc), &z_msg, Z_RELIABILITY_RELIABLE, Z_CONGESTION_CONTROL_BLOCK); + _z_msg_clear(&z_msg); + _z_session_rc_drop(&sess_rc); + return ret; } -z_result_t _z_query_copy(_z_query_t *dst, const _z_query_t *src) { - *dst = _z_query_null(); - _Z_RETURN_IF_ERR(_z_keyexpr_copy(&dst->_key, &src->_key)); - _Z_CLEAN_RETURN_IF_ERR(_z_value_copy(&dst->_value, &src->_value), _z_query_clear_inner(dst)); - _Z_CLEAN_RETURN_IF_ERR(_z_bytes_copy(&dst->attachment, &src->attachment), _z_query_clear_inner(dst)); - dst->_parameters = _z_str_clone(src->_parameters); - if (dst->_parameters == NULL && src->_parameters != NULL) { - _z_query_clear_inner(dst); - return _Z_ERR_SYSTEM_OUT_OF_MEMORY; - } - _z_session_weak_copy(&dst->_zn, &src->_zn); - if (_Z_RC_IS_NULL(&dst->_zn)) { - _z_query_clear_inner(dst); - return _Z_ERR_SYSTEM_OUT_OF_MEMORY; +void _z_query_clear(_z_query_t *q) { + // Send REPLY_FINAL message + if (_z_query_send_reply_final(q) != _Z_RES_OK) { + _Z_ERROR("Query send REPLY_FINAL transport failure !"); } - dst->_anyke = src->_anyke; - dst->_request_id = src->_request_id; - dst->_zn = src->_zn; - return _Z_RES_OK; + // Clean up memory + _z_query_clear_inner(q); } void _z_query_free(_z_query_t **query) { @@ -84,25 +58,6 @@ void _z_query_free(_z_query_t **query) { } #if Z_FEATURE_QUERYABLE == 1 -_z_query_t _z_query_create(_z_value_t *value, _z_keyexpr_t *key, const _z_slice_t *parameters, _z_session_rc_t *zsrc, - uint32_t request_id, const _z_bytes_t attachment) { - _z_query_t q = _z_query_null(); - q._request_id = request_id; - q._zn = _z_session_rc_clone_as_weak(zsrc); - q._parameters = (char *)z_malloc(parameters->len + 1); - memcpy(q._parameters, parameters->start, parameters->len); - q._parameters[parameters->len] = 0; - q._anyke = (strstr(q._parameters, Z_SELECTOR_QUERY_MATCH) == NULL) ? false : true; - q._key = _z_keyexpr_steal(key); - _z_bytes_copy(&q.attachment, &attachment); - _z_value_move(&q._value, value); - return q; -} - -_z_queryable_t _z_queryable_null(void) { return (_z_queryable_t){._entity_id = 0, ._zn = _z_session_weak_null()}; } - -bool _z_queryable_check(const _z_queryable_t *queryable) { return !_Z_RC_IS_NULL(&queryable->_zn); } - void _z_queryable_clear(_z_queryable_t *qbl) { _z_session_weak_drop(&qbl->_zn); *qbl = _z_queryable_null(); diff --git a/src/net/reply.c b/src/net/reply.c index b9038af2e..9c06b2674 100644 --- a/src/net/reply.c +++ b/src/net/reply.c @@ -17,15 +17,6 @@ #include "zenoh-pico/session/utils.h" #include "zenoh-pico/utils/logging.h" -_z_reply_data_t _z_reply_data_null(void) { - return (_z_reply_data_t){.replier_id = {.id = {0}}, ._result.sample = _z_sample_null(), ._tag = _Z_REPLY_TAG_NONE}; -} - -_z_reply_t _z_reply_null(void) { - _z_reply_t r = {.data = _z_reply_data_null()}; - return r; -} - #if Z_FEATURE_QUERY == 1 void _z_reply_data_clear(_z_reply_data_t *reply_data) { if (reply_data->_tag == _Z_REPLY_TAG_DATA) { @@ -48,14 +39,13 @@ void _z_reply_data_free(_z_reply_data_t **reply_data) { } z_result_t _z_reply_data_copy(_z_reply_data_t *dst, const _z_reply_data_t *src) { - *dst = _z_reply_data_null(); if (src->_tag == _Z_REPLY_TAG_DATA) { _Z_RETURN_IF_ERR(_z_sample_copy(&dst->_result.sample, &src->_result.sample)); } else if (src->_tag == _Z_REPLY_TAG_ERROR) { _Z_RETURN_IF_ERR(_z_value_copy(&dst->_result.error, &src->_result.error)); } - dst->replier_id = src->replier_id; dst->_tag = src->_tag; + dst->replier_id = src->replier_id; return _Z_RES_OK; } @@ -78,11 +68,7 @@ void _z_reply_free(_z_reply_t **reply) { } } -z_result_t _z_reply_copy(_z_reply_t *dst, const _z_reply_t *src) { - *dst = _z_reply_null(); - _Z_RETURN_IF_ERR(_z_reply_data_copy(&dst->data, &src->data)); - return _Z_RES_OK; -} +z_result_t _z_reply_copy(_z_reply_t *dst, const _z_reply_t *src) { return _z_reply_data_copy(&dst->data, &src->data); } bool _z_pending_reply_eq(const _z_pending_reply_t *one, const _z_pending_reply_t *two) { return one->_tstamp.time == two->_tstamp.time; @@ -96,46 +82,4 @@ void _z_pending_reply_clear(_z_pending_reply_t *pr) { _z_timestamp_clear(&pr->_tstamp); } -_z_reply_t _z_reply_create(_z_keyexpr_t keyexpr, _z_id_t id, const _z_bytes_t payload, const _z_timestamp_t *timestamp, - _z_encoding_t *encoding, z_sample_kind_t kind, const _z_bytes_t attachment) { - _z_reply_t reply = _z_reply_null(); - reply.data._tag = _Z_REPLY_TAG_DATA; - reply.data.replier_id = id; - - // Create reply sample - reply.data._result.sample.keyexpr = _z_keyexpr_steal(&keyexpr); - reply.data._result.sample.kind = kind; - reply.data._result.sample.timestamp = _z_timestamp_duplicate(timestamp); - _z_bytes_copy(&reply.data._result.sample.payload, &payload); - _z_bytes_copy(&reply.data._result.sample.attachment, &attachment); - _z_encoding_move(&reply.data._result.sample.encoding, encoding); - - return reply; -} - -_z_reply_t _z_reply_err_create(const _z_bytes_t payload, _z_encoding_t *encoding) { - _z_reply_t reply = _z_reply_null(); - reply.data._tag = _Z_REPLY_TAG_ERROR; - _z_bytes_copy(&reply.data._result.error.payload, &payload); - _z_encoding_move(&reply.data._result.error.encoding, encoding); - return reply; -} -#else -_z_reply_t _z_reply_create(_z_keyexpr_t keyexpr, _z_id_t id, const _z_bytes_t payload, const _z_timestamp_t *timestamp, - _z_encoding_t *encoding, z_sample_kind_t kind, const _z_bytes_t attachment) { - _ZP_UNUSED(keyexpr); - _ZP_UNUSED(id); - _ZP_UNUSED(payload); - _ZP_UNUSED(timestamp); - _ZP_UNUSED(encoding); - _ZP_UNUSED(kind); - _ZP_UNUSED(attachment); - return _z_reply_null(); -} - -_z_reply_t _z_reply_err_create(const _z_bytes_t payload, _z_encoding_t *encoding) { - _ZP_UNUSED(payload); - _ZP_UNUSED(encoding); - return _z_reply_null(); -} -#endif +#endif // Z_FEATURE_QUERY == 1 diff --git a/src/net/sample.c b/src/net/sample.c index 6248f57e5..b700e49c1 100644 --- a/src/net/sample.c +++ b/src/net/sample.c @@ -16,39 +16,18 @@ #include "zenoh-pico/session/utils.h" #include "zenoh-pico/utils/logging.h" -_z_sample_t _z_sample_null(void) { - _z_sample_t s = { - .keyexpr = _z_keyexpr_null(), - .payload = _z_bytes_null(), - .encoding = _z_encoding_null(), - .timestamp = _z_timestamp_null(), - .kind = 0, - .qos = {0}, - .attachment = _z_bytes_null(), - }; - return s; -} - -bool _z_sample_check(const _z_sample_t *sample) { - return _z_keyexpr_check(&sample->keyexpr) || _z_bytes_check(&sample->payload) || - _z_bytes_check(&sample->attachment) || _z_encoding_check(&sample->encoding); -} - void _z_sample_move(_z_sample_t *dst, _z_sample_t *src) { _z_keyexpr_move(&dst->keyexpr, &src->keyexpr); _z_bytes_move(&dst->payload, &src->payload); _z_encoding_move(&dst->encoding, &src->encoding); + _z_timestamp_move(&dst->timestamp, &src->timestamp); _z_bytes_move(&dst->attachment, &src->attachment); - - dst->timestamp.time = src->timestamp.time; // FIXME: call the z_timestamp_move - dst->timestamp.id = src->timestamp.id; // FIXME: call the z_timestamp_move } void _z_sample_clear(_z_sample_t *sample) { _z_keyexpr_clear(&sample->keyexpr); - _z_bytes_drop(&sample->payload); _z_encoding_clear(&sample->encoding); - _z_timestamp_clear(&sample->timestamp); + _z_bytes_drop(&sample->payload); _z_bytes_drop(&sample->attachment); } @@ -77,38 +56,3 @@ _z_sample_t _z_sample_duplicate(const _z_sample_t *src) { _z_sample_copy(&dst, src); return dst; } - -#if Z_FEATURE_SUBSCRIPTION == 1 -_z_sample_t _z_sample_create(_z_keyexpr_t *key, const _z_bytes_t payload, const _z_timestamp_t *timestamp, - _z_encoding_t *encoding, const z_sample_kind_t kind, const _z_qos_t qos, - const _z_bytes_t attachment, z_reliability_t reliability) { - _z_sample_t s = _z_sample_null(); - s.keyexpr = _z_keyexpr_steal(key); - s.kind = kind; - if (timestamp != NULL) { - s.timestamp = _z_timestamp_duplicate(timestamp); - } - s.qos = qos; - s.reliability = reliability; - _z_bytes_copy(&s.payload, &payload); - _z_bytes_copy(&s.attachment, &attachment); - if (encoding != NULL) { - _z_encoding_move(&s.encoding, encoding); - } - return s; -} -#else -_z_sample_t _z_sample_create(_z_keyexpr_t *key, const _z_bytes_t payload, const _z_timestamp_t *timestamp, - _z_encoding_t *encoding, const z_sample_kind_t kind, const _z_qos_t qos, - const _z_bytes_t attachment, z_reliability_t reliability) { - _ZP_UNUSED(key); - _ZP_UNUSED(payload); - _ZP_UNUSED(timestamp); - _ZP_UNUSED(encoding); - _ZP_UNUSED(kind); - _ZP_UNUSED(qos); - _ZP_UNUSED(attachment); - _ZP_UNUSED(reliability); - return _z_sample_null(); -} -#endif diff --git a/src/net/session.c b/src/net/session.c index a71cbcc3e..c70fdbabe 100644 --- a/src/net/session.c +++ b/src/net/session.c @@ -37,7 +37,7 @@ #include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/uuid.h" -z_result_t __z_open_inner(_z_session_rc_t *zn, _z_string_t *locator, z_whatami_t mode) { +static z_result_t __z_open_inner(_z_session_rc_t *zn, _z_string_t *locator, z_whatami_t mode, int peer_op) { z_result_t ret = _Z_RES_OK; _z_id_t local_zid = _z_id_empty(); @@ -46,7 +46,7 @@ z_result_t __z_open_inner(_z_session_rc_t *zn, _z_string_t *locator, z_whatami_t local_zid = _z_id_empty(); return ret; } - ret = _z_new_transport(&_Z_RC_IN_VAL(zn)->_tp, &local_zid, locator, mode); + ret = _z_new_transport(&_Z_RC_IN_VAL(zn)->_tp, &local_zid, locator, mode, peer_op); if (ret != _Z_RES_OK) { local_zid = _z_id_empty(); return ret; @@ -64,89 +64,89 @@ z_result_t _z_open(_z_session_rc_t *zn, _z_config_t *config) { if (opt_as_str != NULL) { _z_uuid_to_bytes(zid.id, opt_as_str); } + if (config == NULL) { + _Z_ERROR("A valid config is missing."); + return _Z_ERR_GENERIC; + } + int peer_op = _Z_PEER_OP_LISTEN; + _z_string_svec_t locators = _z_string_svec_make(0); + char *connect = _z_config_get(config, Z_CONFIG_CONNECT_KEY); + char *listen = _z_config_get(config, Z_CONFIG_LISTEN_KEY); + if (connect == NULL && listen == NULL) { // Scout if peer is not configured + opt_as_str = _z_config_get(config, Z_CONFIG_SCOUTING_WHAT_KEY); + if (opt_as_str == NULL) { + opt_as_str = (char *)Z_CONFIG_SCOUTING_WHAT_DEFAULT; + } + z_what_t what = strtol(opt_as_str, NULL, 10); - if (config != NULL) { - _z_string_svec_t locators = _z_string_svec_make(0); - char *connect = _z_config_get(config, Z_CONFIG_CONNECT_KEY); - char *listen = _z_config_get(config, Z_CONFIG_LISTEN_KEY); - if (connect == NULL && listen == NULL) { // Scout if peer is not configured - opt_as_str = _z_config_get(config, Z_CONFIG_SCOUTING_WHAT_KEY); - if (opt_as_str == NULL) { - opt_as_str = (char *)Z_CONFIG_SCOUTING_WHAT_DEFAULT; - } - z_what_t what = strtol(opt_as_str, NULL, 10); - - opt_as_str = _z_config_get(config, Z_CONFIG_MULTICAST_LOCATOR_KEY); - if (opt_as_str == NULL) { - opt_as_str = (char *)Z_CONFIG_MULTICAST_LOCATOR_DEFAULT; - } - _z_string_t mcast_locator = _z_string_alias_str(opt_as_str); + opt_as_str = _z_config_get(config, Z_CONFIG_MULTICAST_LOCATOR_KEY); + if (opt_as_str == NULL) { + opt_as_str = (char *)Z_CONFIG_MULTICAST_LOCATOR_DEFAULT; + } + _z_string_t mcast_locator = _z_string_alias_str(opt_as_str); - opt_as_str = _z_config_get(config, Z_CONFIG_SCOUTING_TIMEOUT_KEY); - if (opt_as_str == NULL) { - opt_as_str = (char *)Z_CONFIG_SCOUTING_TIMEOUT_DEFAULT; - } - uint32_t timeout = (uint32_t)strtoul(opt_as_str, NULL, 10); + opt_as_str = _z_config_get(config, Z_CONFIG_SCOUTING_TIMEOUT_KEY); + if (opt_as_str == NULL) { + opt_as_str = (char *)Z_CONFIG_SCOUTING_TIMEOUT_DEFAULT; + } + uint32_t timeout = (uint32_t)strtoul(opt_as_str, NULL, 10); - // Scout and return upon the first result - _z_hello_list_t *hellos = _z_scout_inner(what, zid, &mcast_locator, timeout, true); - if (hellos != NULL) { - _z_hello_t *hello = _z_hello_list_head(hellos); - _z_string_svec_copy(&locators, &hello->_locators); + // Scout and return upon the first result + _z_hello_list_t *hellos = _z_scout_inner(what, zid, &mcast_locator, timeout, true); + if (hellos != NULL) { + _z_hello_t *hello = _z_hello_list_head(hellos); + _z_string_svec_copy(&locators, &hello->_locators, true); + } + _z_hello_list_free(&hellos); + } else { + uint_fast8_t key = Z_CONFIG_CONNECT_KEY; + if (listen != NULL) { + if (connect == NULL) { + key = Z_CONFIG_LISTEN_KEY; + _zp_config_insert(config, Z_CONFIG_MODE_KEY, Z_CONFIG_MODE_PEER); + } else { + return _Z_ERR_GENERIC; } - _z_hello_list_free(&hellos); } else { - uint_fast8_t key = Z_CONFIG_CONNECT_KEY; - if (listen != NULL) { - if (connect == NULL) { - key = Z_CONFIG_LISTEN_KEY; - _zp_config_insert(config, Z_CONFIG_MODE_KEY, Z_CONFIG_MODE_PEER); - } else { - return _Z_ERR_GENERIC; - } - } - locators = _z_string_svec_make(1); - _z_string_t s = _z_string_copy_from_str(_z_config_get(config, key)); - _z_string_svec_append(&locators, &s); + peer_op = _Z_PEER_OP_OPEN; } + locators = _z_string_svec_make(1); + _z_string_t s = _z_string_copy_from_str(_z_config_get(config, key)); + _z_string_svec_append(&locators, &s, true); + } - ret = _Z_ERR_SCOUT_NO_RESULTS; - size_t len = _z_string_svec_len(&locators); - for (size_t i = 0; i < len; i++) { - ret = _Z_RES_OK; + ret = _Z_ERR_SCOUT_NO_RESULTS; + size_t len = _z_string_svec_len(&locators); + for (size_t i = 0; i < len; i++) { + ret = _Z_RES_OK; - _z_string_t *locator = _z_string_svec_get(&locators, i); - // @TODO: check invalid configurations - // For example, client mode in multicast links + _z_string_t *locator = _z_string_svec_get(&locators, i); + // @TODO: check invalid configurations + // For example, client mode in multicast links - // Check operation mode - char *s_mode = _z_config_get(config, Z_CONFIG_MODE_KEY); - z_whatami_t mode = Z_WHATAMI_CLIENT; // By default, zenoh-pico will operate as a client - if (s_mode != NULL) { - if (_z_str_eq(s_mode, Z_CONFIG_MODE_CLIENT) == true) { - mode = Z_WHATAMI_CLIENT; - } else if (_z_str_eq(s_mode, Z_CONFIG_MODE_PEER) == true) { - mode = Z_WHATAMI_PEER; - } else { - ret = _Z_ERR_CONFIG_INVALID_MODE; - } + // Check operation mode + char *s_mode = _z_config_get(config, Z_CONFIG_MODE_KEY); + z_whatami_t mode = Z_WHATAMI_CLIENT; // By default, zenoh-pico will operate as a client + if (s_mode != NULL) { + if (_z_str_eq(s_mode, Z_CONFIG_MODE_CLIENT) == true) { + mode = Z_WHATAMI_CLIENT; + } else if (_z_str_eq(s_mode, Z_CONFIG_MODE_PEER) == true) { + mode = Z_WHATAMI_PEER; + } else { + ret = _Z_ERR_CONFIG_INVALID_MODE; } + } + if (ret == _Z_RES_OK) { + ret = __z_open_inner(zn, locator, mode, peer_op); if (ret == _Z_RES_OK) { - ret = __z_open_inner(zn, locator, mode); - if (ret == _Z_RES_OK) { - break; - } - } else { - _Z_ERROR("Trying to configure an invalid mode."); + break; } + } else { + _Z_ERROR("Trying to configure an invalid mode."); } - _z_string_svec_clear(&locators); - } else { - _Z_ERROR("A valid config is missing."); - ret = _Z_ERR_GENERIC; } - + _z_string_svec_clear(&locators); return ret; } diff --git a/src/net/subscribe.c b/src/net/subscribe.c index 84960a0a2..12a83c6ac 100644 --- a/src/net/subscribe.c +++ b/src/net/subscribe.c @@ -31,7 +31,4 @@ void _z_subscriber_free(_z_subscriber_t **sub) { } } -bool _z_subscriber_check(const _z_subscriber_t *subscriber) { return !_Z_RC_IS_NULL(&subscriber->_zn); } -_z_subscriber_t _z_subscriber_null(void) { return (_z_subscriber_t){._entity_id = 0, ._zn = _z_session_weak_null()}; } - #endif diff --git a/src/protocol/codec.c b/src/protocol/codec.c index 7564ef782..eea67422e 100644 --- a/src/protocol/codec.c +++ b/src/protocol/codec.c @@ -19,6 +19,7 @@ #include "zenoh-pico/protocol/core.h" #include "zenoh-pico/utils/endianness.h" #include "zenoh-pico/utils/logging.h" +#include "zenoh-pico/utils/pointers.h" #include "zenoh-pico/utils/result.h" /*------------------ uint8 -------------------*/ @@ -67,7 +68,7 @@ z_result_t _z_uint8_decode(uint8_t *u8, _z_zbuf_t *zbf) { if (_z_zbuf_can_read(zbf) == true) { *u8 = _z_zbuf_read(zbf); } else { - _Z_DEBUG("WARNING: Not enough bytes to read"); + _Z_INFO("Not enough bytes to read"); ret |= _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } @@ -182,6 +183,7 @@ z_result_t _z_zsize_decode_with_reader(_z_zint_t *zint, __z_single_byte_reader_t uint64_t i = 0; z_result_t res = _z_zint64_decode_with_reader(&i, reader, context); if (res != _Z_RES_OK || i > SIZE_MAX) { + _Z_INFO("Reader decode failed"); res = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } else { *zint = (_z_zint_t)i; @@ -192,35 +194,51 @@ z_result_t _z_zsize_decode_with_reader(_z_zint_t *zint, __z_single_byte_reader_t z_result_t _z_uint8_decode_reader(uint8_t *zint, void *context) { return _z_uint8_decode(zint, (_z_zbuf_t *)context); } z_result_t _z_zint64_decode(uint64_t *zint, _z_zbuf_t *zbf) { - return _z_zint64_decode_with_reader(zint, _z_uint8_decode_reader, (void *)zbf); + *zint = 0; + uint8_t b = 0; + _Z_RETURN_IF_ERR(_z_uint8_decode(&b, zbf)); + + uint8_t i = 0; + 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 _Z_RES_OK; } z_result_t _z_zint16_decode(uint16_t *zint, _z_zbuf_t *zbf) { - z_result_t ret = _Z_RES_OK; uint64_t buf; _Z_RETURN_IF_ERR(_z_zint64_decode(&buf, zbf)); - if (buf <= UINT16_MAX) { - *zint = (uint16_t)buf; - } else { - ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; + if (buf > UINT16_MAX) { + _Z_INFO("Invalid zint16 value decoded"); + return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } - return ret; + *zint = (uint16_t)buf; + return _Z_RES_OK; } z_result_t _z_zint32_decode(uint32_t *zint, _z_zbuf_t *zbf) { - z_result_t ret = _Z_RES_OK; uint64_t buf; _Z_RETURN_IF_ERR(_z_zint64_decode(&buf, zbf)); - if (buf <= UINT32_MAX) { - *zint = (uint32_t)buf; - } else { - ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; + if (buf > UINT32_MAX) { + _Z_INFO("Invalid zint32 value decoded"); + return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } - return ret; + *zint = (uint32_t)buf; + return _Z_RES_OK; } z_result_t _z_zsize_decode(_z_zint_t *zint, _z_zbuf_t *zbf) { - return _z_zsize_decode_with_reader(zint, _z_uint8_decode_reader, (void *)zbf); + uint64_t buf; + _Z_RETURN_IF_ERR(_z_zint64_decode(&buf, zbf)); + if (buf > SIZE_MAX) { + _Z_INFO("Invalide zsize value decoded"); + return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; + } + *zint = (_z_zint_t)buf; + return _Z_RES_OK; } /*------------------ uint8_array ------------------*/ @@ -255,7 +273,7 @@ z_result_t _z_slice_val_decode_na(_z_slice_t *bs, _z_zbuf_t *zbf) { *bs = _z_slice_alias_buf(_z_zbuf_get_rptr(zbf), bs->len); // Decode without allocating _z_zbuf_set_rpos(zbf, _z_zbuf_get_rpos(zbf) + bs->len); // Move the read position } else { - _Z_DEBUG("WARNING: Not enough bytes to read"); + _Z_INFO("Not enough bytes to read"); bs->len = 0; bs->start = NULL; ret |= _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; @@ -277,14 +295,16 @@ z_result_t _z_slice_val_decode(_z_slice_t *bs, _z_zbuf_t *zbf) { return _z_slice z_result_t _z_slice_decode(_z_slice_t *bs, _z_zbuf_t *zbf) { return _z_slice_decode_na(bs, zbf); } -z_result_t _z_bytes_decode(_z_bytes_t *bs, _z_zbuf_t *zbf) { +z_result_t _z_bytes_decode(_z_bytes_t *bs, _z_zbuf_t *zbf, _z_arc_slice_t *arcs) { + // Decode slice _z_slice_t s; _Z_RETURN_IF_ERR(_z_slice_decode(&s, zbf)); - if (_z_slice_is_alloced(&s)) { - return _z_bytes_from_slice(bs, s); - } else { - return _z_bytes_from_buf(bs, s.start, s.len); - } + // Calc offset + size_t offset = _z_ptr_u8_diff(s.start, _Z_RC_IN_VAL(&zbf->_slice)->start); + // Get ownership of subslice + *arcs = _z_arc_slice_wrap_slice_rc(&zbf->_slice, offset, s.len); + _z_bytes_alias_arc_slice(bs, arcs); + return _Z_RES_OK; } z_result_t _z_bytes_encode_val(_z_wbuf_t *wbf, const _z_bytes_t *bs) { @@ -326,11 +346,12 @@ z_result_t _z_str_decode(char **str, _z_zbuf_t *zbf) { } *str = tmp; } else { - _Z_DEBUG("WARNING: Not enough bytes to read"); + _Z_INFO("Not enough bytes to read"); *str = NULL; ret |= _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } } else { + _Z_INFO("Size decode failed"); *str = NULL; ret |= _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } @@ -344,22 +365,17 @@ z_result_t _z_string_encode(_z_wbuf_t *wbf, const _z_string_t *s) { } z_result_t _z_string_decode(_z_string_t *str, _z_zbuf_t *zbf) { - *str = _z_string_null(); _z_zint_t len = 0; // Decode string length _Z_RETURN_IF_ERR(_z_zsize_decode(&len, zbf)); // Check if we have enough bytes to read if (_z_zbuf_len(zbf) < len) { - _Z_DEBUG("WARNING: Not enough bytes to read"); + _Z_INFO("Not enough bytes to read"); return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } - // Allocate space for the string terminator - *str = _z_string_preallocate(len); - if (str->_slice.start == NULL) { - return _Z_ERR_SYSTEM_OUT_OF_MEMORY; - } - // Read bytes - _z_zbuf_read_bytes(zbf, (uint8_t *)_z_string_data(str), 0, len); + // Alias string + *str = _z_string_alias_substr((const char *)_z_zbuf_get_rptr(zbf), len); + _z_zbuf_set_rpos(zbf, _z_zbuf_get_rpos(zbf) + len); return _Z_RES_OK; } diff --git a/src/protocol/codec/declarations.c b/src/protocol/codec/declarations.c index be68a2f81..905aad397 100644 --- a/src/protocol/codec/declarations.c +++ b/src/protocol/codec/declarations.c @@ -32,6 +32,7 @@ #include "zenoh-pico/protocol/keyexpr.h" #include "zenoh-pico/session/session.h" #include "zenoh-pico/system/platform.h" +#include "zenoh-pico/utils/logging.h" z_result_t _z_decl_ext_keyexpr_encode(_z_wbuf_t *wbf, _z_keyexpr_t ke, bool has_next_ext) { uint8_t header = _Z_MSG_EXT_ENC_ZBUF | _Z_MSG_EXT_FLAG_M | 0x0f | (has_next_ext ? _Z_FLAG_Z_Z : 0); @@ -162,7 +163,6 @@ z_result_t _z_declaration_encode(_z_wbuf_t *wbf, const _z_declaration_t *decl) { case _Z_DECL_FINAL: { ret = _z_decl_final_encode(wbf); } break; - ; } return ret; } @@ -234,6 +234,7 @@ z_result_t _z_decl_commons_decode(_z_zbuf_t *zbf, uint8_t header, bool *has_exte _z_zint_t len; _Z_RETURN_IF_ERR(_z_zsize_decode(&len, zbf)); if (_z_zbuf_len(zbf) < len) { + _Z_INFO("Not enough bytes to read"); return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } ke->_suffix = _z_string_preallocate(len); @@ -364,6 +365,7 @@ z_result_t _z_declaration_decode(_z_declaration_t *decl, _z_zbuf_t *zbf) { ret = _z_decl_final_decode(&decl->_body._decl_final, zbf, header); } break; default: { + _Z_INFO("Unknown token type"); ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } } diff --git a/src/protocol/codec/message.c b/src/protocol/codec/message.c index f2bdf6bfb..c11c62e46 100644 --- a/src/protocol/codec/message.c +++ b/src/protocol/codec/message.c @@ -104,7 +104,9 @@ z_result_t _z_timestamp_decode(_z_timestamp_t *ts, _z_zbuf_t *zbf) { ret |= _z_zint64_decode(&ts->time, zbf); ret |= _z_id_decode_as_slice(&ts->id, zbf); - + if (ret == _Z_RES_OK) { + ts->valid = true; + } return ret; } @@ -200,6 +202,7 @@ z_result_t _z_source_info_decode(_z_source_info_t *info, _z_zbuf_t *zbf) { if (_z_zbuf_len(zbf) >= zidlen) { _z_zbuf_read_bytes(zbf, info->_id.id, 0, zidlen); } else { + _Z_INFO("Not enough bytes to read"); ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } } @@ -208,6 +211,7 @@ z_result_t _z_source_info_decode(_z_source_info_t *info, _z_zbuf_t *zbf) { if (intbuf <= UINT32_MAX) { info->_entity_id = (uint32_t)intbuf; } else { + _Z_INFO("Invalid value decoded"); ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } } @@ -216,6 +220,7 @@ z_result_t _z_source_info_decode(_z_source_info_t *info, _z_zbuf_t *zbf) { if (intbuf <= UINT32_MAX) { info->_source_sn = (uint32_t)intbuf; } else { + _Z_INFO("Invalid value decoded"); ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } } @@ -322,12 +327,11 @@ z_result_t _z_push_body_decode_extensions(_z_msg_ext_t *extension, void *ctx) { return ret; } -z_result_t _z_push_body_decode(_z_push_body_t *pshb, _z_zbuf_t *zbf, uint8_t header) { +z_result_t _z_push_body_decode(_z_push_body_t *pshb, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs) { z_result_t ret = _Z_RES_OK; switch (_Z_MID(header)) { case _Z_MID_Z_PUT: { pshb->_is_put = true; - pshb->_body._put = (_z_msg_put_t){0}; if (_Z_HAS_FLAG(header, _Z_FLAG_Z_P_T)) { _Z_RETURN_IF_ERR(_z_timestamp_decode(&pshb->_body._put._commons._timestamp, zbf)); } @@ -338,13 +342,12 @@ z_result_t _z_push_body_decode(_z_push_body_t *pshb, _z_zbuf_t *zbf, uint8_t hea _Z_RETURN_IF_ERR(_z_msg_ext_decode_iter(zbf, _z_push_body_decode_extensions, pshb)); } if (ret == _Z_RES_OK) { - _Z_RETURN_IF_ERR(_z_bytes_decode(&pshb->_body._put._payload, zbf)); + _Z_RETURN_IF_ERR(_z_bytes_decode(&pshb->_body._put._payload, zbf, arcs)); } break; } case _Z_MID_Z_DEL: { pshb->_is_put = false; - pshb->_body._del = (_z_msg_del_t){0}; if (_Z_HAS_FLAG(header, _Z_FLAG_Z_D_T)) { _Z_RETURN_IF_ERR(_z_timestamp_decode(&pshb->_body._put._commons._timestamp, zbf)); } @@ -364,10 +367,10 @@ z_result_t _z_put_encode(_z_wbuf_t *wbf, const _z_msg_put_t *put) { _z_push_body_t body = {._is_put = true, ._body = {._put = *put}}; return _z_push_body_encode(wbf, &body); } -z_result_t _z_put_decode(_z_msg_put_t *put, _z_zbuf_t *zbf, uint8_t header) { +z_result_t _z_put_decode(_z_msg_put_t *put, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs) { assert(_Z_MID(header) == _Z_MID_Z_PUT); _z_push_body_t body = {._is_put = true, ._body = {._put = *put}}; - z_result_t ret = _z_push_body_decode(&body, zbf, header); + z_result_t ret = _z_push_body_decode(&body, zbf, header, arcs); *put = body._body._put; return ret; } @@ -379,7 +382,7 @@ z_result_t _z_del_encode(_z_wbuf_t *wbf, const _z_msg_del_t *del) { z_result_t _z_del_decode(_z_msg_del_t *del, _z_zbuf_t *zbf, uint8_t header) { assert(_Z_MID(header) == _Z_MID_Z_DEL); _z_push_body_t body = {._is_put = false, ._body = {._del = *del}}; - z_result_t ret = _z_push_body_decode(&body, zbf, header); + z_result_t ret = _z_push_body_decode(&body, zbf, header, NULL); *del = body._body._del; return ret; } @@ -465,7 +468,6 @@ z_result_t _z_query_decode_extensions(_z_msg_ext_t *extension, void *ctx) { z_result_t _z_query_decode(_z_msg_query_t *msg, _z_zbuf_t *zbf, uint8_t header) { _Z_DEBUG("Decoding _Z_MID_Z_QUERY"); - *msg = (_z_msg_query_t){0}; z_result_t ret = _Z_RES_OK; if (_Z_HAS_FLAG(header, _Z_FLAG_Z_Q_C)) { @@ -507,8 +509,7 @@ z_result_t _z_reply_decode_extension(_z_msg_ext_t *extension, void *ctx) { } return ret; } -z_result_t _z_reply_decode(_z_msg_reply_t *reply, _z_zbuf_t *zbf, uint8_t header) { - *reply = (_z_msg_reply_t){0}; +z_result_t _z_reply_decode(_z_msg_reply_t *reply, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs) { if (_Z_HAS_FLAG(header, _Z_FLAG_Z_R_C)) { _Z_RETURN_IF_ERR(_z_uint8_decode((uint8_t *)&reply->_consolidation, zbf)); } else { @@ -519,7 +520,7 @@ z_result_t _z_reply_decode(_z_msg_reply_t *reply, _z_zbuf_t *zbf, uint8_t header } uint8_t put_header = 0; _Z_RETURN_IF_ERR(_z_uint8_decode(&put_header, zbf)); - _Z_RETURN_IF_ERR(_z_push_body_decode(&reply->_body, zbf, put_header)); + _Z_RETURN_IF_ERR(_z_push_body_decode(&reply->_body, zbf, put_header, arcs)); return _Z_RES_OK; } @@ -568,16 +569,14 @@ z_result_t _z_err_decode_extension(_z_msg_ext_t *extension, void *ctx) { } return ret; } -z_result_t _z_err_decode(_z_msg_err_t *err, _z_zbuf_t *zbf, uint8_t header) { - *err = (_z_msg_err_t){0}; - +z_result_t _z_err_decode(_z_msg_err_t *err, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs) { if (_Z_HAS_FLAG(header, _Z_FLAG_Z_E_E)) { _Z_RETURN_IF_ERR(_z_encoding_decode(&err->_encoding, zbf)); } if (_Z_HAS_FLAG(header, _Z_FLAG_Z_Z)) { _Z_RETURN_IF_ERR(_z_msg_ext_decode_iter(zbf, _z_err_decode_extension, err)); } - _Z_RETURN_IF_ERR(_z_bytes_decode(&err->_payload, zbf)); + _Z_RETURN_IF_ERR(_z_bytes_decode(&err->_payload, zbf, arcs)); return _Z_RES_OK; } diff --git a/src/protocol/codec/network.c b/src/protocol/codec/network.c index 426c6d150..44cf9cde9 100644 --- a/src/protocol/codec/network.c +++ b/src/protocol/codec/network.c @@ -73,6 +73,7 @@ z_result_t _z_push_decode_ext_cb(_z_msg_ext_t *extension, void *ctx) { switch (_Z_EXT_FULL_ID(extension->_header)) { case _Z_MSG_EXT_ENC_ZINT | 0x01: { // QOS ext if (extension->_body._zint._val > UINT32_MAX) { + _Z_INFO("Invalid value decoded"); return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } msg->_qos = (_z_n_qos_t){._val = (uint8_t)extension->_body._zint._val}; @@ -91,9 +92,8 @@ z_result_t _z_push_decode_ext_cb(_z_msg_ext_t *extension, void *ctx) { return ret; } -z_result_t _z_push_decode(_z_n_msg_push_t *msg, _z_zbuf_t *zbf, uint8_t header) { +z_result_t _z_push_decode(_z_n_msg_push_t *msg, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs) { z_result_t ret = _Z_RES_OK; - *msg = (_z_n_msg_push_t){0}; msg->_qos = _Z_N_QOS_DEFAULT; ret |= _z_keyexpr_decode(&msg->_key, zbf, _Z_HAS_FLAG(header, _Z_FLAG_N_PUSH_N)); _z_keyexpr_set_mapping(&msg->_key, _Z_HAS_FLAG(header, _Z_FLAG_N_PUSH_M) ? _Z_KEYEXPR_MAPPING_UNKNOWN_REMOTE @@ -104,7 +104,7 @@ z_result_t _z_push_decode(_z_n_msg_push_t *msg, _z_zbuf_t *zbf, uint8_t header) if (ret == _Z_RES_OK) { uint8_t msgheader; _Z_RETURN_IF_ERR(_z_uint8_decode(&msgheader, zbf)); - _Z_RETURN_IF_ERR(_z_push_body_decode(&msg->_body, zbf, msgheader)); + _Z_RETURN_IF_ERR(_z_push_body_decode(&msg->_body, zbf, msgheader, arcs)); } return ret; @@ -172,6 +172,7 @@ z_result_t _z_request_decode_extensions(_z_msg_ext_t *extension, void *ctx) { switch (_Z_EXT_FULL_ID(extension->_header)) { case 0x01 | _Z_MSG_EXT_ENC_ZINT: { // QOS ext if (extension->_body._zint._val > UINT8_MAX) { + _Z_INFO("Invalid value decoded"); return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } msg->_ext_qos = (_z_n_qos_t){._val = (uint8_t)extension->_body._zint._val}; @@ -185,6 +186,7 @@ z_result_t _z_request_decode_extensions(_z_msg_ext_t *extension, void *ctx) { case 0x04 | _Z_MSG_EXT_ENC_ZINT | _Z_MSG_EXT_FLAG_M: { msg->_ext_target = (uint8_t)extension->_body._zint._val; if (msg->_ext_target > 2) { + _Z_INFO("Invalid value decoded"); return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } } break; @@ -204,8 +206,7 @@ z_result_t _z_request_decode_extensions(_z_msg_ext_t *extension, void *ctx) { } return _Z_RES_OK; } -z_result_t _z_request_decode(_z_n_msg_request_t *msg, _z_zbuf_t *zbf, const uint8_t header) { - *msg = (_z_n_msg_request_t){0}; +z_result_t _z_request_decode(_z_n_msg_request_t *msg, _z_zbuf_t *zbf, const uint8_t header, _z_arc_slice_t *arcs) { msg->_ext_qos = _Z_N_QOS_DEFAULT; _Z_RETURN_IF_ERR(_z_zsize_decode(&msg->_rid, zbf)); _Z_RETURN_IF_ERR(_z_keyexpr_decode(&msg->_key, zbf, _Z_HAS_FLAG(header, _Z_FLAG_N_REQUEST_N))); @@ -223,13 +224,14 @@ z_result_t _z_request_decode(_z_n_msg_request_t *msg, _z_zbuf_t *zbf, const uint } break; case _Z_MID_Z_PUT: { msg->_tag = _Z_REQUEST_PUT; - _Z_RETURN_IF_ERR(_z_put_decode(&msg->_body._put, zbf, zheader)); + _Z_RETURN_IF_ERR(_z_put_decode(&msg->_body._put, zbf, zheader, arcs)); } break; case _Z_MID_Z_DEL: { msg->_tag = _Z_REQUEST_DEL; _Z_RETURN_IF_ERR(_z_del_decode(&msg->_body._del, zbf, zheader)); } break; default: + _Z_INFO("Unknown request type received: %d", _Z_MID(zheader)); return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } return _Z_RES_OK; @@ -332,9 +334,8 @@ z_result_t _z_response_decode_extension(_z_msg_ext_t *extension, void *ctx) { return ret; } -z_result_t _z_response_decode(_z_n_msg_response_t *msg, _z_zbuf_t *zbf, uint8_t header) { +z_result_t _z_response_decode(_z_n_msg_response_t *msg, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_t *arcs) { _Z_DEBUG("Decoding _Z_MID_N_RESPONSE"); - *msg = (_z_n_msg_response_t){0}; msg->_ext_qos = _Z_N_QOS_DEFAULT; z_result_t ret = _Z_RES_OK; _z_keyexpr_set_mapping(&msg->_key, _Z_HAS_FLAG(header, _Z_FLAG_N_RESPONSE_M) ? _Z_KEYEXPR_MAPPING_UNKNOWN_REMOTE @@ -351,12 +352,12 @@ z_result_t _z_response_decode(_z_n_msg_response_t *msg, _z_zbuf_t *zbf, uint8_t switch (_Z_MID(inner_header)) { case _Z_MID_Z_REPLY: { msg->_tag = _Z_RESPONSE_BODY_REPLY; - ret = _z_reply_decode(&msg->_body._reply, zbf, inner_header); + ret = _z_reply_decode(&msg->_body._reply, zbf, inner_header, arcs); break; } case _Z_MID_Z_ERR: { msg->_tag = _Z_RESPONSE_BODY_ERR; - ret = _z_err_decode(&msg->_body._err, zbf, inner_header); + ret = _z_err_decode(&msg->_body._err, zbf, inner_header, arcs); break; } default: { @@ -380,8 +381,6 @@ z_result_t _z_response_final_encode(_z_wbuf_t *wbf, const _z_n_msg_response_fina z_result_t _z_response_final_decode(_z_n_msg_response_final_t *msg, _z_zbuf_t *zbf, uint8_t header) { (void)(header); - - *msg = (_z_n_msg_response_final_t){0}; z_result_t ret = _Z_RES_OK; ret |= _z_zsize_decode(&msg->_request_id, zbf); if (_Z_HAS_FLAG(header, _Z_FLAG_Z_Z)) { @@ -440,7 +439,6 @@ z_result_t _z_declare_decode_extensions(_z_msg_ext_t *extension, void *ctx) { return _Z_RES_OK; } z_result_t _z_declare_decode(_z_n_msg_declare_t *decl, _z_zbuf_t *zbf, uint8_t header) { - *decl = (_z_n_msg_declare_t){0}; decl->_ext_qos = _Z_N_QOS_DEFAULT; // Retrieve interest id if (_Z_HAS_FLAG(header, _Z_FLAG_N_DECLARE_I)) { @@ -515,7 +513,7 @@ z_result_t _z_network_message_encode(_z_wbuf_t *wbf, const _z_network_message_t return _Z_ERR_GENERIC; } } -z_result_t _z_network_message_decode(_z_network_message_t *msg, _z_zbuf_t *zbf) { +z_result_t _z_network_message_decode(_z_network_message_t *msg, _z_zbuf_t *zbf, _z_arc_slice_t *arcs) { uint8_t header; _Z_RETURN_IF_ERR(_z_uint8_decode(&header, zbf)); switch (_Z_MID(header)) { @@ -525,15 +523,15 @@ z_result_t _z_network_message_decode(_z_network_message_t *msg, _z_zbuf_t *zbf) } break; case _Z_MID_N_PUSH: { msg->_tag = _Z_N_PUSH; - return _z_push_decode(&msg->_body._push, zbf, header); + return _z_push_decode(&msg->_body._push, zbf, header, arcs); } break; case _Z_MID_N_REQUEST: { msg->_tag = _Z_N_REQUEST; - return _z_request_decode(&msg->_body._request, zbf, header); + return _z_request_decode(&msg->_body._request, zbf, header, arcs); } break; case _Z_MID_N_RESPONSE: { msg->_tag = _Z_N_RESPONSE; - return _z_response_decode(&msg->_body._response, zbf, header); + return _z_response_decode(&msg->_body._response, zbf, header, arcs); } break; case _Z_MID_N_RESPONSE_FINAL: { msg->_tag = _Z_N_RESPONSE_FINAL; @@ -544,6 +542,7 @@ z_result_t _z_network_message_decode(_z_network_message_t *msg, _z_zbuf_t *zbf) return _z_n_interest_decode(&msg->_body._interest, zbf, header); } break; default: + _Z_INFO("Unknown message type received: %d", _Z_MID(header)); return _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } } diff --git a/src/protocol/codec/transport.c b/src/protocol/codec/transport.c index bdc3d17f6..894fb41d8 100644 --- a/src/protocol/codec/transport.c +++ b/src/protocol/codec/transport.c @@ -139,6 +139,7 @@ z_result_t _z_join_decode(_z_t_msg_join_t *msg, _z_zbuf_t *zbf, uint8_t header) if (_z_zbuf_len(zbf) >= zidlen) { _z_zbuf_read_bytes(zbf, msg->_zid.id, 0, zidlen); } else { + _Z_INFO("Invalid zid length received"); ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } } @@ -246,6 +247,7 @@ z_result_t _z_init_decode(_z_t_msg_init_t *msg, _z_zbuf_t *zbf, uint8_t header) if (_z_zbuf_len(zbf) >= zidlen) { _z_zbuf_read_bytes(zbf, msg->_zid.id, 0, zidlen); } else { + _Z_INFO("Invalid zid length received"); ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } } @@ -265,7 +267,7 @@ z_result_t _z_init_decode(_z_t_msg_init_t *msg, _z_zbuf_t *zbf, uint8_t header) if ((ret == _Z_RES_OK) && (_Z_HAS_FLAG(header, _Z_FLAG_T_INIT_A) == true)) { ret |= _z_slice_decode(&msg->_cookie, zbf); } else { - msg->_cookie = _z_slice_empty(); + msg->_cookie = _z_slice_null(); } msg->_patch = _Z_NO_PATCH; @@ -311,10 +313,10 @@ z_result_t _z_open_decode(_z_t_msg_open_t *msg, _z_zbuf_t *zbf, uint8_t header) if ((ret == _Z_RES_OK) && (_Z_HAS_FLAG(header, _Z_FLAG_T_OPEN_A) == false)) { ret |= _z_slice_decode(&msg->_cookie, zbf); if (ret != _Z_RES_OK) { - msg->_cookie = _z_slice_empty(); + msg->_cookie = _z_slice_null(); } } else { - msg->_cookie = _z_slice_empty(); + msg->_cookie = _z_slice_null(); } if ((ret == _Z_RES_OK) && (_Z_HAS_FLAG(header, _Z_FLAG_T_Z) == true)) { ret |= _z_msg_ext_skip_non_mandatories(zbf, 0x02); @@ -384,50 +386,110 @@ z_result_t _z_frame_encode(_z_wbuf_t *wbf, uint8_t header, const _z_t_msg_frame_ ret = _Z_ERR_MESSAGE_SERIALIZATION_FAILED; } if (ret == _Z_RES_OK) { - size_t len = _z_network_message_vec_len(&msg->_messages); + size_t len = _z_network_message_svec_len(&msg->_messages); for (size_t i = 0; i < len; i++) { - _Z_RETURN_IF_ERR(_z_network_message_encode(wbf, _z_network_message_vec_get(&msg->_messages, i))) + _Z_RETURN_IF_ERR(_z_network_message_encode(wbf, _z_network_message_svec_get(&msg->_messages, i))) } } return ret; } -z_result_t _z_frame_decode(_z_t_msg_frame_t *msg, _z_zbuf_t *zbf, uint8_t header) { +static void _z_frame_update_arcs_msg_pool(_z_network_message_svec_t *msg_pool, _z_arc_slice_svec_t *arc_pool) { + for (size_t i = 0; i < arc_pool->_len; i++) { + _z_network_message_t *nm = _z_network_message_svec_get(msg_pool, i); + switch (nm->_tag) { + case _Z_N_PUSH: { + if (!nm->_body._push._body._is_put) { + continue; + } + _z_bytes_alias_arc_slice(&nm->_body._push._body._body._put._payload, + _z_arc_slice_svec_get(arc_pool, i)); + } break; + case _Z_N_REQUEST: { + if (nm->_body._request._tag != _Z_REQUEST_PUT) { + continue; + } + _z_bytes_alias_arc_slice(&nm->_body._request._body._put._payload, _z_arc_slice_svec_get(arc_pool, i)); + } break; + case _Z_N_RESPONSE: { + switch (nm->_body._response._tag) { + case _Z_RESPONSE_BODY_REPLY: + if (!nm->_body._response._body._reply._body._is_put) { + continue; + } + _z_bytes_alias_arc_slice(&nm->_body._response._body._reply._body._body._put._payload, + _z_arc_slice_svec_get(arc_pool, i)); + break; + + case _Z_RESPONSE_BODY_ERR: + _z_bytes_alias_arc_slice(&nm->_body._response._body._err._payload, + _z_arc_slice_svec_get(arc_pool, i)); + break; + + default: + continue; + } + } + default: + continue; + } + } +} + +z_result_t _z_frame_decode(_z_t_msg_frame_t *msg, _z_zbuf_t *zbf, uint8_t header, _z_arc_slice_svec_t *arc_pool, + _z_network_message_svec_t *msg_pool) { z_result_t ret = _Z_RES_OK; *msg = (_z_t_msg_frame_t){0}; - ret |= _z_zsize_decode(&msg->_sn, zbf); - if ((ret == _Z_RES_OK) && (_Z_HAS_FLAG(header, _Z_FLAG_T_Z) == true)) { - ret |= _z_msg_ext_skip_non_mandatories(zbf, 0x04); - } - if (ret == _Z_RES_OK) { - msg->_messages = _z_network_message_vec_make(_ZENOH_PICO_FRAME_MESSAGES_VEC_SIZE); - while (_z_zbuf_len(zbf) > 0) { - // Mark the reading position of the iobfer - size_t r_pos = _z_zbuf_get_rpos(zbf); - _z_network_message_t *nm = (_z_network_message_t *)z_malloc(sizeof(_z_network_message_t)); - memset(nm, 0, sizeof(_z_network_message_t)); - ret |= _z_network_message_decode(nm, zbf); - if (ret == _Z_RES_OK) { - _z_network_message_vec_append(&msg->_messages, nm); - } else { - _z_n_msg_free(&nm); - - _z_zbuf_set_rpos(zbf, r_pos); // Restore the reading position of the iobfer - - // FIXME: Check for the return error, since not all of them means a decoding error - // in this particular case. As of now, we roll-back the reading position - // and return to the Zenoh transport-level decoder. - // https://github.com/eclipse-zenoh/zenoh-pico/pull/132#discussion_r1045593602 - if ((ret & _Z_ERR_MESSAGE_ZENOH_UNKNOWN) == _Z_ERR_MESSAGE_ZENOH_UNKNOWN) { - ret = _Z_RES_OK; - } - break; + _Z_RETURN_IF_ERR(_z_zsize_decode(&msg->_sn, zbf)); + if (_Z_HAS_FLAG(header, _Z_FLAG_T_Z)) { + _Z_RETURN_IF_ERR(_z_msg_ext_skip_non_mandatories(zbf, 0x04)); + } + // Init message vector + msg_pool->_len = 0; + arc_pool->_len = 0; + _z_network_message_svec_init(msg_pool, 0); + size_t msg_idx = 0; + while (_z_zbuf_len(zbf) > 0) { + // Expand message vector if needed + if (msg_idx >= msg_pool->_capacity) { + _Z_RETURN_IF_ERR(_z_network_message_svec_expand(msg_pool, false)); + _z_network_message_svec_init(msg_pool, msg_pool->_len); + } + // Expand arc pool if needed + if (msg_idx >= arc_pool->_capacity) { + _Z_RETURN_IF_ERR(_z_arc_slice_svec_expand(arc_pool, false)); + // Update arcs references in msg pool + _z_frame_update_arcs_msg_pool(msg_pool, arc_pool); + } + // Mark the reading position of the iobfer + size_t r_pos = _z_zbuf_get_rpos(zbf); + // Retrieve storage in resource pool + _z_network_message_t *nm = _z_network_message_svec_get_mut(msg_pool, msg_idx); + _z_arc_slice_t *arcs = _z_arc_slice_svec_get_mut(arc_pool, msg_idx); + // Decode message + ret = _z_network_message_decode(nm, zbf, arcs); + if (ret != _Z_RES_OK) { + _z_network_message_svec_reset(msg_pool); + _z_zbuf_set_rpos(zbf, r_pos); // Restore the reading position of the iobfer + + // FIXME: Check for the return error, since not all of them means a decoding error + // in this particular case. As of now, we roll-back the reading position + // and return to the Zenoh transport-level decoder. + // https://github.com/eclipse-zenoh/zenoh-pico/pull/132#discussion_r1045593602 + if ((ret & _Z_ERR_MESSAGE_ZENOH_UNKNOWN) == _Z_ERR_MESSAGE_ZENOH_UNKNOWN) { + ret = _Z_RES_OK; } + return ret; } + arc_pool->_len++; + msg_pool->_len++; + msg_idx++; } - return ret; + // Alias network message svec in frame struct + msg->_messages = _z_network_message_svec_alias(msg_pool); + return _Z_RES_OK; } /*------------------ Fragment Message ------------------*/ @@ -483,9 +545,7 @@ z_result_t _z_fragment_decode(_z_t_msg_fragment_t *msg, _z_zbuf_t *zbf, uint8_t if ((ret == _Z_RES_OK) && (_Z_HAS_FLAG(header, _Z_FLAG_T_Z) == true)) { ret |= _z_msg_ext_decode_iter(zbf, _z_fragment_decode_ext, msg); } - - _z_slice_t slice = _z_slice_alias_buf((uint8_t *)_z_zbuf_start(zbf), _z_zbuf_len(zbf)); - _z_slice_copy(&msg->_payload, &slice); + msg->_payload = _z_slice_alias_buf((uint8_t *)_z_zbuf_start(zbf), _z_zbuf_len(zbf)); zbf->_ios._r_pos = zbf->_ios._w_pos; return ret; @@ -556,7 +616,8 @@ z_result_t _z_transport_message_encode(_z_wbuf_t *wbf, const _z_transport_messag return ret; } -z_result_t _z_transport_message_decode(_z_transport_message_t *msg, _z_zbuf_t *zbf) { +z_result_t _z_transport_message_decode(_z_transport_message_t *msg, _z_zbuf_t *zbf, _z_arc_slice_svec_t *arc_pool, + _z_network_message_svec_t *msg_pool) { z_result_t ret = _Z_RES_OK; ret |= _z_uint8_decode(&msg->_header, zbf); // Decode the header @@ -564,7 +625,7 @@ z_result_t _z_transport_message_decode(_z_transport_message_t *msg, _z_zbuf_t *z uint8_t mid = _Z_MID(msg->_header); switch (mid) { case _Z_MID_T_FRAME: { - ret |= _z_frame_decode(&msg->_body._frame, zbf, msg->_header); + ret |= _z_frame_decode(&msg->_body._frame, zbf, msg->_header, arc_pool, msg_pool); } break; case _Z_MID_T_FRAGMENT: { ret |= _z_fragment_decode(&msg->_body._fragment, zbf, msg->_header); diff --git a/src/protocol/core.c b/src/protocol/core.c index ed0479cc5..37c2a7bcc 100644 --- a/src/protocol/core.c +++ b/src/protocol/core.c @@ -19,7 +19,9 @@ #include "zenoh-pico/api/primitives.h" #include "zenoh-pico/api/types.h" +#include "zenoh-pico/collections/bytes.h" #include "zenoh-pico/collections/slice.h" +#include "zenoh-pico/net/encoding.h" #include "zenoh-pico/protocol/core.h" #include "zenoh-pico/protocol/iobuf.h" #include "zenoh-pico/utils/endianness.h" @@ -27,6 +29,8 @@ #define _Z_ID_LEN (16) +const _z_id_t empty_id = {0}; + uint8_t _z_id_len(_z_id_t id) { uint8_t len = _Z_ID_LEN; while (len > 0) { @@ -38,38 +42,6 @@ uint8_t _z_id_len(_z_id_t id) { } return len; } -bool _z_id_check(_z_id_t id) { - bool ret = false; - for (int i = 0; !ret && i < _Z_ID_LEN; i++) { - ret |= id.id[i] != 0; - } - return ret; -} -_z_id_t _z_id_empty(void) { - return (_z_id_t){.id = { - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - }}; -} - -_z_source_info_t _z_source_info_null(void) { - return (_z_source_info_t){._source_sn = 0, ._entity_id = 0, ._id = _z_id_empty()}; -} -_z_timestamp_t _z_timestamp_null(void) { return (_z_timestamp_t){.id = _z_id_empty(), .time = 0}; } uint64_t _z_timestamp_ntp64_from_time(uint32_t seconds, uint32_t nanos) { const uint64_t FRAC_PER_SEC = (uint64_t)1 << 32; @@ -79,7 +51,6 @@ uint64_t _z_timestamp_ntp64_from_time(uint32_t seconds, uint32_t nanos) { return ((uint64_t)seconds << 32) | fractions; } -_z_value_t _z_value_null(void) { return (_z_value_t){.payload = _z_bytes_null(), .encoding = _z_encoding_null()}; } _z_value_t _z_value_steal(_z_value_t *value) { _z_value_t ret = *value; *value = _z_value_null(); @@ -91,20 +62,22 @@ z_result_t _z_value_copy(_z_value_t *dst, const _z_value_t *src) { _Z_CLEAN_RETURN_IF_ERR(_z_bytes_copy(&dst->payload, &src->payload), _z_encoding_clear(&dst->encoding)); return _Z_RES_OK; } +_z_value_t _z_value_alias(_z_value_t src) { + _z_value_t dst; + dst.payload = _z_bytes_alias(src.payload); + dst.encoding = _z_encoding_alias(src.encoding); + return dst; +} z_result_t _z_hello_copy(_z_hello_t *dst, const _z_hello_t *src) { *dst = _z_hello_null(); - _Z_RETURN_IF_ERR(_z_string_svec_copy(&dst->_locators, &src->_locators) ? _Z_RES_OK : _Z_ERR_SYSTEM_OUT_OF_MEMORY); + _Z_RETURN_IF_ERR(_z_string_svec_copy(&dst->_locators, &src->_locators, true)); dst->_version = src->_version; dst->_whatami = src->_whatami; memcpy(&dst->_zid.id, &src->_zid.id, _Z_ID_LEN); return _Z_RES_OK; } -_z_hello_t _z_hello_null(void) { - return (_z_hello_t){._zid = _z_id_empty(), ._version = 0, ._whatami = 0x0, ._locators = _z_string_svec_make(0)}; -} - void _z_value_move(_z_value_t *dst, _z_value_t *src) { _z_encoding_move(&dst->encoding, &src->encoding); _z_bytes_move(&dst->payload, &src->payload); diff --git a/src/protocol/definitions/declarations.c b/src/protocol/definitions/declarations.c index 878f14f6d..df693f2a1 100644 --- a/src/protocol/definitions/declarations.c +++ b/src/protocol/definitions/declarations.c @@ -71,7 +71,7 @@ _z_declaration_t _z_make_undecl_subscriber(uint32_t id, _Z_OPTIONAL const _z_key return (_z_declaration_t){ ._tag = _Z_UNDECL_SUBSCRIBER, ._body = {._undecl_subscriber = { - ._id = id, ._ext_keyexpr = (key == NULL) ? _z_keyexpr_null() : _z_keyexpr_duplicate(*key)}}}; + ._id = id, ._ext_keyexpr = (key == NULL) ? _z_keyexpr_null() : _z_keyexpr_duplicate(key)}}}; } _z_declaration_t _z_make_decl_queryable(_Z_MOVE(_z_keyexpr_t) key, uint32_t id, bool complete, uint16_t distance) { @@ -85,7 +85,7 @@ _z_declaration_t _z_make_undecl_queryable(uint32_t id, _Z_OPTIONAL const _z_keye return (_z_declaration_t){ ._tag = _Z_UNDECL_QUERYABLE, ._body = {._undecl_queryable = { - ._id = id, ._ext_keyexpr = (key == NULL) ? _z_keyexpr_null() : _z_keyexpr_duplicate(*key)}}}; + ._id = id, ._ext_keyexpr = (key == NULL) ? _z_keyexpr_null() : _z_keyexpr_duplicate(key)}}}; } _z_declaration_t _z_make_decl_token(_Z_MOVE(_z_keyexpr_t) key, uint32_t id) { return (_z_declaration_t){._tag = _Z_DECL_TOKEN, @@ -98,22 +98,12 @@ _z_declaration_t _z_make_undecl_token(uint32_t id, _Z_OPTIONAL const _z_keyexpr_ return (_z_declaration_t){ ._tag = _Z_UNDECL_TOKEN, ._body = {._undecl_token = {._id = id, - ._ext_keyexpr = (key == NULL) ? _z_keyexpr_null() : _z_keyexpr_duplicate(*key)}}}; + ._ext_keyexpr = (key == NULL) ? _z_keyexpr_null() : _z_keyexpr_duplicate(key)}}}; } _z_declaration_t _z_make_decl_final(void) { return (_z_declaration_t){._tag = _Z_DECL_FINAL, ._body = {._decl_final = {0}}}; } -_z_decl_kexpr_t _z_decl_kexpr_null(void) { return (_z_decl_kexpr_t){0}; } -_z_decl_subscriber_t _z_decl_subscriber_null(void) { return (_z_decl_subscriber_t){0}; } -_z_decl_queryable_t _z_decl_queryable_null(void) { return (_z_decl_queryable_t){0}; } -_z_decl_token_t _z_decl_token_null(void) { return (_z_decl_token_t){0}; } -_z_undecl_kexpr_t _z_undecl_kexpr_null(void) { return (_z_undecl_kexpr_t){0}; } -_z_undecl_subscriber_t _z_undecl_subscriber_null(void) { return (_z_undecl_subscriber_t){0}; } -_z_undecl_queryable_t _z_undecl_queryable_null(void) { return (_z_undecl_queryable_t){0}; } -_z_undecl_token_t _z_undecl_token_null(void) { return (_z_undecl_token_t){0}; } -_z_decl_final_t _z_decl_final_null(void) { return (_z_decl_final_t){0}; } - void _z_decl_fix_mapping(_z_declaration_t *msg, uint16_t mapping) { switch (msg->_tag) { case _Z_DECL_KEXPR: { diff --git a/src/protocol/definitions/interest.c b/src/protocol/definitions/interest.c index 7c0254ee7..cfdc1795e 100644 --- a/src/protocol/definitions/interest.c +++ b/src/protocol/definitions/interest.c @@ -33,5 +33,3 @@ _z_interest_t _z_make_interest_final(uint32_t id) { .flags = 0, }; } - -_z_interest_t _z_interest_null(void) { return (_z_interest_t){0}; } diff --git a/src/protocol/definitions/network.c b/src/protocol/definitions/network.c index 177e0bc0a..1a84ea1cd 100644 --- a/src/protocol/definitions/network.c +++ b/src/protocol/definitions/network.c @@ -19,6 +19,8 @@ #include "zenoh-pico/protocol/definitions/message.h" #include "zenoh-pico/utils/logging.h" +const _z_qos_t _Z_N_QOS_DEFAULT = {._val = 5}; + _z_n_msg_request_exts_t _z_n_msg_request_needed_exts(const _z_n_msg_request_t *msg) { _z_n_msg_request_exts_t ret = {.n = 0, .ext_budget = msg->_ext_budget != 0, @@ -72,10 +74,15 @@ _z_push_body_t _z_push_body_steal(_z_push_body_t *msg) { *msg = _z_push_body_null(); return ret; } -_z_push_body_t _z_push_body_null(void) { - return (_z_push_body_t){ - ._is_put = false, - ._body._del._commons = {._timestamp = _z_timestamp_null(), ._source_info = _z_source_info_null()}}; + +static z_result_t _z_push_body_copy(_z_push_body_t *dst, const _z_push_body_t *src) { + if (src->_is_put) { + _Z_RETURN_IF_ERR(_z_bytes_copy(&dst->_body._put._attachment, &src->_body._put._attachment)); + _Z_RETURN_IF_ERR(_z_bytes_copy(&dst->_body._put._payload, &src->_body._put._payload)); + } else { + _Z_RETURN_IF_ERR(_z_bytes_copy(&dst->_body._del._attachment, &src->_body._del._attachment)); + } + return _Z_RES_OK; } void _z_n_msg_response_final_clear(_z_n_msg_response_final_t *msg) { (void)(msg); } @@ -229,6 +236,124 @@ _z_network_message_t _z_n_msg_make_interest(_z_interest_t interest) { }; } +static z_result_t _z_n_msg_push_copy(_z_network_message_t *dst, const _z_network_message_t *src) { + memcpy(dst, src, sizeof(_z_network_message_t)); + _Z_RETURN_IF_ERR(_z_keyexpr_copy(&dst->_body._push._key, &src->_body._push._key)); + return _z_push_body_copy(&dst->_body._push._body, &src->_body._push._body); +} + +static z_result_t _z_n_msg_request_copy(_z_network_message_t *dst, const _z_network_message_t *src) { + memcpy(dst, src, sizeof(_z_network_message_t)); + _Z_RETURN_IF_ERR(_z_keyexpr_copy(&dst->_body._request._key, &src->_body._request._key)); + switch (src->_body._request._tag) { + case _Z_REQUEST_QUERY: + _Z_RETURN_IF_ERR(_z_slice_copy(&dst->_body._request._body._query._parameters, + &src->_body._request._body._query._parameters)); + _Z_RETURN_IF_ERR(_z_bytes_copy(&dst->_body._request._body._query._ext_attachment, + &src->_body._request._body._query._ext_attachment)); + _Z_RETURN_IF_ERR(_z_bytes_copy(&dst->_body._request._body._query._ext_value.payload, + &src->_body._request._body._query._ext_value.payload)); + break; + case _Z_REQUEST_PUT: + _Z_RETURN_IF_ERR(_z_bytes_copy(&dst->_body._request._body._put._attachment, + &src->_body._request._body._put._attachment)); + _Z_RETURN_IF_ERR( + _z_bytes_copy(&dst->_body._request._body._put._payload, &src->_body._request._body._put._payload)); + break; + case _Z_REQUEST_DEL: + _Z_RETURN_IF_ERR(_z_bytes_copy(&dst->_body._request._body._del._attachment, + &src->_body._request._body._del._attachment)); + break; + } + return _Z_RES_OK; +} + +static z_result_t _z_n_msg_response_copy(_z_network_message_t *dst, const _z_network_message_t *src) { + memcpy(dst, src, sizeof(_z_network_message_t)); + _Z_RETURN_IF_ERR(_z_keyexpr_copy(&dst->_body._response._key, &src->_body._response._key)); + switch (src->_body._response._tag) { + case _Z_RESPONSE_BODY_REPLY: + _Z_RETURN_IF_ERR( + _z_push_body_copy(&dst->_body._response._body._reply._body, &src->_body._response._body._reply._body)); + break; + case _Z_RESPONSE_BODY_ERR: + _Z_RETURN_IF_ERR( + _z_bytes_copy(&dst->_body._response._body._err._payload, &src->_body._response._body._err._payload)); + break; + } + return _Z_RES_OK; +} + +static z_result_t _z_n_msg_response_final_copy(_z_network_message_t *dst, const _z_network_message_t *src) { + memcpy(dst, src, sizeof(_z_network_message_t)); + return _Z_RES_OK; +} + +static z_result_t _z_n_msg_declare_copy(_z_network_message_t *dst, const _z_network_message_t *src) { + memcpy(dst, src, sizeof(_z_network_message_t)); + const _z_declaration_t *src_decl = &src->_body._declare._decl; + _z_declaration_t *dst_decl = &dst->_body._declare._decl; + switch (src_decl->_tag) { + case _Z_DECL_KEXPR: { + _Z_RETURN_IF_ERR( + _z_keyexpr_copy(&dst_decl->_body._decl_kexpr._keyexpr, &src_decl->_body._decl_kexpr._keyexpr)); + } break; + case _Z_DECL_SUBSCRIBER: { + _Z_RETURN_IF_ERR(_z_keyexpr_copy(&dst_decl->_body._decl_subscriber._keyexpr, + &src_decl->_body._decl_subscriber._keyexpr)); + } break; + case _Z_UNDECL_SUBSCRIBER: { + _Z_RETURN_IF_ERR(_z_keyexpr_copy(&dst_decl->_body._undecl_subscriber._ext_keyexpr, + &src_decl->_body._undecl_subscriber._ext_keyexpr)); + } break; + case _Z_DECL_QUERYABLE: { + _Z_RETURN_IF_ERR( + _z_keyexpr_copy(&dst_decl->_body._decl_queryable._keyexpr, &src_decl->_body._decl_queryable._keyexpr)); + } break; + case _Z_UNDECL_QUERYABLE: { + _Z_RETURN_IF_ERR(_z_keyexpr_copy(&dst_decl->_body._undecl_queryable._ext_keyexpr, + &src_decl->_body._undecl_queryable._ext_keyexpr)); + } break; + case _Z_DECL_TOKEN: { + _Z_RETURN_IF_ERR( + _z_keyexpr_copy(&dst_decl->_body._decl_token._keyexpr, &src_decl->_body._decl_token._keyexpr)); + } break; + case _Z_UNDECL_TOKEN: { + _Z_RETURN_IF_ERR(_z_keyexpr_copy(&dst_decl->_body._undecl_token._ext_keyexpr, + &src_decl->_body._undecl_token._ext_keyexpr)); + } break; + default: + break; + } + return _Z_RES_OK; +} + +static z_result_t _z_n_msg_interest_copy(_z_network_message_t *dst, const _z_network_message_t *src) { + memcpy(dst, src, sizeof(_z_network_message_t)); + _Z_RETURN_IF_ERR( + _z_keyexpr_copy(&dst->_body._interest._interest._keyexpr, &src->_body._interest._interest._keyexpr)); + return _Z_RES_OK; +} + +z_result_t _z_n_msg_copy(_z_network_message_t *dst, const _z_network_message_t *src) { + switch (src->_tag) { + case _Z_N_PUSH: + return _z_n_msg_push_copy(dst, src); + case _Z_N_REQUEST: + return _z_n_msg_request_copy(dst, src); + case _Z_N_RESPONSE: + return _z_n_msg_response_copy(dst, src); + case _Z_N_RESPONSE_FINAL: + return _z_n_msg_response_final_copy(dst, src); + case _Z_N_DECLARE: + return _z_n_msg_declare_copy(dst, src); + case _Z_N_INTEREST: + return _z_n_msg_interest_copy(dst, src); + default: + return _Z_ERR_ENTITY_UNKNOWN; + } +} + void _z_msg_fix_mapping(_z_zenoh_message_t *msg, uint16_t mapping) { switch (msg->_tag) { case _Z_N_DECLARE: { diff --git a/src/protocol/definitions/transport.c b/src/protocol/definitions/transport.c index 486aa43fd..69b1cc502 100644 --- a/src/protocol/definitions/transport.c +++ b/src/protocol/definitions/transport.c @@ -40,7 +40,12 @@ void _z_t_msg_close_clear(_z_t_msg_close_t *msg) { (void)(msg); } void _z_t_msg_keep_alive_clear(_z_t_msg_keep_alive_t *msg) { (void)(msg); } -void _z_t_msg_frame_clear(_z_t_msg_frame_t *msg) { _z_network_message_vec_clear(&msg->_messages); } +void _z_t_msg_frame_clear(_z_t_msg_frame_t *msg) { + // TODO (sashacmc): make in more correct way + if (!msg->_messages._aliased) { + _z_network_message_svec_clear(&msg->_messages); + } +} void _z_t_msg_fragment_clear(_z_t_msg_fragment_t *msg) { _z_slice_clear(&msg->_payload); } @@ -241,7 +246,7 @@ _z_transport_message_t _z_t_msg_make_keep_alive(void) { return msg; } -_z_transport_message_t _z_t_msg_make_frame(_z_zint_t sn, _z_network_message_vec_t messages, +_z_transport_message_t _z_t_msg_make_frame(_z_zint_t sn, _z_network_message_svec_t messages, z_reliability_t reliability) { _z_transport_message_t msg; msg._header = _Z_MID_T_FRAME; @@ -266,7 +271,7 @@ _z_transport_message_t _z_t_msg_make_frame_header(_z_zint_t sn, z_reliability_t _Z_SET_FLAG(msg._header, _Z_FLAG_T_FRAME_R); } - msg._body._frame._messages = _z_network_message_vec_make(0); + msg._body._frame._messages = _z_network_message_svec_null(); return msg; } @@ -274,7 +279,7 @@ _z_transport_message_t _z_t_msg_make_frame_header(_z_zint_t sn, z_reliability_t /*------------------ Fragment Message ------------------*/ _z_transport_message_t _z_t_msg_make_fragment_header(_z_zint_t sn, z_reliability_t reliability, bool is_last, bool first, bool drop) { - return _z_t_msg_make_fragment(sn, _z_slice_empty(), reliability, is_last, first, drop); + return _z_t_msg_make_fragment(sn, _z_slice_null(), reliability, is_last, first, drop); } _z_transport_message_t _z_t_msg_make_fragment(_z_zint_t sn, _z_slice_t payload, z_reliability_t reliability, bool is_last, bool first, bool drop) { @@ -347,7 +352,7 @@ void _z_t_msg_copy_keep_alive(_z_t_msg_keep_alive_t *clone, _z_t_msg_keep_alive_ void _z_t_msg_copy_frame(_z_t_msg_frame_t *clone, _z_t_msg_frame_t *msg) { clone->_sn = msg->_sn; - _z_network_message_vec_copy(&clone->_messages, &msg->_messages); + _z_network_message_svec_copy(&clone->_messages, &msg->_messages, false); } /*------------------ Transport Message ------------------*/ diff --git a/src/protocol/iobuf.c b/src/protocol/iobuf.c index c398bc766..ac78d0ec3 100644 --- a/src/protocol/iobuf.c +++ b/src/protocol/iobuf.c @@ -20,6 +20,7 @@ #include #include "zenoh-pico/config.h" +#include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/pointers.h" #include "zenoh-pico/utils/result.h" @@ -35,6 +36,12 @@ _z_iosli_t _z_iosli_wrap(const uint8_t *buf, size_t length, size_t r_pos, size_t return ios; } +_z_iosli_t _z_iosli_steal(_z_iosli_t *ios) { + _z_iosli_t new_ios = *ios; + *ios = _z_iosli_null(); + return new_ios; +} + void __z_iosli_init(_z_iosli_t *ios, size_t capacity) { ios->_r_pos = 0; ios->_w_pos = 0; @@ -75,6 +82,13 @@ void _z_iosli_read_bytes(_z_iosli_t *ios, uint8_t *dst, size_t offset, size_t le ios->_r_pos = ios->_r_pos + length; } +void _z_iosli_copy_bytes(_z_iosli_t *dst, const _z_iosli_t *src) { + size_t length = _z_iosli_readable(src); + assert(dst->_capacity >= length); + (void)memcpy(dst->_buf + dst->_w_pos, src->_buf + src->_r_pos, length); + dst->_w_pos += length; +} + uint8_t _z_iosli_get(const _z_iosli_t *ios, size_t pos) { assert(pos < ios->_capacity); return ios->_buf[pos]; @@ -160,8 +174,18 @@ _z_iosli_t *_z_iosli_clone(const _z_iosli_t *src) { /*------------------ ZBuf ------------------*/ _z_zbuf_t _z_zbuf_make(size_t capacity) { - _z_zbuf_t zbf; + _z_zbuf_t zbf = _z_zbuf_null(); zbf._ios = _z_iosli_make(capacity); + if (_z_zbuf_capacity(&zbf) == 0) { + return zbf; + } + _z_slice_t s = _z_slice_from_buf_custom_deleter(zbf._ios._buf, zbf._ios._capacity, _z_delete_context_default()); + zbf._slice = _z_slice_simple_rc_new_from_val(&s); + if (_Z_RC_IS_NULL(&zbf._slice)) { + _Z_ERROR("slice rc creation failed"); + _z_iosli_clear(&zbf._ios); + } + zbf._ios._is_alloc = false; return zbf; } @@ -169,6 +193,7 @@ _z_zbuf_t _z_zbuf_view(_z_zbuf_t *zbf, size_t length) { assert(_z_iosli_readable(&zbf->_ios) >= length); _z_zbuf_t v; v._ios = _z_iosli_wrap(_z_zbuf_get_rptr(zbf), length, 0, length); + v._slice = zbf->_slice; return v; } _z_zbuf_t _z_slice_as_zbuf(_z_slice_t slice) { @@ -188,6 +213,8 @@ uint8_t const *_z_zbuf_start(const _z_zbuf_t *zbf) { } size_t _z_zbuf_len(const _z_zbuf_t *zbf) { return _z_iosli_readable(&zbf->_ios); } +void _z_zbuf_copy_bytes(_z_zbuf_t *dst, const _z_zbuf_t *src) { _z_iosli_copy_bytes(&dst->_ios, &src->_ios); } + bool _z_zbuf_can_read(const _z_zbuf_t *zbf) { return _z_zbuf_len(zbf) > (size_t)0; } uint8_t _z_zbuf_read(_z_zbuf_t *zbf) { return _z_iosli_read(&zbf->_ios); } @@ -218,7 +245,10 @@ uint8_t *_z_zbuf_get_wptr(const _z_zbuf_t *zbf) { return zbf->_ios._buf + zbf->_ void _z_zbuf_reset(_z_zbuf_t *zbf) { _z_iosli_reset(&zbf->_ios); } -void _z_zbuf_clear(_z_zbuf_t *zbf) { _z_iosli_clear(&zbf->_ios); } +void _z_zbuf_clear(_z_zbuf_t *zbf) { + _z_iosli_clear(&zbf->_ios); + _z_slice_simple_rc_drop(&zbf->_slice); +} void _z_zbuf_compact(_z_zbuf_t *zbf) { if ((zbf->_ios._r_pos != 0) || (zbf->_ios._w_pos != 0)) { @@ -260,15 +290,8 @@ size_t _z_wbuf_len_iosli(const _z_wbuf_t *wbf) { return _z_iosli_vec_len(&wbf->_ _z_wbuf_t _z_wbuf_make(size_t capacity, bool is_expandable) { _z_wbuf_t wbf; - if (is_expandable == true) { - // Preallocate 4 slots, this is usually what we expect - // when fragmenting a zenoh data message with attachment - wbf._ioss = _z_iosli_vec_make(4); - _z_wbuf_add_iosli(&wbf, __z_wbuf_new_iosli(capacity)); - } else { - wbf._ioss = _z_iosli_vec_make(1); - _z_wbuf_add_iosli(&wbf, __z_wbuf_new_iosli(capacity)); - } + wbf._ioss = _z_iosli_vec_make(1); + _z_wbuf_add_iosli(&wbf, __z_wbuf_new_iosli(capacity)); wbf._w_idx = 0; // This __must__ come after adding ioslices to reset w_idx wbf._r_idx = 0; wbf._expansion_step = is_expandable ? capacity : 0; @@ -288,7 +311,7 @@ size_t _z_wbuf_capacity(const _z_wbuf_t *wbf) { size_t _z_wbuf_len(const _z_wbuf_t *wbf) { size_t len = 0; - for (size_t i = wbf->_r_idx; i <= wbf->_w_idx; i++) { + for (size_t i = wbf->_r_idx; (i < _z_wbuf_len_iosli(wbf)) && (i <= wbf->_w_idx); i++) { _z_iosli_t *ios = _z_wbuf_get_iosli(wbf, i); len = len + _z_iosli_readable(ios); } @@ -517,6 +540,23 @@ _z_zbuf_t _z_wbuf_to_zbuf(const _z_wbuf_t *wbf) { return zbf; } +_z_zbuf_t _z_wbuf_moved_as_zbuf(_z_wbuf_t *wbf) { + // Can only move single buffer wbuf + assert(_z_iosli_vec_len(&wbf->_ioss) == 1); + + _z_zbuf_t zbf = _z_zbuf_null(); + _z_iosli_t *ios = _z_wbuf_get_iosli(wbf, 0); + zbf._ios = _z_iosli_steal(ios); + _z_slice_t s = _z_slice_from_buf_custom_deleter(zbf._ios._buf, zbf._ios._capacity, _z_delete_context_default()); + zbf._slice = _z_slice_simple_rc_new_from_val(&s); + if (_Z_RC_IS_NULL(&zbf._slice)) { + _Z_ERROR("slice rc creation failed"); + } + zbf._ios._is_alloc = false; + _z_wbuf_clear(wbf); + return zbf; +} + z_result_t _z_wbuf_siphon(_z_wbuf_t *dst, _z_wbuf_t *src, size_t length) { z_result_t ret = _Z_RES_OK; size_t llength = length; @@ -569,7 +609,10 @@ void _z_wbuf_reset(_z_wbuf_t *wbf) { } } -void _z_wbuf_clear(_z_wbuf_t *wbf) { _z_iosli_vec_clear(&wbf->_ioss); } +void _z_wbuf_clear(_z_wbuf_t *wbf) { + _z_iosli_vec_clear(&wbf->_ioss); + *wbf = _z_wbuf_null(); +} void _z_wbuf_free(_z_wbuf_t **wbf) { _z_wbuf_t *ptr = *wbf; diff --git a/src/protocol/keyexpr.c b/src/protocol/keyexpr.c index ce185f512..5a4882566 100644 --- a/src/protocol/keyexpr.c +++ b/src/protocol/keyexpr.c @@ -36,7 +36,7 @@ _z_keyexpr_t _z_keyexpr_from_string(uint16_t rid, _z_string_t *str) { return (_z_keyexpr_t){ ._id = rid, ._mapping = _z_keyexpr_mapping(_Z_KEYEXPR_MAPPING_LOCAL), - ._suffix = (_z_string_check(str)) ? _z_string_alias(str) : _z_string_null(), + ._suffix = (_z_string_check(str)) ? _z_string_alias(*str) : _z_string_null(), }; } @@ -64,9 +64,9 @@ z_result_t _z_keyexpr_copy(_z_keyexpr_t *dst, const _z_keyexpr_t *src) { return _Z_RES_OK; } -_z_keyexpr_t _z_keyexpr_duplicate(_z_keyexpr_t src) { +_z_keyexpr_t _z_keyexpr_duplicate(const _z_keyexpr_t *src) { _z_keyexpr_t dst; - _z_keyexpr_copy(&dst, &src); + _z_keyexpr_copy(&dst, src); return dst; } @@ -88,10 +88,7 @@ void _z_keyexpr_move(_z_keyexpr_t *dst, _z_keyexpr_t *src) { *dst = _z_keyexpr_s void _z_keyexpr_clear(_z_keyexpr_t *rk) { rk->_id = 0; - if (_z_keyexpr_has_suffix(rk)) { - _z_string_clear(&rk->_suffix); - } - rk->_suffix = _z_string_null(); + _z_string_clear(&rk->_suffix); } void _z_keyexpr_free(_z_keyexpr_t **rk) { @@ -112,16 +109,15 @@ bool _z_keyexpr_equals(const _z_keyexpr_t *left, const _z_keyexpr_t *right) { if (_z_keyexpr_mapping_id(left) != _z_keyexpr_mapping_id(right)) { return false; } - return _z_string_equals(&left->_suffix, &right->_suffix); -} - -_z_keyexpr_t _z_keyexpr_alias(_z_keyexpr_t src) { - _z_keyexpr_t alias = { - ._id = src._id, - ._mapping = src._mapping, - ._suffix = _z_string_alias(&src._suffix), - }; - return alias; + bool l_suffix = _z_keyexpr_has_suffix(left); + bool r_suffix = _z_keyexpr_has_suffix(right); + if (l_suffix != r_suffix) { + return false; + } + if (l_suffix && r_suffix) { + return _z_string_equals(&left->_suffix, &right->_suffix); + } + return true; } _z_keyexpr_t _z_keyexpr_alias_from_user_defined(_z_keyexpr_t src, bool try_declared) { diff --git a/src/session/interest.c b/src/session/interest.c index d0d7e407d..88a0626bd 100644 --- a/src/session/interest.c +++ b/src/session/interest.c @@ -28,6 +28,7 @@ #include "zenoh-pico/session/resource.h" #include "zenoh-pico/session/session.h" #include "zenoh-pico/session/utils.h" +#include "zenoh-pico/transport/common/tx.h" #include "zenoh-pico/utils/logging.h" #if Z_FEATURE_INTEREST == 1 @@ -96,9 +97,9 @@ static _z_session_interest_rc_list_t *__unsafe_z_get_interest_by_key_and_flags(_ } static z_result_t _z_interest_send_decl_resource(_z_session_t *zn, uint32_t interest_id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_resource_list_t *res_list = _z_resource_list_clone(zn->_local_resources); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); _z_resource_list_t *xs = res_list; while (xs != NULL) { _z_resource_t *res = _z_resource_list_head(xs); @@ -118,9 +119,9 @@ static z_result_t _z_interest_send_decl_resource(_z_session_t *zn, uint32_t inte #if Z_FEATURE_SUBSCRIPTION == 1 static z_result_t _z_interest_send_decl_subscriber(_z_session_t *zn, uint32_t interest_id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_subscription_rc_list_t *sub_list = _z_subscription_rc_list_clone(zn->_subscriptions); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); _z_subscription_rc_list_t *xs = sub_list; while (xs != NULL) { _z_subscription_rc_t *sub = _z_subscription_rc_list_head(xs); @@ -147,9 +148,9 @@ static z_result_t _z_interest_send_decl_subscriber(_z_session_t *zn, uint32_t in #if Z_FEATURE_QUERYABLE == 1 static z_result_t _z_interest_send_decl_queryable(_z_session_t *zn, uint32_t interest_id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_session_queryable_rc_list_t *qle_list = _z_session_queryable_rc_list_clone(zn->_local_queryable); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); _z_session_queryable_rc_list_t *xs = qle_list; while (xs != NULL) { _z_session_queryable_rc_t *qle = _z_session_queryable_rc_list_head(xs); @@ -177,9 +178,9 @@ static z_result_t _z_interest_send_decl_queryable(_z_session_t *zn, uint32_t int #if Z_FEATURE_LIVELINESS == 1 static z_result_t _z_interest_send_decl_token(_z_session_t *zn, uint32_t interest_id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_keyexpr_intmap_t token_list = _z_keyexpr_intmap_clone(&zn->_local_tokens); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); _z_keyexpr_intmap_iterator_t iter = _z_keyexpr_intmap_iterator_make(&token_list); while (_z_keyexpr_intmap_iterator_next(&iter)) { // Build the declare message to send on the wire @@ -215,9 +216,9 @@ static z_result_t _z_interest_send_declare_final(_z_session_t *zn, uint32_t inte } _z_session_interest_rc_t *_z_get_interest_by_id(_z_session_t *zn, const _z_zint_t id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_session_interest_rc_t *intr = __unsafe_z_get_interest_by_id(zn, id); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return intr; } @@ -226,13 +227,13 @@ _z_session_interest_rc_t *_z_register_interest(_z_session_t *zn, _z_session_inte (int)_z_string_len(&intr->_key._suffix), _z_string_data(&intr->_key._suffix)); _z_session_interest_rc_t *ret = NULL; - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); ret = (_z_session_interest_rc_t *)z_malloc(sizeof(_z_session_interest_rc_t)); if (ret != NULL) { *ret = _z_session_interest_rc_new_from_val(intr); zn->_local_interests = _z_session_interest_rc_list_push(zn->_local_interests, ret); } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return ret; } @@ -259,7 +260,7 @@ static _z_keyexpr_t _unsafe_z_get_key_from_declare(_z_session_t *zn, uint32_t id while (xs != NULL) { _z_declare_data_t *decl = _z_declare_data_list_head(xs); if (_z_declare_data_eq(&comp, decl)) { - return _z_keyexpr_duplicate(decl->_key); + return _z_keyexpr_duplicate(&decl->_key); } xs = _z_declare_data_list_tail(xs); } @@ -307,17 +308,17 @@ z_result_t _z_interest_process_declares(_z_session_t *zn, const _z_declaration_t return _Z_ERR_MESSAGE_ZENOH_DECLARATION_UNKNOWN; } // Retrieve key - _zp_session_lock_mutex(zn); - _z_keyexpr_t key = __unsafe_z_get_expanded_key_from_key(zn, decl_key); + _z_session_mutex_lock(zn); + _z_keyexpr_t key = __unsafe_z_get_expanded_key_from_key(zn, decl_key, true); if (!_z_keyexpr_has_suffix(&key)) { - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return _Z_ERR_KEYEXPR_UNKNOWN; } // Register declare _unsafe_z_register_declare(zn, &key, msg.id, decl_type); // Retrieve interests _z_session_interest_rc_list_t *intrs = __unsafe_z_get_interest_by_key_and_flags(zn, flags, &key); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); // Parse session_interest list _z_session_interest_rc_list_t *xs = intrs; while (xs != NULL) { @@ -359,17 +360,17 @@ z_result_t _z_interest_process_undeclares(_z_session_t *zn, const _z_declaration default: return _Z_ERR_MESSAGE_ZENOH_DECLARATION_UNKNOWN; } - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); // Retrieve declare data _z_keyexpr_t key = _unsafe_z_get_key_from_declare(zn, msg.id, decl_type); if (!_z_keyexpr_has_suffix(&key)) { - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return _Z_ERR_KEYEXPR_UNKNOWN; } _z_session_interest_rc_list_t *intrs = __unsafe_z_get_interest_by_key_and_flags(zn, flags, &key); // Remove declare _unsafe_z_unregister_declare(zn, msg.id, decl_type); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); // Parse session_interest list _z_session_interest_rc_list_t *xs = intrs; @@ -387,25 +388,25 @@ z_result_t _z_interest_process_undeclares(_z_session_t *zn, const _z_declaration } void _z_unregister_interest(_z_session_t *zn, _z_session_interest_rc_t *intr) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); zn->_local_interests = _z_session_interest_rc_list_drop_filter(zn->_local_interests, _z_session_interest_rc_eq, intr); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } void _z_flush_interest(_z_session_t *zn) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_session_interest_rc_list_free(&zn->_local_interests); _z_declare_data_list_free(&zn->_remote_declares); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } z_result_t _z_interest_process_declare_final(_z_session_t *zn, uint32_t id) { _z_interest_msg_t msg = {.type = _Z_INTEREST_MSG_TYPE_FINAL, .id = id}; // Retrieve interest - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_session_interest_rc_t *intr = __unsafe_z_get_interest_by_id(zn, id); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); if (intr == NULL) { return _Z_RES_OK; } diff --git a/src/session/liveliness.c b/src/session/liveliness.c index 11d883243..dcf2bd0ac 100644 --- a/src/session/liveliness.c +++ b/src/session/liveliness.c @@ -34,58 +34,59 @@ /**************** Liveliness Token ****************/ -z_result_t _z_liveliness_register_token(_z_session_t *zn, uint32_t id, const _z_keyexpr_t keyexpr) { +z_result_t _z_liveliness_register_token(_z_session_t *zn, uint32_t id, const _z_keyexpr_t *keyexpr) { z_result_t ret = _Z_RES_OK; - _Z_DEBUG("Register liveliness token (%i:%.*s)", (int)id, (int)_z_string_len(&keyexpr._suffix), - _z_string_data(&keyexpr._suffix)); + _Z_DEBUG("Register liveliness token (%i:%.*s)", (int)id, (int)_z_string_len(&keyexpr->_suffix), + _z_string_data(&keyexpr->_suffix)); - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); const _z_keyexpr_t *pkeyexpr = _z_keyexpr_intmap_get(&zn->_local_tokens, id); if (pkeyexpr != NULL) { _Z_ERROR("Duplicate token id %i", (int)id); ret = _Z_ERR_ENTITY_DECLARATION_FAILED; } else { - _z_keyexpr_intmap_insert(&zn->_local_tokens, id, _z_keyexpr_clone(&keyexpr)); + _z_keyexpr_intmap_insert(&zn->_local_tokens, id, _z_keyexpr_clone(keyexpr)); } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return ret; } void _z_liveliness_unregister_token(_z_session_t *zn, uint32_t id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _Z_DEBUG("Unregister liveliness token (%i)", (int)id); _z_keyexpr_intmap_remove(&zn->_local_tokens, id); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } /**************** Liveliness Subscriber ****************/ #if Z_FEATURE_SUBSCRIPTION == 1 -z_result_t _z_liveliness_subscription_declare(_z_session_t *zn, uint32_t id, const _z_keyexpr_t keyexpr, +z_result_t _z_liveliness_subscription_declare(_z_session_t *zn, uint32_t id, const _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp) { z_result_t ret = _Z_RES_OK; - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); const _z_keyexpr_t *pkeyexpr = _z_keyexpr_intmap_get(&zn->_remote_tokens, id); if (pkeyexpr != NULL) { _Z_ERROR("Duplicate token id %i", (int)id); ret = _Z_ERR_ENTITY_DECLARATION_FAILED; } else { - _z_keyexpr_intmap_insert(&zn->_remote_tokens, id, _z_keyexpr_clone(&keyexpr)); + _z_keyexpr_intmap_insert(&zn->_remote_tokens, id, _z_keyexpr_clone(keyexpr)); } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); if (ret == _Z_RES_OK) { - ret = _z_trigger_liveliness_subscriptions_declare(zn, keyexpr, timestamp); + _z_keyexpr_t key = _z_keyexpr_alias(*keyexpr); + ret = _z_trigger_liveliness_subscriptions_declare(zn, &key, timestamp); } return ret; @@ -95,7 +96,7 @@ z_result_t _z_liveliness_subscription_undeclare(_z_session_t *zn, uint32_t id, c z_result_t ret = _Z_RES_OK; _z_keyexpr_t *key = NULL; - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); const _z_keyexpr_t *keyexpr = _z_keyexpr_intmap_get(&zn->_remote_tokens, id); if (keyexpr != NULL) { key = _z_keyexpr_clone(keyexpr); @@ -103,32 +104,32 @@ z_result_t _z_liveliness_subscription_undeclare(_z_session_t *zn, uint32_t id, c } else { ret = _Z_ERR_ENTITY_UNKNOWN; } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); if (key != NULL) { - ret = _z_trigger_liveliness_subscriptions_undeclare(zn, *key, timestamp); + ret = _z_trigger_liveliness_subscriptions_undeclare(zn, key, timestamp); _z_keyexpr_free(&key); } return ret; } -z_result_t _z_liveliness_subscription_trigger_history(_z_session_t *zn, _z_keyexpr_t keyexpr) { +z_result_t _z_liveliness_subscription_trigger_history(_z_session_t *zn, const _z_keyexpr_t *keyexpr) { z_result_t ret = _Z_RES_OK; - _Z_DEBUG("Retrieve liveliness history for %.*s", (int)_z_string_len(&keyexpr._suffix), - _z_string_data(&keyexpr._suffix)); + _Z_DEBUG("Retrieve liveliness history for %.*s", (int)_z_string_len(&keyexpr->_suffix), + _z_string_data(&keyexpr->_suffix)); - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_keyexpr_intmap_t token_list = _z_keyexpr_intmap_clone(&zn->_remote_tokens); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); _z_keyexpr_intmap_iterator_t iter = _z_keyexpr_intmap_iterator_make(&token_list); _z_timestamp_t tm = _z_timestamp_null(); while (_z_keyexpr_intmap_iterator_next(&iter)) { _z_keyexpr_t key = *_z_keyexpr_intmap_iterator_value(&iter); - if (_z_keyexpr_suffix_intersects(&key, &keyexpr)) { - ret = _z_trigger_liveliness_subscriptions_declare(zn, key, &tm); + if (_z_keyexpr_suffix_intersects(&key, keyexpr)) { + ret = _z_trigger_liveliness_subscriptions_declare(zn, &key, &tm); if (ret != _Z_RES_OK) { break; } @@ -174,7 +175,7 @@ z_result_t _z_liveliness_register_pending_query(_z_session_t *zn, uint32_t id, _ _Z_DEBUG("Register liveliness query for (%ju:%.*s)", (uintmax_t)pen_qry->_key._id, (int)_z_string_len(&pen_qry->_key._suffix), _z_string_data(&pen_qry->_key._suffix)); - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); const _z_liveliness_pending_query_t *pq = _z_liveliness_pending_query_intmap_get(&zn->_liveliness_pending_queries, id); @@ -185,16 +186,16 @@ z_result_t _z_liveliness_register_pending_query(_z_session_t *zn, uint32_t id, _ _z_liveliness_pending_query_intmap_insert(&zn->_liveliness_pending_queries, id, pen_qry); } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return ret; } -z_result_t _z_liveliness_pending_query_reply(_z_session_t *zn, uint32_t interest_id, const _z_keyexpr_t keyexpr, +z_result_t _z_liveliness_pending_query_reply(_z_session_t *zn, uint32_t interest_id, const _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp) { z_result_t ret = _Z_RES_OK; - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); const _z_liveliness_pending_query_t *pq = _z_liveliness_pending_query_intmap_get(&zn->_liveliness_pending_queries, interest_id); @@ -205,9 +206,9 @@ z_result_t _z_liveliness_pending_query_reply(_z_session_t *zn, uint32_t interest _Z_DEBUG("Liveliness pending query reply %i resolve result %i", (int)interest_id, ret); if (ret == _Z_RES_OK) { - _Z_DEBUG("Resolving %d - %.*s on mapping 0x%x", keyexpr._id, (int)_z_string_len(&keyexpr._suffix), - _z_string_data(&keyexpr._suffix), _z_keyexpr_mapping_id(&keyexpr)); - _z_keyexpr_t expanded_ke = __unsafe_z_get_expanded_key_from_key(zn, &keyexpr); + _Z_DEBUG("Resolving %d - %.*s on mapping 0x%x", keyexpr->_id, (int)_z_string_len(&keyexpr->_suffix), + _z_string_data(&keyexpr->_suffix), _z_keyexpr_mapping_id(keyexpr)); + _z_keyexpr_t expanded_ke = __unsafe_z_get_expanded_key_from_key(zn, keyexpr, true); _Z_DEBUG("Reply liveliness query for %d - %.*s", expanded_ke._id, (int)_z_string_len(&expanded_ke._suffix), _z_string_data(&expanded_ke._suffix)); @@ -217,15 +218,17 @@ z_result_t _z_liveliness_pending_query_reply(_z_session_t *zn, uint32_t interest if (ret == _Z_RES_OK) { _z_encoding_t encoding = _z_encoding_null(); - _z_reply_t reply = _z_reply_create(expanded_ke, zn->_local_zid, _z_bytes_null(), timestamp, &encoding, - Z_SAMPLE_KIND_PUT, _z_bytes_null()); + _z_bytes_t payload = _z_bytes_null(); + _z_bytes_t attachment = _z_bytes_null(); + _z_reply_t reply = _z_reply_alias(&expanded_ke, zn->_local_zid, &payload, timestamp, &encoding, + Z_SAMPLE_KIND_PUT, &attachment); pq->_callback(&reply, pq->_arg); _z_reply_clear(&reply); } } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return ret; } @@ -233,7 +236,7 @@ z_result_t _z_liveliness_pending_query_reply(_z_session_t *zn, uint32_t interest z_result_t _z_liveliness_pending_query_drop(_z_session_t *zn, uint32_t interest_id) { z_result_t ret = _Z_RES_OK; - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); const _z_liveliness_pending_query_t *pq = _z_liveliness_pending_query_intmap_get(&zn->_liveliness_pending_queries, interest_id); @@ -247,17 +250,17 @@ z_result_t _z_liveliness_pending_query_drop(_z_session_t *zn, uint32_t interest_ _z_liveliness_pending_query_intmap_remove(&zn->_liveliness_pending_queries, interest_id); } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return ret; } void _z_liveliness_unregister_pending_query(_z_session_t *zn, uint32_t id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_liveliness_pending_query_intmap_remove(&zn->_liveliness_pending_queries, id); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } #endif // Z_FEATURE_QUERY == 1 @@ -267,14 +270,14 @@ void _z_liveliness_unregister_pending_query(_z_session_t *zn, uint32_t id) { z_result_t _z_liveliness_process_token_declare(_z_session_t *zn, const _z_n_msg_declare_t *decl) { #if Z_FEATURE_QUERY == 1 if (decl->has_interest_id) { - _z_liveliness_pending_query_reply(zn, decl->_interest_id, decl->_decl._body._decl_token._keyexpr, + _z_liveliness_pending_query_reply(zn, decl->_interest_id, &decl->_decl._body._decl_token._keyexpr, &decl->_ext_timestamp); } #endif #if Z_FEATURE_SUBSCRIPTION == 1 return _z_liveliness_subscription_declare(zn, decl->_decl._body._decl_token._id, - decl->_decl._body._decl_token._keyexpr, &decl->_ext_timestamp); + &decl->_decl._body._decl_token._keyexpr, &decl->_ext_timestamp); #else _ZP_UNUSED(zn); _ZP_UNUSED(decl); @@ -308,7 +311,7 @@ z_result_t _z_liveliness_process_declare_final(_z_session_t *zn, const _z_n_msg_ /**************** Init/Clear ****************/ void _z_liveliness_init(_z_session_t *zn) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); zn->_remote_tokens = _z_keyexpr_intmap_make(); zn->_local_tokens = _z_keyexpr_intmap_make(); @@ -317,11 +320,11 @@ void _z_liveliness_init(_z_session_t *zn) { zn->_liveliness_pending_queries = _z_liveliness_pending_query_intmap_make(); #endif - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } void _z_liveliness_clear(_z_session_t *zn) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); #if Z_FEATURE_QUERY == 1 _z_liveliness_pending_query_intmap_clear(&zn->_liveliness_pending_queries); @@ -329,7 +332,7 @@ void _z_liveliness_clear(_z_session_t *zn) { _z_keyexpr_intmap_clear(&zn->_local_tokens); _z_keyexpr_intmap_clear(&zn->_remote_tokens); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } #endif // Z_FEATURE_LIVELINESS == 1 diff --git a/src/session/push.c b/src/session/push.c index 6f0a6c371..098a1ec24 100644 --- a/src/session/push.c +++ b/src/session/push.c @@ -29,11 +29,11 @@ z_result_t _z_trigger_push(_z_session_t *zn, _z_n_msg_push_t *push, z_reliabilit if (push->_body._is_put) { _z_msg_put_t *put = &push->_body._body._put; - ret = _z_trigger_subscriptions_put(zn, push->_key, put->_payload, &put->_encoding, &put->_commons._timestamp, - push->_qos, put->_attachment, reliability); + ret = _z_trigger_subscriptions_put(zn, &push->_key, &put->_payload, &put->_encoding, &put->_commons._timestamp, + push->_qos, &put->_attachment, reliability); } else { _z_msg_del_t *del = &push->_body._body._del; - ret = _z_trigger_subscriptions_del(zn, push->_key, &del->_commons._timestamp, push->_qos, del->_attachment, + ret = _z_trigger_subscriptions_del(zn, &push->_key, &del->_commons._timestamp, push->_qos, &del->_attachment, reliability); } return ret; diff --git a/src/session/query.c b/src/session/query.c index 9515f47d3..1a16e9161 100644 --- a/src/session/query.c +++ b/src/session/query.c @@ -35,6 +35,23 @@ void _z_pending_query_clear(_z_pending_query_t *pen_qry) { bool _z_pending_query_eq(const _z_pending_query_t *one, const _z_pending_query_t *two) { return one->_id == two->_id; } +static bool _z_pending_query_timeout(const _z_pending_query_t *foo, const _z_pending_query_t *pq) { + _ZP_UNUSED(foo); + bool result = z_clock_elapsed_ms((z_clock_t *)&pq->_start_time) >= pq->_timeout; + if (result) { + _Z_INFO("Dropping query because of timeout"); + } + return result; +} + +void _z_pending_query_process_timeout(_z_session_t *zn) { + // Lock session + _z_session_mutex_lock(zn); + // Drop all queries with timeout elapsed + zn->_pending_queries = _z_pending_query_list_drop_filter(zn->_pending_queries, _z_pending_query_timeout, NULL); + _z_session_mutex_unlock(zn); +} + /*------------------ Query ------------------*/ _z_zint_t _z_get_query_id(_z_session_t *zn) { return zn->_query_id++; } @@ -66,11 +83,11 @@ _z_pending_query_t *__unsafe__z_get_pending_query_by_id(_z_session_t *zn, const } _z_pending_query_t *_z_get_pending_query_by_id(_z_session_t *zn, const _z_zint_t id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_pending_query_t *pql = __unsafe__z_get_pending_query_by_id(zn, id); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return pql; } @@ -80,7 +97,7 @@ z_result_t _z_register_pending_query(_z_session_t *zn, _z_pending_query_t *pen_q _Z_DEBUG(">>> Allocating query for (%ju:%.*s)", (uintmax_t)pen_qry->_key._id, (int)_z_string_len(&pen_qry->_key._suffix), _z_string_data(&pen_qry->_key._suffix)); - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_pending_query_t *pql = __unsafe__z_get_pending_query_by_id(zn, pen_qry->_id); if (pql == NULL) { // Register query only if a pending one with the same ID does not exist @@ -89,44 +106,43 @@ z_result_t _z_register_pending_query(_z_session_t *zn, _z_pending_query_t *pen_q ret = _Z_ERR_ENTITY_DECLARATION_FAILED; } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return ret; } -z_result_t _z_trigger_query_reply_partial(_z_session_t *zn, const _z_zint_t id, const _z_keyexpr_t keyexpr, - _z_msg_put_t *msg, z_sample_kind_t kind) { - z_result_t ret = _Z_RES_OK; - - _zp_session_lock_mutex(zn); +static z_result_t _z_trigger_query_reply_partial_inner(_z_session_t *zn, const _z_zint_t id, + const _z_keyexpr_t *keyexpr, _z_msg_put_t *msg, + z_sample_kind_t kind) { + _z_session_mutex_lock(zn); + // Get query infos _z_pending_query_t *pen_qry = __unsafe__z_get_pending_query_by_id(zn, id); - if ((ret == _Z_RES_OK) && (pen_qry == NULL)) { - ret = _Z_ERR_ENTITY_UNKNOWN; + if (pen_qry == NULL) { + _z_session_mutex_unlock(zn); + return _Z_ERR_ENTITY_UNKNOWN; } - - _z_keyexpr_t expanded_ke = __unsafe_z_get_expanded_key_from_key(zn, &keyexpr); - if ((ret == _Z_RES_OK) && - ((pen_qry->_anykey == false) && (_z_keyexpr_suffix_intersects(&pen_qry->_key, &keyexpr) == false))) { - ret = _Z_ERR_QUERY_NOT_MATCH; + _z_keyexpr_t expanded_ke = __unsafe_z_get_expanded_key_from_key(zn, keyexpr, true); + if (!pen_qry->_anykey && !_z_keyexpr_suffix_intersects(&pen_qry->_key, keyexpr)) { + _z_session_mutex_unlock(zn); + return _Z_ERR_QUERY_NOT_MATCH; } - // Build the reply - _z_reply_t reply = _z_reply_create(expanded_ke, zn->_local_zid, msg->_payload, &msg->_commons._timestamp, - &msg->_encoding, kind, msg->_attachment); - - bool drop = false; - // Verify if this is a newer reply, free the old one in case it is - if ((ret == _Z_RES_OK) && ((pen_qry->_consolidation == Z_CONSOLIDATION_MODE_LATEST) || - (pen_qry->_consolidation == Z_CONSOLIDATION_MODE_MONOTONIC))) { + _z_reply_t reply = _z_reply_alias(&expanded_ke, zn->_local_zid, &msg->_payload, &msg->_commons._timestamp, + &msg->_encoding, kind, &msg->_attachment); + // Process monotonic & latest consolidation mode + if ((pen_qry->_consolidation == Z_CONSOLIDATION_MODE_LATEST) || + (pen_qry->_consolidation == Z_CONSOLIDATION_MODE_MONOTONIC)) { + bool drop = false; _z_pending_reply_list_t *pen_rps = pen_qry->_pending_replies; _z_pending_reply_t *pen_rep = NULL; + + // Verify if this is a newer reply, free the old one in case it is while (pen_rps != NULL) { pen_rep = _z_pending_reply_list_head(pen_rps); - // Check if this is the same resource key if (_z_string_equals(&pen_rep->_reply.data._result.sample.keyexpr._suffix, - &reply.data._result.sample.keyexpr._suffix) == true) { + &reply.data._result.sample.keyexpr._suffix)) { if (msg->_commons._timestamp.time <= pen_rep->_tstamp.time) { drop = true; } else { @@ -137,86 +153,74 @@ z_result_t _z_trigger_query_reply_partial(_z_session_t *zn, const _z_zint_t id, } pen_rps = _z_pending_reply_list_tail(pen_rps); } - - if (drop == false) { + if (!drop) { // Cache most recent reply pen_rep = (_z_pending_reply_t *)z_malloc(sizeof(_z_pending_reply_t)); - if (pen_rep != NULL) { - if (pen_qry->_consolidation == Z_CONSOLIDATION_MODE_MONOTONIC) { - // No need to store the whole reply in the monotonic mode. - _z_reply_t partial_reply; - (void)memset(&partial_reply, 0, - sizeof(_z_reply_t)); // Avoid warnings on uninitialized values on the reply - partial_reply.data._tag = _Z_REPLY_TAG_DATA; - partial_reply.data._result.sample.keyexpr = _z_keyexpr_duplicate(reply.data._result.sample.keyexpr); - pen_rep->_reply = partial_reply; - } else { - pen_rep->_reply = reply; // Store the whole reply in the latest mode - } - pen_rep->_tstamp = _z_timestamp_duplicate(&msg->_commons._timestamp); - pen_qry->_pending_replies = _z_pending_reply_list_push(pen_qry->_pending_replies, pen_rep); + if (pen_rep == NULL) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + if (pen_qry->_consolidation == Z_CONSOLIDATION_MODE_MONOTONIC) { + // No need to store the whole reply in the monotonic mode. + pen_rep->_reply = _z_reply_null(); + pen_rep->_reply.data._tag = _Z_REPLY_TAG_DATA; + pen_rep->_reply.data._result.sample.keyexpr = _z_keyexpr_duplicate(&reply.data._result.sample.keyexpr); } else { - ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; + // Copy the reply to store it out of context + _Z_RETURN_IF_ERR(_z_reply_copy(&pen_rep->_reply, &reply)); } + pen_rep->_tstamp = _z_timestamp_duplicate(&msg->_commons._timestamp); + pen_qry->_pending_replies = _z_pending_reply_list_push(pen_qry->_pending_replies, pen_rep); } } + _z_session_mutex_unlock(zn); - _zp_session_unlock_mutex(zn); - - // Trigger the user callback - if ((ret == _Z_RES_OK) && (pen_qry->_consolidation != Z_CONSOLIDATION_MODE_LATEST)) { - _z_reply_t cb_reply = _z_reply_null(); - cb_reply = _z_reply_move(&reply); - pen_qry->_callback(&cb_reply, pen_qry->_arg); - _z_reply_clear(&cb_reply); - return ret; - } - // Other cases - if (drop || (ret != _Z_RES_OK)) { - _z_reply_clear(&reply); + // Trigger callback if applicable + if (pen_qry->_consolidation != Z_CONSOLIDATION_MODE_LATEST) { + pen_qry->_callback(&reply, pen_qry->_arg); } + return _Z_RES_OK; +} +z_result_t _z_trigger_query_reply_partial(_z_session_t *zn, const _z_zint_t id, _z_keyexpr_t *keyexpr, + _z_msg_put_t *msg, z_sample_kind_t kind) { + z_result_t ret = _z_trigger_query_reply_partial_inner(zn, id, keyexpr, msg, kind); + // Clean up + _z_keyexpr_clear(keyexpr); + _z_bytes_drop(&msg->_payload); + _z_bytes_drop(&msg->_attachment); + _z_encoding_clear(&msg->_encoding); return ret; } z_result_t _z_trigger_query_reply_err(_z_session_t *zn, _z_zint_t id, _z_msg_err_t *msg) { z_result_t ret = _Z_RES_OK; - _zp_session_lock_mutex(zn); - + // Retrieve query + _z_session_mutex_lock(zn); _z_pending_query_t *pen_qry = __unsafe__z_get_pending_query_by_id(zn, id); - if ((ret == _Z_RES_OK) && (pen_qry == NULL)) { + if (pen_qry == NULL) { ret = _Z_ERR_ENTITY_UNKNOWN; } - - // Build the reply - _z_reply_t reply = _z_reply_err_create(msg->_payload, &msg->_encoding); - - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); // Trigger the user callback if (ret == _Z_RES_OK) { - _z_reply_t cb_reply = _z_reply_null(); - cb_reply = _z_reply_move(&reply); - pen_qry->_callback(&cb_reply, pen_qry->_arg); - _z_reply_clear(&cb_reply); - } - - if (ret != _Z_RES_OK) { - _z_reply_clear(&reply); + _z_reply_t reply = _z_reply_err_alias(&msg->_payload, &msg->_encoding); + pen_qry->_callback(&reply, pen_qry->_arg); } - + // Clean up + _z_bytes_drop(&msg->_payload); + _z_encoding_clear(&msg->_encoding); return ret; } z_result_t _z_trigger_query_reply_final(_z_session_t *zn, _z_zint_t id) { z_result_t ret = _Z_RES_OK; - _zp_session_lock_mutex(zn); - - // Final reply received for unknown query id + // Retrieve query + _z_session_mutex_lock(zn); _z_pending_query_t *pen_qry = __unsafe__z_get_pending_query_by_id(zn, id); - if ((ret == _Z_RES_OK) && (pen_qry == NULL)) { + if (pen_qry == NULL) { ret = _Z_ERR_ENTITY_UNKNOWN; } // The reply is the final one, apply consolidation if needed @@ -225,37 +229,38 @@ z_result_t _z_trigger_query_reply_final(_z_session_t *zn, _z_zint_t id) { _z_pending_reply_t *pen_rep = _z_pending_reply_list_head(pen_qry->_pending_replies); // Trigger the query handler - _z_reply_t cb_reply = _z_reply_null(); - cb_reply = _z_reply_move(&pen_rep->_reply); - pen_qry->_callback(&cb_reply, pen_qry->_arg); + pen_qry->_callback(&pen_rep->_reply, pen_qry->_arg); pen_qry->_pending_replies = _z_pending_reply_list_pop(pen_qry->_pending_replies, NULL); - _z_reply_clear(&cb_reply); } } - if (ret == _Z_RES_OK) { // Dropping a pending query triggers the dropper callback that is now the equivalent to a reply with the FINAL zn->_pending_queries = _z_pending_query_list_drop_filter(zn->_pending_queries, _z_pending_query_eq, pen_qry); } - - _zp_session_unlock_mutex(zn); - + _z_session_mutex_unlock(zn); return ret; } void _z_unregister_pending_query(_z_session_t *zn, _z_pending_query_t *pen_qry) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); zn->_pending_queries = _z_pending_query_list_drop_filter(zn->_pending_queries, _z_pending_query_eq, pen_qry); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } void _z_flush_pending_queries(_z_session_t *zn) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_pending_query_list_free(&zn->_pending_queries); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); +} +#else + +void _z_pending_query_process_timeout(_z_session_t *zn) { + _ZP_UNUSED(zn); + return; } + #endif diff --git a/src/session/queryable.c b/src/session/queryable.c index 3e05a70c3..cc1ef0b82 100644 --- a/src/session/queryable.c +++ b/src/session/queryable.c @@ -25,8 +25,63 @@ #include "zenoh-pico/session/resource.h" #include "zenoh-pico/session/utils.h" #include "zenoh-pico/utils/logging.h" +#include "zenoh-pico/utils/pointers.h" +#include "zenoh-pico/utils/string.h" #if Z_FEATURE_QUERYABLE == 1 + +#define _Z_QLEINFOS_VEC_SIZE 4 // Arbitrary initial size + +#if Z_FEATURE_RX_CACHE == 1 +static inline bool _z_queryable_get_from_cache(_z_session_t *zn, const _z_keyexpr_t *ke, _z_keyexpr_t *ke_val, + _z_queryable_infos_svec_t *infos_val, size_t *qle_nb) { + if (!_z_keyexpr_equals(ke, &zn->_queryable_cache.ke_in)) { + return false; + } + *ke_val = _z_keyexpr_alias(zn->_queryable_cache.ke_out); + *infos_val = _z_queryable_infos_svec_alias(&zn->_queryable_cache.infos); + *qle_nb = zn->_queryable_cache.qle_nb; + return true; +} + +static inline void _z_queryable_update_cache(_z_session_t *zn, const _z_keyexpr_t *ke_in, const _z_keyexpr_t *ke_out, + _z_queryable_infos_svec_t *infos) { + // Clear previous data + _z_queryable_cache_clear(&zn->_queryable_cache); + // Register new info + zn->_queryable_cache.ke_in = _z_keyexpr_duplicate(ke_in); + zn->_queryable_cache.ke_out = _z_keyexpr_duplicate(ke_out); + zn->_queryable_cache.infos = _z_queryable_infos_svec_alias(infos); + zn->_queryable_cache.qle_nb = _z_queryable_infos_svec_len(infos); +} + +void _z_queryable_cache_clear(_z_queryable_cache_t *cache) { + _z_queryable_infos_svec_clear(&cache->infos); + _z_keyexpr_clear(&cache->ke_in); + _z_keyexpr_clear(&cache->ke_out); +} + +#else +static inline bool _z_queryable_get_from_cache(_z_session_t *zn, const _z_keyexpr_t *ke, _z_keyexpr_t *ke_val, + _z_queryable_infos_svec_t *infos_val, size_t *sub_nb) { + _ZP_UNUSED(zn); + _ZP_UNUSED(ke); + _ZP_UNUSED(ke_val); + _ZP_UNUSED(infos_val); + _ZP_UNUSED(sub_nb); + return false; +} + +static inline void _z_queryable_update_cache(_z_session_t *zn, const _z_keyexpr_t *ke_in, const _z_keyexpr_t *ke_out, + _z_queryable_infos_svec_t *infos) { + _ZP_UNUSED(zn); + _ZP_UNUSED(ke_in); + _ZP_UNUSED(ke_out); + _ZP_UNUSED(infos); + return; +} +#endif // Z_FEATURE_RX_CACHE == 1 + bool _z_session_queryable_eq(const _z_session_queryable_t *one, const _z_session_queryable_t *two) { return one->_id == two->_id; } @@ -39,7 +94,8 @@ void _z_session_queryable_clear(_z_session_queryable_t *qle) { } /*------------------ Queryable ------------------*/ -_z_session_queryable_rc_t *__z_get_session_queryable_by_id(_z_session_queryable_rc_list_t *qles, const _z_zint_t id) { +static _z_session_queryable_rc_t *__z_get_session_queryable_by_id(_z_session_queryable_rc_list_t *qles, + const _z_zint_t id) { _z_session_queryable_rc_t *ret = NULL; _z_session_queryable_rc_list_t *xs = qles; @@ -56,29 +112,12 @@ _z_session_queryable_rc_t *__z_get_session_queryable_by_id(_z_session_queryable_ return ret; } -_z_session_queryable_rc_list_t *__z_get_session_queryable_by_key(_z_session_queryable_rc_list_t *qles, - const _z_keyexpr_t key) { - _z_session_queryable_rc_list_t *ret = NULL; - - _z_session_queryable_rc_list_t *xs = qles; - while (xs != NULL) { - _z_session_queryable_rc_t *qle = _z_session_queryable_rc_list_head(xs); - if (_z_keyexpr_suffix_intersects(&_Z_RC_IN_VAL(qle)->_key, &key) == true) { - ret = _z_session_queryable_rc_list_push(ret, _z_session_queryable_rc_clone_as_ptr(qle)); - } - - xs = _z_session_queryable_rc_list_tail(xs); - } - - return ret; -} - /** * This function is unsafe because it operates in potentially concurrent data. * Make sure that the following mutexes are locked before calling this function: * - zn->_mutex_inner */ -_z_session_queryable_rc_t *__unsafe_z_get_session_queryable_by_id(_z_session_t *zn, const _z_zint_t id) { +static _z_session_queryable_rc_t *__unsafe_z_get_session_queryable_by_id(_z_session_t *zn, const _z_zint_t id) { _z_session_queryable_rc_list_t *qles = zn->_local_queryable; return __z_get_session_queryable_by_id(qles, id); } @@ -88,38 +127,41 @@ _z_session_queryable_rc_t *__unsafe_z_get_session_queryable_by_id(_z_session_t * * Make sure that the following mutexes are locked before calling this function: * - zn->_mutex_inner */ -_z_session_queryable_rc_list_t *__unsafe_z_get_session_queryable_by_key(_z_session_t *zn, const _z_keyexpr_t key) { +static z_result_t __unsafe_z_get_session_queryable_by_key(_z_session_t *zn, const _z_keyexpr_t *key, + _z_queryable_infos_svec_t *qle_infos) { _z_session_queryable_rc_list_t *qles = zn->_local_queryable; - return __z_get_session_queryable_by_key(qles, key); + + *qle_infos = _z_queryable_infos_svec_make(_Z_QLEINFOS_VEC_SIZE); + _z_session_queryable_rc_list_t *xs = qles; + while (xs != NULL) { + // Parse queryable list + _z_session_queryable_rc_t *qle = _z_session_queryable_rc_list_head(xs); + if (_z_keyexpr_suffix_intersects(&_Z_RC_IN_VAL(qle)->_key, key)) { + _z_queryable_infos_t new_qle_info = {.arg = _Z_RC_IN_VAL(qle)->_arg, + .callback = _Z_RC_IN_VAL(qle)->_callback}; + _Z_RETURN_IF_ERR(_z_queryable_infos_svec_append(qle_infos, &new_qle_info, false)); + } + xs = _z_session_queryable_rc_list_tail(xs); + } + return _Z_RES_OK; } _z_session_queryable_rc_t *_z_get_session_queryable_by_id(_z_session_t *zn, const _z_zint_t id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_session_queryable_rc_t *qle = __unsafe_z_get_session_queryable_by_id(zn, id); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return qle; } -_z_session_queryable_rc_list_t *_z_get_session_queryable_by_key(_z_session_t *zn, const _z_keyexpr_t *keyexpr) { - _zp_session_lock_mutex(zn); - - _z_keyexpr_t key = __unsafe_z_get_expanded_key_from_key(zn, keyexpr); - _z_session_queryable_rc_list_t *qles = __unsafe_z_get_session_queryable_by_key(zn, key); - - _zp_session_unlock_mutex(zn); - - return qles; -} - _z_session_queryable_rc_t *_z_register_session_queryable(_z_session_t *zn, _z_session_queryable_t *q) { _Z_DEBUG(">>> Allocating queryable for (%ju:%.*s)", (uintmax_t)q->_key._id, (int)_z_string_len(&q->_key._suffix), _z_string_data(&q->_key._suffix)); _z_session_queryable_rc_t *ret = NULL; - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); ret = (_z_session_queryable_rc_t *)z_malloc(sizeof(_z_session_queryable_rc_t)); if (ret != NULL) { @@ -127,60 +169,118 @@ _z_session_queryable_rc_t *_z_register_session_queryable(_z_session_t *zn, _z_se zn->_local_queryable = _z_session_queryable_rc_list_push(zn->_local_queryable, ret); } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return ret; } -z_result_t _z_trigger_queryables(_z_session_rc_t *zsrc, _z_msg_query_t *msgq, const _z_keyexpr_t q_key, uint32_t qid, - const _z_bytes_t attachment) { - z_result_t ret = _Z_RES_OK; - _z_session_t *zn = _Z_RC_IN_VAL(zsrc); - - _zp_session_lock_mutex(zn); - - _z_keyexpr_t key = __unsafe_z_get_expanded_key_from_key(zn, &q_key); - if (_z_keyexpr_has_suffix(&key)) { - _z_session_queryable_rc_list_t *qles = __unsafe_z_get_session_queryable_by_key(zn, key); +static z_result_t _z_session_queryable_get_infos(_z_session_t *zn, const _z_keyexpr_t *keyexpr, _z_keyexpr_t *key, + _z_queryable_infos_svec_t *qles, size_t *qle_nb) { + // Check cache + if (!_z_queryable_get_from_cache(zn, keyexpr, key, qles, qle_nb)) { + _Z_DEBUG("Resolving %d - %.*s on mapping 0x%x", keyexpr->_id, (int)_z_string_len(&keyexpr->_suffix), + _z_string_data(&keyexpr->_suffix), _z_keyexpr_mapping_id(keyexpr)); + _z_session_mutex_lock(zn); + *key = __unsafe_z_get_expanded_key_from_key(zn, keyexpr, true); + + if (!_z_keyexpr_has_suffix(key)) { + _z_session_mutex_unlock(zn); + return _Z_ERR_KEYEXPR_UNKNOWN; + } + // Get queryable list + z_result_t ret = __unsafe_z_get_session_queryable_by_key(zn, key, qles); + _z_session_mutex_unlock(zn); + if (ret != _Z_RES_OK) { + return ret; + } + *qle_nb = _z_queryable_infos_svec_len(qles); + // Update cache + _z_queryable_update_cache(zn, keyexpr, key, qles); + } + return _Z_RES_OK; +} - _zp_session_unlock_mutex(zn); +static inline void _z_queryable_query_steal_data(_z_query_t *query, _z_session_rc_t *zsrc, _z_msg_query_t *msgq, + _z_keyexpr_t *key, uint32_t qid, bool anyke) { + // Steal received data in query + *query = _z_query_alias(&msgq->_ext_value, key, &msgq->_parameters, zsrc, qid, &msgq->_ext_attachment, anyke); + msgq->_ext_value = _z_value_null(); + msgq->_ext_attachment = _z_bytes_null(); + msgq->_parameters = _z_slice_null(); +} - // Build the z_query - _z_query_t q = _z_query_create(&msgq->_ext_value, &key, &msgq->_parameters, zsrc, qid, attachment); - _z_query_rc_t query = _z_query_rc_new_from_val(&q); - // Parse session_queryable list - _z_session_queryable_rc_list_t *xs = qles; - while (xs != NULL) { - _z_session_queryable_rc_t *qle = _z_session_queryable_rc_list_head(xs); - _Z_RC_IN_VAL(qle)->_callback(&query, _Z_RC_IN_VAL(qle)->_arg); - xs = _z_session_queryable_rc_list_tail(xs); +static z_result_t _z_trigger_queryables_inner(_z_session_rc_t *zsrc, _z_msg_query_t *msgq, _z_keyexpr_t *q_key, + uint32_t qid) { + _z_session_t *zn = _Z_RC_IN_VAL(zsrc); + _z_keyexpr_t key; + _z_queryable_infos_svec_t qles; + size_t qle_nb; + // Retrieve sub infos + _Z_RETURN_IF_ERR(_z_session_queryable_get_infos(zn, q_key, &key, &qles, &qle_nb)); + // Check if there are queryables + _Z_DEBUG("Triggering %ju queryables for key %d - %.*s", (uintmax_t)qle_nb, key._id, + (int)_z_string_len(&key._suffix), _z_string_data(&key._suffix)); + if (qle_nb == 0) { + _z_keyexpr_clear(&key); + return _Z_RES_OK; + } + // Check anyke + char *slice_end = _z_ptr_char_offset((char *)msgq->_parameters.start, (ptrdiff_t)msgq->_parameters.len); + bool anyke = false; + if (_z_slice_check(&msgq->_parameters)) { + if (_z_strstr((char *)msgq->_parameters.start, slice_end, Z_SELECTOR_QUERY_MATCH) != NULL) { + anyke = true; } - // Clean up - _z_query_rc_drop(&query); - _z_session_queryable_rc_list_free(&qles); - } else { - _zp_session_unlock_mutex(zn); - ret = _Z_ERR_KEYEXPR_UNKNOWN; } + // Build the z_query + _z_query_t *q = z_malloc(sizeof(_z_query_t)); + if (q == NULL) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + _z_query_rc_t query = _z_query_rc_new(q); + if (_Z_RC_IS_NULL(&query)) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + _z_queryable_query_steal_data(q, zsrc, msgq, &key, qid, anyke); + // Parse session_queryable svec + for (size_t i = 0; i < qle_nb; i++) { + _z_queryable_infos_t *qle_info = _z_queryable_infos_svec_get(&qles, i); + qle_info->callback(&query, qle_info->arg); + } + _z_query_rc_drop(&query); + // Clean up + _z_keyexpr_clear(&key); +#if Z_FEATURE_RX_CACHE != 1 + _z_queryable_infos_svec_release(&qles); // Otherwise it's released with cache +#endif + return _Z_RES_OK; +} +z_result_t _z_trigger_queryables(_z_session_rc_t *zsrc, _z_msg_query_t *msgq, _z_keyexpr_t *q_key, uint32_t qid) { + z_result_t ret = _z_trigger_queryables_inner(zsrc, msgq, q_key, qid); + // Clean up + _z_keyexpr_clear(q_key); + _z_value_clear(&msgq->_ext_value); + _z_bytes_drop(&msgq->_ext_attachment); + _z_slice_clear(&msgq->_parameters); return ret; } void _z_unregister_session_queryable(_z_session_t *zn, _z_session_queryable_rc_t *qle) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); zn->_local_queryable = _z_session_queryable_rc_list_drop_filter(zn->_local_queryable, _z_session_queryable_rc_eq, qle); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } void _z_flush_session_queryable(_z_session_t *zn) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_session_queryable_rc_list_free(&zn->_local_queryable); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } #endif diff --git a/src/session/reply.c b/src/session/reply.c index 7eb13194c..6361099ee 100644 --- a/src/session/reply.c +++ b/src/session/reply.c @@ -19,7 +19,7 @@ #include "zenoh-pico/session/query.h" #include "zenoh-pico/utils/logging.h" -z_result_t _z_trigger_reply_partial(_z_session_t *zn, _z_zint_t id, _z_keyexpr_t key, _z_msg_reply_t *reply) { +z_result_t _z_trigger_reply_partial(_z_session_t *zn, _z_zint_t id, _z_keyexpr_t *key, _z_msg_reply_t *reply) { z_result_t ret = _Z_RES_OK; // TODO check id to know where to dispatch diff --git a/src/session/resource.c b/src/session/resource.c index ec8f90703..b95b4c850 100644 --- a/src/session/resource.c +++ b/src/session/resource.c @@ -92,24 +92,35 @@ _z_resource_t *__z_get_resource_by_key(_z_resource_list_t *rl, const _z_keyexpr_ return ret; } -_z_keyexpr_t __z_get_expanded_key_from_key(_z_resource_list_t *xs, const _z_keyexpr_t *keyexpr) { - _z_keyexpr_t ret = {._id = Z_RESOURCE_ID_NONE, ._suffix = _z_string_null(), ._mapping = _z_keyexpr_mapping(0)}; +static _z_keyexpr_t __z_get_expanded_key_from_key(_z_resource_list_t *xs, const _z_keyexpr_t *keyexpr, + bool force_alias) { + _z_zint_t id = keyexpr->_id; + + // Check if ke is already expanded + if (id == Z_RESOURCE_ID_NONE) { + if (!_z_keyexpr_has_suffix(keyexpr)) { + return _z_keyexpr_null(); + } + // Keyexpr can be aliased from a rx buffer + if (force_alias) { + return _z_keyexpr_alias(*keyexpr); + } else { + return _z_keyexpr_duplicate(keyexpr); + } + } // Need to build the complete resource name, by recursively look at RIDs // Resource names are looked up from right to left + _z_keyexpr_t ret = _z_keyexpr_null(); _z_string_list_t *strs = NULL; size_t len = 0; // Append suffix as the right-most segment if (_z_keyexpr_has_suffix(keyexpr)) { len = len + _z_string_len(&keyexpr->_suffix); - strs = _z_string_list_push(strs, (_z_string_t *)&keyexpr->_suffix); // Warning: list must be release with - // _z_list_free(&strs, _z_noop_free); - // or will release the suffix as well + strs = _z_string_list_push(strs, (_z_string_t *)&keyexpr->_suffix); } - // Recursively go through all the RIDs - _z_zint_t id = keyexpr->_id; uint16_t mapping = _z_keyexpr_mapping_id(keyexpr); while (id != Z_RESOURCE_ID_NONE) { _z_resource_t *res = __z_get_resource_by_id(xs, mapping, id); @@ -119,9 +130,7 @@ _z_keyexpr_t __z_get_expanded_key_from_key(_z_resource_list_t *xs, const _z_keye } if (_z_keyexpr_has_suffix(&res->_key)) { len = len + _z_string_len(&res->_key._suffix); - strs = _z_string_list_push(strs, &res->_key._suffix); // Warning: list must be release with - // _z_list_free(&strs, _z_noop_free); - // or will release the suffix as well + strs = _z_string_list_push(strs, &res->_key._suffix); } id = res->_key._id; } @@ -141,6 +150,7 @@ _z_keyexpr_t __z_get_expanded_key_from_key(_z_resource_list_t *xs, const _z_keye } } } + // Warning: list must be released with _z_list_free(&strs, _z_noop_free) or will release the suffix as well _z_list_free(&strs, _z_noop_free); return ret; } @@ -170,17 +180,17 @@ _z_resource_t *__unsafe_z_get_resource_by_key(_z_session_t *zn, const _z_keyexpr * Make sure that the following mutexes are locked before calling this function: * - zn->_mutex_inner */ -_z_keyexpr_t __unsafe_z_get_expanded_key_from_key(_z_session_t *zn, const _z_keyexpr_t *keyexpr) { +_z_keyexpr_t __unsafe_z_get_expanded_key_from_key(_z_session_t *zn, const _z_keyexpr_t *keyexpr, bool force_alias) { _z_resource_list_t *decls = _z_keyexpr_is_local(keyexpr) ? zn->_local_resources : zn->_remote_resources; - return __z_get_expanded_key_from_key(decls, keyexpr); + return __z_get_expanded_key_from_key(decls, keyexpr, force_alias); } _z_resource_t *_z_get_resource_by_id(_z_session_t *zn, uint16_t mapping, _z_zint_t rid) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_resource_t *res = __unsafe_z_get_resource_by_id(zn, mapping, rid); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return res; } @@ -189,20 +199,20 @@ _z_resource_t *_z_get_resource_by_key(_z_session_t *zn, const _z_keyexpr_t *keye if (!_z_keyexpr_has_suffix(keyexpr)) { return _z_get_resource_by_id(zn, _z_keyexpr_mapping_id(keyexpr), keyexpr->_id); } - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_resource_t *res = __unsafe_z_get_resource_by_key(zn, keyexpr); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return res; } _z_keyexpr_t _z_get_expanded_key_from_key(_z_session_t *zn, const _z_keyexpr_t *keyexpr) { - _zp_session_lock_mutex(zn); - _z_keyexpr_t res = __unsafe_z_get_expanded_key_from_key(zn, keyexpr); + _z_session_mutex_lock(zn); + _z_keyexpr_t res = __unsafe_z_get_expanded_key_from_key(zn, keyexpr, false); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return res; } @@ -210,18 +220,17 @@ _z_keyexpr_t _z_get_expanded_key_from_key(_z_session_t *zn, const _z_keyexpr_t * /// Returns the ID of the registered keyexpr. Returns 0 if registration failed. uint16_t _z_register_resource(_z_session_t *zn, _z_keyexpr_t key, uint16_t id, uint16_t register_to_mapping) { uint16_t ret = Z_RESOURCE_ID_NONE; - key = _z_keyexpr_alias(key); uint16_t mapping = register_to_mapping; uint16_t parent_mapping = _z_keyexpr_mapping_id(&key); - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); if (key._id != Z_RESOURCE_ID_NONE) { if (parent_mapping == mapping) { _z_resource_t *parent = __unsafe_z_get_resource_by_id(zn, parent_mapping, key._id); parent->_refcount++; } else { - key = __unsafe_z_get_expanded_key_from_key(zn, &key); + key = __unsafe_z_get_expanded_key_from_key(zn, &key, false); } } ret = key._id; @@ -231,7 +240,7 @@ uint16_t _z_register_resource(_z_session_t *zn, _z_keyexpr_t key, uint16_t id, u ret = Z_RESOURCE_ID_NONE; } else { res->_refcount = 1; - res->_key = _z_keyexpr_duplicate(key); + res->_key = _z_keyexpr_duplicate(&key); ret = id == Z_RESOURCE_ID_NONE ? _z_get_resource_id(zn) : id; res->_id = ret; // Register the resource @@ -243,7 +252,7 @@ uint16_t _z_register_resource(_z_session_t *zn, _z_keyexpr_t key, uint16_t id, u } } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return ret; } @@ -251,7 +260,7 @@ uint16_t _z_register_resource(_z_session_t *zn, _z_keyexpr_t key, uint16_t id, u void _z_unregister_resource(_z_session_t *zn, uint16_t id, uint16_t mapping) { bool is_local = mapping == _Z_KEYEXPR_MAPPING_LOCAL; _Z_DEBUG("unregistering: id %d, mapping: %d", id, mapping); - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_resource_list_t **parent_mut = is_local ? &zn->_local_resources : &zn->_remote_resources; while (id != 0) { _z_resource_list_t *parent = *parent_mut; @@ -273,7 +282,7 @@ void _z_unregister_resource(_z_session_t *zn, uint16_t id, uint16_t mapping) { parent = *parent_mut; } } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } bool _z_unregister_resource_for_peer_filter(const _z_resource_t *candidate, const _z_resource_t *ctx) { @@ -281,19 +290,19 @@ bool _z_unregister_resource_for_peer_filter(const _z_resource_t *candidate, cons return _z_keyexpr_mapping_id(&candidate->_key) == mapping; } void _z_unregister_resources_for_peer(_z_session_t *zn, uint16_t mapping) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_resource_t ctx = {._id = mapping, ._refcount = 0, ._key = {0}}; zn->_remote_resources = _z_resource_list_drop_filter(zn->_remote_resources, _z_unregister_resource_for_peer_filter, &ctx); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } void _z_flush_resources(_z_session_t *zn) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_resource_list_free(&zn->_local_resources); _z_resource_list_free(&zn->_remote_resources); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } diff --git a/src/session/rx.c b/src/session/rx.c index df53fe63e..eb5ddf6e4 100644 --- a/src/session/rx.c +++ b/src/session/rx.c @@ -32,6 +32,7 @@ #include "zenoh-pico/session/resource.h" #include "zenoh-pico/session/subscription.h" #include "zenoh-pico/session/utils.h" +#include "zenoh-pico/transport/common/tx.h" #include "zenoh-pico/utils/logging.h" /*------------------ Handle message ------------------*/ @@ -101,8 +102,7 @@ z_result_t _z_handle_network_message(_z_session_rc_t *zsrc, _z_zenoh_message_t * case _Z_REQUEST_QUERY: { #if Z_FEATURE_QUERYABLE == 1 _z_msg_query_t *query = &req->_body._query; - ret = _z_trigger_queryables(zsrc, query, req->_key, (uint32_t)req->_rid, - req->_body._query._ext_attachment); + ret = _z_trigger_queryables(zsrc, query, &req->_key, (uint32_t)req->_rid); #else _Z_DEBUG("_Z_REQUEST_QUERY dropped, queryables not supported"); #endif @@ -110,8 +110,8 @@ z_result_t _z_handle_network_message(_z_session_rc_t *zsrc, _z_zenoh_message_t * case _Z_REQUEST_PUT: { #if Z_FEATURE_SUBSCRIPTION == 1 _z_msg_put_t put = req->_body._put; - ret = _z_trigger_subscriptions_put(zn, req->_key, put._payload, &put._encoding, - &put._commons._timestamp, req->_ext_qos, put._attachment, + ret = _z_trigger_subscriptions_put(zn, &req->_key, &put._payload, &put._encoding, + &put._commons._timestamp, req->_ext_qos, &put._attachment, msg->_reliability); #endif if (ret == _Z_RES_OK) { @@ -122,8 +122,8 @@ z_result_t _z_handle_network_message(_z_session_rc_t *zsrc, _z_zenoh_message_t * case _Z_REQUEST_DEL: { #if Z_FEATURE_SUBSCRIPTION == 1 _z_msg_del_t del = req->_body._del; - ret = _z_trigger_subscriptions_del(zn, req->_key, &del._commons._timestamp, req->_ext_qos, - del._attachment, msg->_reliability); + ret = _z_trigger_subscriptions_del(zn, &req->_key, &del._commons._timestamp, req->_ext_qos, + &del._attachment, msg->_reliability); #endif if (ret == _Z_RES_OK) { _z_network_message_t final = _z_n_msg_make_response_final(req->_rid); @@ -138,7 +138,7 @@ z_result_t _z_handle_network_message(_z_session_rc_t *zsrc, _z_zenoh_message_t * switch (response->_tag) { case _Z_RESPONSE_BODY_REPLY: { _z_msg_reply_t *reply = &response->_body._reply; - ret = _z_trigger_reply_partial(zn, response->_request_id, response->_key, reply); + ret = _z_trigger_reply_partial(zn, response->_request_id, &response->_key, reply); } break; case _Z_RESPONSE_BODY_ERR: { _z_msg_err_t *error = &response->_body._err; @@ -164,6 +164,7 @@ z_result_t _z_handle_network_message(_z_session_rc_t *zsrc, _z_zenoh_message_t * } } } + // TODO (sashacmc): why it was removed??? _z_msg_clear(msg); return ret; } diff --git a/src/session/scout.c b/src/session/scout.c index 7c35d1e21..5badf7201 100644 --- a/src/session/scout.c +++ b/src/session/scout.c @@ -76,7 +76,7 @@ _z_hello_list_t *__z_scout_loop(const _z_wbuf_t *wbf, _z_string_t *locator, unsi switch (_Z_MID(s_msg._header)) { case _Z_MID_HELLO: { - _Z_INFO("Received _Z_HELLO message"); + _Z_DEBUG("Received _Z_HELLO message"); _z_hello_t *hello = (_z_hello_t *)z_malloc(sizeof(_z_hello_t)); if (hello != NULL) { hello->_version = s_msg._body._hello._version; @@ -89,7 +89,7 @@ _z_hello_list_t *__z_scout_loop(const _z_wbuf_t *wbf, _z_string_t *locator, unsi for (size_t i = 0; i < n_loc; i++) { _z_string_t s = _z_locator_to_string(&s_msg._body._hello._locators._val[i]); - _z_string_svec_append(&hello->_locators, &s); + _z_string_svec_append(&hello->_locators, &s, true); } } else { // @TODO: construct the locator departing from the sock address diff --git a/src/session/subscription.c b/src/session/subscription.c index eb611815e..f614ddf0e 100644 --- a/src/session/subscription.c +++ b/src/session/subscription.c @@ -30,6 +30,60 @@ #include "zenoh-pico/utils/logging.h" #if Z_FEATURE_SUBSCRIPTION == 1 + +#define _Z_SUBINFOS_VEC_SIZE 4 // Arbitrary initial size + +#if Z_FEATURE_RX_CACHE == 1 +static inline bool _z_subscription_get_from_cache(_z_session_t *zn, const _z_keyexpr_t *ke, _z_keyexpr_t *ke_val, + _z_subscription_infos_svec_t *infos_val, size_t *sub_nb) { + if (!_z_keyexpr_equals(ke, &zn->_subscription_cache.ke_in)) { + return false; + } + *ke_val = _z_keyexpr_alias(zn->_subscription_cache.ke_out); + *infos_val = _z_subscription_infos_svec_alias(&zn->_subscription_cache.infos); + *sub_nb = zn->_subscription_cache.sub_nb; + return true; +} + +static inline void _z_subscription_update_cache(_z_session_t *zn, const _z_keyexpr_t *ke_in, const _z_keyexpr_t *ke_out, + _z_subscription_infos_svec_t *infos) { + // Clear previous data + _z_subscription_cache_clear(&zn->_subscription_cache); + // Register new info + zn->_subscription_cache.ke_in = _z_keyexpr_duplicate(ke_in); + zn->_subscription_cache.ke_out = _z_keyexpr_duplicate(ke_out); + zn->_subscription_cache.infos = _z_subscription_infos_svec_alias(infos); + zn->_subscription_cache.sub_nb = _z_subscription_infos_svec_len(infos); +} + +void _z_subscription_cache_clear(_z_subscription_cache_t *cache) { + _z_subscription_infos_svec_clear(&cache->infos); + _z_keyexpr_clear(&cache->ke_in); + _z_keyexpr_clear(&cache->ke_out); +} + +#else +static inline bool _z_subscription_get_from_cache(_z_session_t *zn, const _z_keyexpr_t *ke, _z_keyexpr_t *ke_val, + _z_subscription_infos_svec_t *infos_val, size_t *sub_nb) { + _ZP_UNUSED(zn); + _ZP_UNUSED(ke); + _ZP_UNUSED(ke_val); + _ZP_UNUSED(infos_val); + _ZP_UNUSED(sub_nb); + return false; +} + +static inline void _z_subscription_update_cache(_z_session_t *zn, const _z_keyexpr_t *ke_in, const _z_keyexpr_t *ke_out, + _z_subscription_infos_svec_t *infos) { + _ZP_UNUSED(zn); + _ZP_UNUSED(ke_in); + _ZP_UNUSED(ke_out); + _ZP_UNUSED(infos); + return; +} +#endif // Z_FEATURE_RX_CACHE == 1 + +// Subscription bool _z_subscription_eq(const _z_subscription_t *other, const _z_subscription_t *this_) { return this_->_id == other->_id; } @@ -58,22 +112,6 @@ _z_subscription_rc_t *__z_get_subscription_by_id(_z_subscription_rc_list_t *subs return ret; } -_z_subscription_rc_list_t *__z_get_subscriptions_by_key(_z_subscription_rc_list_t *subs, const _z_keyexpr_t *key) { - _z_subscription_rc_list_t *ret = NULL; - - _z_subscription_rc_list_t *xs = subs; - while (xs != NULL) { - _z_subscription_rc_t *sub = _z_subscription_rc_list_head(xs); - if (_z_keyexpr_suffix_intersects(&_Z_RC_IN_VAL(sub)->_key, key) == true) { - ret = _z_subscription_rc_list_push(ret, _z_subscription_rc_clone_as_ptr(sub)); - } - - xs = _z_subscription_rc_list_tail(xs); - } - - return ret; -} - /** * This function is unsafe because it operates in potentially concurrent data. * Make sure that the following mutexes are locked before calling this function: @@ -91,40 +129,43 @@ _z_subscription_rc_t *__unsafe_z_get_subscription_by_id(_z_session_t *zn, _z_sub * Make sure that the following mutexes are locked before calling this function: * - zn->_mutex_inner */ -_z_subscription_rc_list_t *__unsafe_z_get_subscriptions_by_key(_z_session_t *zn, _z_subscriber_kind_t kind, - const _z_keyexpr_t *key) { +static z_result_t __unsafe_z_get_subscriptions_by_key(_z_session_t *zn, _z_subscriber_kind_t kind, + const _z_keyexpr_t *key, + _z_subscription_infos_svec_t *sub_infos) { _z_subscription_rc_list_t *subs = (kind == _Z_SUBSCRIBER_KIND_SUBSCRIBER) ? zn->_subscriptions : zn->_liveliness_subscriptions; - return __z_get_subscriptions_by_key(subs, key); + + *sub_infos = _z_subscription_infos_svec_make(_Z_SUBINFOS_VEC_SIZE); + _z_subscription_rc_list_t *xs = subs; + while (xs != NULL) { + // Parse subscription list + _z_subscription_rc_t *sub = _z_subscription_rc_list_head(xs); + if (_z_keyexpr_suffix_intersects(&_Z_RC_IN_VAL(sub)->_key, key)) { + _z_subscription_infos_t new_sub_info = {.arg = _Z_RC_IN_VAL(sub)->_arg, + .callback = _Z_RC_IN_VAL(sub)->_callback}; + _Z_RETURN_IF_ERR(_z_subscription_infos_svec_append(sub_infos, &new_sub_info, false)); + } + xs = _z_subscription_rc_list_tail(xs); + } + return _Z_RES_OK; } _z_subscription_rc_t *_z_get_subscription_by_id(_z_session_t *zn, _z_subscriber_kind_t kind, const _z_zint_t id) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_subscription_rc_t *sub = __unsafe_z_get_subscription_by_id(zn, kind, id); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return sub; } -_z_subscription_rc_list_t *_z_get_subscriptions_by_key(_z_session_t *zn, _z_subscriber_kind_t kind, - const _z_keyexpr_t *key) { - _zp_session_lock_mutex(zn); - - _z_subscription_rc_list_t *subs = __unsafe_z_get_subscriptions_by_key(zn, kind, key); - - _zp_session_unlock_mutex(zn); - - return subs; -} - _z_subscription_rc_t *_z_register_subscription(_z_session_t *zn, _z_subscriber_kind_t kind, _z_subscription_t *s) { _Z_DEBUG(">>> Allocating sub decl for (%ju:%.*s)", (uintmax_t)s->_key._id, (int)_z_string_len(&s->_key._suffix), _z_string_data(&s->_key._suffix)); _z_subscription_rc_t *ret = NULL; - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); ret = (_z_subscription_rc_t *)z_malloc(sizeof(_z_subscription_rc_t)); if (ret != NULL) { @@ -136,84 +177,127 @@ _z_subscription_rc_t *_z_register_subscription(_z_session_t *zn, _z_subscriber_k } } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); return ret; } -z_result_t _z_trigger_subscriptions_put(_z_session_t *zn, const _z_keyexpr_t keyexpr, const _z_bytes_t payload, +z_result_t _z_trigger_subscriptions_put(_z_session_t *zn, _z_keyexpr_t *keyexpr, _z_bytes_t *payload, _z_encoding_t *encoding, const _z_timestamp_t *timestamp, const _z_n_qos_t qos, - const _z_bytes_t attachment, z_reliability_t reliability) { + _z_bytes_t *attachment, z_reliability_t reliability) { return _z_trigger_subscriptions_impl(zn, _Z_SUBSCRIBER_KIND_SUBSCRIBER, keyexpr, payload, encoding, Z_SAMPLE_KIND_PUT, timestamp, qos, attachment, reliability); } -z_result_t _z_trigger_subscriptions_del(_z_session_t *zn, const _z_keyexpr_t keyexpr, const _z_timestamp_t *timestamp, - const _z_n_qos_t qos, const _z_bytes_t attachment, - z_reliability_t reliability) { +z_result_t _z_trigger_subscriptions_del(_z_session_t *zn, _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp, + const _z_n_qos_t qos, _z_bytes_t *attachment, z_reliability_t reliability) { _z_encoding_t encoding = _z_encoding_null(); - return _z_trigger_subscriptions_impl(zn, _Z_SUBSCRIBER_KIND_SUBSCRIBER, keyexpr, _z_bytes_null(), &encoding, + _z_bytes_t payload = _z_bytes_null(); + return _z_trigger_subscriptions_impl(zn, _Z_SUBSCRIBER_KIND_SUBSCRIBER, keyexpr, &payload, &encoding, Z_SAMPLE_KIND_DELETE, timestamp, qos, attachment, reliability); } -z_result_t _z_trigger_liveliness_subscriptions_declare(_z_session_t *zn, const _z_keyexpr_t keyexpr, +z_result_t _z_trigger_liveliness_subscriptions_declare(_z_session_t *zn, _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp) { _z_encoding_t encoding = _z_encoding_null(); - return _z_trigger_subscriptions_impl(zn, _Z_SUBSCRIBER_KIND_LIVELINESS_SUBSCRIBER, keyexpr, _z_bytes_null(), - &encoding, Z_SAMPLE_KIND_PUT, timestamp, _Z_N_QOS_DEFAULT, _z_bytes_null(), + _z_bytes_t payload = _z_bytes_null(); + _z_bytes_t attachment = _z_bytes_null(); + _z_keyexpr_t key = _z_keyexpr_alias(*keyexpr); + return _z_trigger_subscriptions_impl(zn, _Z_SUBSCRIBER_KIND_LIVELINESS_SUBSCRIBER, &key, &payload, &encoding, + Z_SAMPLE_KIND_PUT, timestamp, _Z_N_QOS_DEFAULT, &attachment, Z_RELIABILITY_RELIABLE); } -z_result_t _z_trigger_liveliness_subscriptions_undeclare(_z_session_t *zn, const _z_keyexpr_t keyexpr, +z_result_t _z_trigger_liveliness_subscriptions_undeclare(_z_session_t *zn, _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp) { _z_encoding_t encoding = _z_encoding_null(); - return _z_trigger_subscriptions_impl(zn, _Z_SUBSCRIBER_KIND_LIVELINESS_SUBSCRIBER, keyexpr, _z_bytes_null(), - &encoding, Z_SAMPLE_KIND_DELETE, timestamp, _Z_N_QOS_DEFAULT, _z_bytes_null(), + _z_bytes_t payload = _z_bytes_null(); + _z_bytes_t attachment = _z_bytes_null(); + _z_keyexpr_t key = _z_keyexpr_alias(*keyexpr); + return _z_trigger_subscriptions_impl(zn, _Z_SUBSCRIBER_KIND_LIVELINESS_SUBSCRIBER, &key, &payload, &encoding, + Z_SAMPLE_KIND_DELETE, timestamp, _Z_N_QOS_DEFAULT, &attachment, Z_RELIABILITY_RELIABLE); } -z_result_t _z_trigger_subscriptions_impl(_z_session_t *zn, _z_subscriber_kind_t subscriber_kind, - const _z_keyexpr_t keyexpr, const _z_bytes_t payload, _z_encoding_t *encoding, - const _z_zint_t sample_kind, const _z_timestamp_t *timestamp, - const _z_n_qos_t qos, const _z_bytes_t attachment, - z_reliability_t reliability) { - z_result_t ret = _Z_RES_OK; - - _zp_session_lock_mutex(zn); - - _Z_DEBUG("Resolving %d - %.*s on mapping 0x%x", keyexpr._id, (int)_z_string_len(&keyexpr._suffix), - _z_string_data(&keyexpr._suffix), _z_keyexpr_mapping_id(&keyexpr)); - _z_keyexpr_t key = __unsafe_z_get_expanded_key_from_key(zn, &keyexpr); - _Z_DEBUG("Triggering subs for %d - %.*s", key._id, (int)_z_string_len(&key._suffix), _z_string_data(&key._suffix)); - if (_z_keyexpr_has_suffix(&key)) { - _z_subscription_rc_list_t *subs = __unsafe_z_get_subscriptions_by_key(zn, subscriber_kind, &key); - - _zp_session_unlock_mutex(zn); - - // Build the sample - _z_sample_t sample = - _z_sample_create(&key, payload, timestamp, encoding, sample_kind, qos, attachment, reliability); - // Parse subscription list - _z_subscription_rc_list_t *xs = subs; - _Z_DEBUG("Triggering %ju subs", (uintmax_t)_z_subscription_rc_list_len(xs)); - while (xs != NULL) { - _z_subscription_rc_t *sub = _z_subscription_rc_list_head(xs); - _Z_RC_IN_VAL(sub)->_callback(&sample, _Z_RC_IN_VAL(sub)->_arg); - xs = _z_subscription_rc_list_tail(xs); +static z_result_t _z_subscription_get_infos(_z_session_t *zn, _z_subscriber_kind_t kind, const _z_keyexpr_t *keyexpr, + _z_keyexpr_t *key, _z_subscription_infos_svec_t *subs, size_t *sub_nb) { + // Check cache + if (!_z_subscription_get_from_cache(zn, keyexpr, key, subs, sub_nb)) { + _Z_DEBUG("Resolving %d - %.*s on mapping 0x%x", keyexpr->_id, (int)_z_string_len(&keyexpr->_suffix), + _z_string_data(&keyexpr->_suffix), _z_keyexpr_mapping_id(keyexpr)); + _z_session_mutex_lock(zn); + *key = __unsafe_z_get_expanded_key_from_key(zn, keyexpr, true); + + if (!_z_keyexpr_has_suffix(key)) { + _z_session_mutex_unlock(zn); + return _Z_ERR_KEYEXPR_UNKNOWN; } - // Clean up - _z_sample_clear(&sample); - _z_subscription_rc_list_free(&subs); + // Get subscription list + z_result_t ret = __unsafe_z_get_subscriptions_by_key(zn, kind, key, subs); + _z_session_mutex_unlock(zn); + if (ret != _Z_RES_OK) { + return ret; + } + *sub_nb = _z_subscription_infos_svec_len(subs); + // Update cache + _z_subscription_update_cache(zn, keyexpr, key, subs); + } + return _Z_RES_OK; +} + +static z_result_t _z_trigger_subscriptions_inner(_z_session_t *zn, _z_subscriber_kind_t sub_kind, + const _z_keyexpr_t *keyexpr, _z_bytes_t *payload, + _z_encoding_t *encoding, const _z_zint_t sample_kind, + const _z_timestamp_t *timestamp, const _z_n_qos_t qos, + _z_bytes_t *attachment, z_reliability_t reliability) { + _z_keyexpr_t key; + _z_subscription_infos_svec_t subs; + size_t sub_nb; + // Retrieve sub infos + _Z_RETURN_IF_ERR(_z_subscription_get_infos(zn, sub_kind, keyexpr, &key, &subs, &sub_nb)); + // Check if there are subs + _Z_DEBUG("Triggering %ju subs for key %d - %.*s", (uintmax_t)sub_nb, key._id, (int)_z_string_len(&key._suffix), + _z_string_data(&key._suffix)); + if (sub_nb == 0) { + _z_keyexpr_clear(&key); + _z_subscription_infos_svec_release(&subs); + return _Z_RES_OK; + } + // Create sample + _z_sample_t sample = _z_sample_alias(&key, payload, timestamp, encoding, sample_kind, qos, attachment, reliability); + // Parse subscription infos svec + for (size_t i = 0; i < sub_nb; i++) { + _z_subscription_infos_t *sub_info = _z_subscription_infos_svec_get(&subs, i); + sub_info->callback(&sample, sub_info->arg); + } + // Clean up + _z_keyexpr_clear(&key); + if (sub_kind == _Z_SUBSCRIBER_KIND_LIVELINESS_SUBSCRIBER) { + _z_subscription_infos_svec_release(&subs); } else { - _zp_session_unlock_mutex(zn); - ret = _Z_ERR_KEYEXPR_UNKNOWN; +#if Z_FEATURE_RX_CACHE != 1 + _z_subscription_infos_svec_release(&subs); // Otherwise it's released with cache +#endif } + return _Z_RES_OK; +} +z_result_t _z_trigger_subscriptions_impl(_z_session_t *zn, _z_subscriber_kind_t sub_kind, _z_keyexpr_t *keyexpr, + _z_bytes_t *payload, _z_encoding_t *encoding, const _z_zint_t sample_kind, + const _z_timestamp_t *timestamp, const _z_n_qos_t qos, _z_bytes_t *attachment, + z_reliability_t reliability) { + z_result_t ret = _z_trigger_subscriptions_inner(zn, sub_kind, keyexpr, payload, encoding, sample_kind, timestamp, + qos, attachment, reliability); + // Clean up + _z_keyexpr_clear(keyexpr); + _z_encoding_clear(encoding); + _z_bytes_drop(payload); + _z_bytes_drop(attachment); return ret; } void _z_unregister_subscription(_z_session_t *zn, _z_subscriber_kind_t kind, _z_subscription_rc_t *sub) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); if (kind == _Z_SUBSCRIBER_KIND_SUBSCRIBER) { zn->_subscriptions = _z_subscription_rc_list_drop_filter(zn->_subscriptions, _z_subscription_rc_eq, sub); @@ -222,22 +306,22 @@ void _z_unregister_subscription(_z_session_t *zn, _z_subscriber_kind_t kind, _z_ _z_subscription_rc_list_drop_filter(zn->_liveliness_subscriptions, _z_subscription_rc_eq, sub); } - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } void _z_flush_subscriptions(_z_session_t *zn) { - _zp_session_lock_mutex(zn); + _z_session_mutex_lock(zn); _z_subscription_rc_list_free(&zn->_subscriptions); _z_subscription_rc_list_free(&zn->_liveliness_subscriptions); - _zp_session_unlock_mutex(zn); + _z_session_mutex_unlock(zn); } #else // Z_FEATURE_SUBSCRIPTION == 0 -z_result_t _z_trigger_subscriptions_put(_z_session_t *zn, const _z_keyexpr_t keyexpr, const _z_bytes_t payload, +z_result_t _z_trigger_subscriptions_put(_z_session_t *zn, _z_keyexpr_t *keyexpr, _z_bytes_t *payload, _z_encoding_t *encoding, const _z_timestamp_t *timestamp, const _z_n_qos_t qos, - const _z_bytes_t attachment, z_reliability_t reliability) { + _z_bytes_t *attachment, z_reliability_t reliability) { _ZP_UNUSED(zn); _ZP_UNUSED(keyexpr); _ZP_UNUSED(payload); @@ -250,9 +334,8 @@ z_result_t _z_trigger_subscriptions_put(_z_session_t *zn, const _z_keyexpr_t key return _Z_RES_OK; } -z_result_t _z_trigger_subscriptions_del(_z_session_t *zn, const _z_keyexpr_t keyexpr, const _z_timestamp_t *timestamp, - const _z_n_qos_t qos, const _z_bytes_t attachment, - z_reliability_t reliability) { +z_result_t _z_trigger_subscriptions_del(_z_session_t *zn, _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp, + const _z_n_qos_t qos, _z_bytes_t *attachment, z_reliability_t reliability) { _ZP_UNUSED(zn); _ZP_UNUSED(keyexpr); _ZP_UNUSED(qos); @@ -263,7 +346,7 @@ z_result_t _z_trigger_subscriptions_del(_z_session_t *zn, const _z_keyexpr_t key return _Z_RES_OK; } -z_result_t _z_trigger_liveliness_subscriptions_declare(_z_session_t *zn, const _z_keyexpr_t keyexpr, +z_result_t _z_trigger_liveliness_subscriptions_declare(_z_session_t *zn, _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp) { _ZP_UNUSED(zn); _ZP_UNUSED(keyexpr); @@ -272,7 +355,7 @@ z_result_t _z_trigger_liveliness_subscriptions_declare(_z_session_t *zn, const _ return _Z_RES_OK; } -z_result_t _z_trigger_liveliness_subscriptions_undeclare(_z_session_t *zn, const _z_keyexpr_t keyexpr, +z_result_t _z_trigger_liveliness_subscriptions_undeclare(_z_session_t *zn, _z_keyexpr_t *keyexpr, const _z_timestamp_t *timestamp) { _ZP_UNUSED(zn); _ZP_UNUSED(keyexpr); diff --git a/src/session/tx.c b/src/session/tx.c deleted file mode 100644 index 64746930e..000000000 --- a/src/session/tx.c +++ /dev/null @@ -1,41 +0,0 @@ -// -// 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, -// - -#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/logging.h" - -z_result_t _z_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl) { - z_result_t ret = _Z_RES_OK; - _Z_DEBUG(">> send network message"); - // Call transport function - switch (zn->_tp._type) { - case _Z_TRANSPORT_UNICAST_TYPE: - ret = _z_unicast_send_n_msg(zn, z_msg, reliability, cong_ctrl); - break; - case _Z_TRANSPORT_MULTICAST_TYPE: - ret = _z_multicast_send_n_msg(zn, z_msg, reliability, cong_ctrl); - break; - case _Z_TRANSPORT_RAWETH_TYPE: - ret = _z_raweth_send_n_msg(zn, z_msg, reliability, cong_ctrl); - break; - default: - ret = _Z_ERR_TRANSPORT_NOT_AVAILABLE; - break; - } - return ret; -} diff --git a/src/session/utils.c b/src/session/utils.c index 44874a115..90ec97912 100644 --- a/src/session/utils.c +++ b/src/session/utils.c @@ -26,20 +26,20 @@ #include "zenoh-pico/session/subscription.h" /*------------------ clone helpers ------------------*/ -_z_timestamp_t _z_timestamp_duplicate(const _z_timestamp_t *tstamp) { - _z_timestamp_t ts; - ts.id = tstamp->id; - ts.time = tstamp->time; - return ts; +void _z_timestamp_copy(_z_timestamp_t *dst, const _z_timestamp_t *src) { *dst = *src; } + +_z_timestamp_t _z_timestamp_duplicate(const _z_timestamp_t *tstamp) { return *tstamp; } + +void _z_timestamp_move(_z_timestamp_t *dst, _z_timestamp_t *src) { + *dst = *src; + _z_timestamp_clear(src); } void _z_timestamp_clear(_z_timestamp_t *tstamp) { - memset(&tstamp->id, 0, sizeof(_z_id_t)); + tstamp->valid = false; tstamp->time = 0; } -bool _z_timestamp_check(const _z_timestamp_t *stamp) { return _z_id_check(stamp->id); } - z_result_t _z_session_generate_zid(_z_id_t *bs, uint8_t size) { z_result_t ret = _Z_RES_OK; z_random_fill((uint8_t *)bs->id, size); @@ -62,9 +62,15 @@ z_result_t _z_session_init(_z_session_rc_t *zsrc, _z_id_t *zid) { #if Z_FEATURE_SUBSCRIPTION == 1 zn->_subscriptions = NULL; zn->_liveliness_subscriptions = NULL; +#if Z_FEATURE_RX_CACHE == 1 + memset(&zn->_subscription_cache, 0, sizeof(zn->_subscription_cache)); +#endif #endif #if Z_FEATURE_QUERYABLE == 1 zn->_local_queryable = NULL; +#if Z_FEATURE_RX_CACHE == 1 + memset(&zn->_queryable_cache, 0, sizeof(zn->_queryable_cache)); +#endif #endif #if Z_FEATURE_QUERY == 1 zn->_pending_queries = NULL; @@ -86,13 +92,13 @@ z_result_t _z_session_init(_z_session_rc_t *zsrc, _z_id_t *zid) { // Note session in transport switch (zn->_tp._type) { case _Z_TRANSPORT_UNICAST_TYPE: - zn->_tp._transport._unicast._session = zsrc; + zn->_tp._transport._unicast._common._session = zsrc; break; case _Z_TRANSPORT_MULTICAST_TYPE: - zn->_tp._transport._multicast._session = zsrc; + zn->_tp._transport._multicast._common._session = zsrc; break; case _Z_TRANSPORT_RAWETH_TYPE: - zn->_tp._transport._raweth._session = zsrc; + zn->_tp._transport._raweth._common._session = zsrc; break; default: break; @@ -117,9 +123,15 @@ void _z_session_clear(_z_session_t *zn) { _z_flush_resources(zn); #if Z_FEATURE_SUBSCRIPTION == 1 _z_flush_subscriptions(zn); +#if Z_FEATURE_RX_CACHE == 1 + _z_subscription_cache_clear(&zn->_subscription_cache); +#endif #endif #if Z_FEATURE_QUERYABLE == 1 _z_flush_session_queryable(zn); +#if Z_FEATURE_RX_CACHE == 1 + _z_queryable_cache_clear(&zn->_queryable_cache); +#endif #endif #if Z_FEATURE_QUERY == 1 _z_flush_pending_queries(zn); @@ -143,11 +155,3 @@ z_result_t _z_session_close(_z_session_t *zn, uint8_t reason) { return ret; } - -#if Z_FEATURE_MULTI_THREAD == 1 -void _zp_session_lock_mutex(_z_session_t *zn) { (void)_z_mutex_lock(&zn->_mutex_inner); } -void _zp_session_unlock_mutex(_z_session_t *zn) { (void)_z_mutex_unlock(&zn->_mutex_inner); } -#else -void _zp_session_lock_mutex(_z_session_t *zn) { _ZP_UNUSED(zn); } -void _zp_session_unlock_mutex(_z_session_t *zn) { _ZP_UNUSED(zn); } -#endif diff --git a/src/system/arduino/esp32/network.cpp b/src/system/arduino/esp32/network.cpp index 8c88d4dc6..c0a82927d 100644 --- a/src/system/arduino/esp32/network.cpp +++ b/src/system/arduino/esp32/network.cpp @@ -500,7 +500,7 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ if (!((a->sin_port == b->sin_port) && (a->sin_addr.s_addr == b->sin_addr.s_addr))) { // If addr is not NULL, it means that the raddr was requested by the upper-layers if (addr != NULL) { - *addr = _z_slice_make(sizeof(in_addr_t) + sizeof(in_port_t)); + addr->len = sizeof(in_addr_t) + sizeof(in_port_t); (void)memcpy((uint8_t *)addr->start, &b->sin_addr.s_addr, sizeof(in_addr_t)); (void)memcpy((uint8_t *)(addr->start + sizeof(in_addr_t)), &b->sin_port, sizeof(in_port_t)); } @@ -513,7 +513,7 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ (memcmp(&a->sin6_addr, &b->sin6_addr, sizeof(struct in6_addr)) != 0)) { // If addr is not NULL, it means that the raddr was requested by the upper-layers if (addr != NULL) { - *addr = _z_slice_make(sizeof(struct in6_addr) + sizeof(in_port_t)); + addr->len = sizeof(struct in6_addr) + sizeof(in_port_t); (void)memcpy((uint8_t *)addr->start, &b->sin6_addr.s6_addr, sizeof(struct in6_addr)); (void)memcpy((uint8_t *)(addr->start + sizeof(struct in6_addr)), &b->sin6_port, sizeof(in_port_t)); } diff --git a/src/system/arduino/opencr/network.cpp b/src/system/arduino/opencr/network.cpp index b690559f1..c02765d8c 100644 --- a/src/system/arduino/opencr/network.cpp +++ b/src/system/arduino/opencr/network.cpp @@ -295,8 +295,8 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ IPAddress rip = sock._udp->remoteIP(); uint16_t rport = sock._udp->remotePort(); - *addr = _z_slice_make(strlen((const char *)&rip[0]) + strlen((const char *)&rip[1]) + - strlen((const char *)&rip[2]) + strlen((const char *)&rip[3]) + sizeof(uint16_t)); + addr->len = strlen((const char *)&rip[0]) + strlen((const char *)&rip[1]) + + strlen((const char *)&rip[2]) + strlen((const char *)&rip[3]) + sizeof(uint16_t); uint8_t offset = 0; for (uint8_t i = 0; i < (uint8_t)4; i++) { (void)memcpy(const_cast(addr->start + offset), &rip[i], strlen((const char *)&rip[i])); diff --git a/src/system/espidf/network.c b/src/system/espidf/network.c index bc91f50c3..0634d2a23 100644 --- a/src/system/espidf/network.c +++ b/src/system/espidf/network.c @@ -496,7 +496,7 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ if (!((a->sin_port == b->sin_port) && (a->sin_addr.s_addr == b->sin_addr.s_addr))) { // If addr is not NULL, it means that the raddr was requested by the upper-layers if (addr != NULL) { - *addr = _z_slice_make(sizeof(in_addr_t) + sizeof(in_port_t)); + addr->len = sizeof(in_addr_t) + sizeof(in_port_t); (void)memcpy((uint8_t *)addr->start, &b->sin_addr.s_addr, sizeof(in_addr_t)); (void)memcpy((uint8_t *)(addr->start + sizeof(in_addr_t)), &b->sin_port, sizeof(in_port_t)); } @@ -509,7 +509,7 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ (memcmp(&a->sin6_addr, &b->sin6_addr, sizeof(struct in6_addr)) != 0)) { // If addr is not NULL, it means that the raddr was requested by the upper-layers if (addr != NULL) { - *addr = _z_slice_make(sizeof(struct in6_addr) + sizeof(in_port_t)); + addr->len = sizeof(struct in6_addr) + sizeof(in_port_t); (void)memcpy((uint8_t *)addr->start, &b->sin6_addr.s6_addr, sizeof(struct in6_addr)); (void)memcpy((uint8_t *)(addr->start + sizeof(struct in6_addr)), &b->sin6_port, sizeof(in_port_t)); } diff --git a/src/system/mbed/network.cpp b/src/system/mbed/network.cpp index 310a107af..a9d365782 100644 --- a/src/system/mbed/network.cpp +++ b/src/system/mbed/network.cpp @@ -274,13 +274,13 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ } if (raddr.get_ip_version() == NSAPI_IPv4) { - *addr = _z_slice_make(NSAPI_IPv4_BYTES + sizeof(uint16_t)); + addr->len = NSAPI_IPv4_BYTES + sizeof(uint16_t); (void)memcpy(const_cast(addr->start), raddr.get_ip_bytes(), NSAPI_IPv4_BYTES); uint16_t port = raddr.get_port(); (void)memcpy(const_cast(addr->start + NSAPI_IPv4_BYTES), &port, sizeof(uint16_t)); break; } else if (raddr.get_ip_version() == NSAPI_IPv6) { - *addr = _z_slice_make(NSAPI_IPv6_BYTES + sizeof(uint16_t)); + addr->len = NSAPI_IPv6_BYTES + sizeof(uint16_t); (void)memcpy(const_cast(addr->start), raddr.get_ip_bytes(), NSAPI_IPv6_BYTES); uint16_t port = raddr.get_port(); (void)memcpy(const_cast(addr->start + NSAPI_IPv6_BYTES), &port, sizeof(uint16_t)); diff --git a/src/system/unix/link/raweth.c b/src/system/unix/link/raweth.c index f1abcf325..6e15cae0d 100644 --- a/src/system/unix/link/raweth.c +++ b/src/system/unix/link/raweth.c @@ -118,7 +118,7 @@ size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buf // Copy sender mac if needed if (addr != NULL) { uint8_t *header_addr = (uint8_t *)buff; - *addr = _z_slice_make(sizeof(ETH_ALEN)); + addr->len = sizeof(ETH_ALEN); (void)memcpy((uint8_t *)addr->start, (header_addr + ETH_ALEN), sizeof(ETH_ALEN)); } return (size_t)bytesRead; diff --git a/src/system/unix/network.c b/src/system/unix/network.c index 86be15dc0..c822e4aa5 100644 --- a/src/system/unix/network.c +++ b/src/system/unix/network.c @@ -13,6 +13,7 @@ // #include +#include #include #include #include @@ -114,12 +115,69 @@ z_result_t _z_open_tcp(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t re z_result_t _z_listen_tcp(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t lep) { z_result_t ret = _Z_RES_OK; - (void)sock; - (void)lep; - - // @TODO: To be implemented - ret = _Z_ERR_GENERIC; - + // Open socket + sock->_fd = socket(lep._iptcp->ai_family, lep._iptcp->ai_socktype, lep._iptcp->ai_protocol); + if (sock->_fd == -1) { + return _Z_ERR_GENERIC; + } + // Set options + int value = true; + if ((ret == _Z_RES_OK) && (setsockopt(sock->_fd, SOL_SOCKET, SO_REUSEADDR, &value, sizeof(value)) < 0)) { + ret = _Z_ERR_GENERIC; + } + int flags = 1; + if ((ret == _Z_RES_OK) && (setsockopt(sock->_fd, SOL_SOCKET, SO_KEEPALIVE, (void *)&flags, sizeof(flags)) < 0)) { + ret = _Z_ERR_GENERIC; + } +#if Z_FEATURE_TCP_NODELAY == 1 + if ((ret == _Z_RES_OK) && (setsockopt(sock->_fd, IPPROTO_TCP, TCP_NODELAY, (void *)&flags, sizeof(flags)) < 0)) { + ret = _Z_ERR_GENERIC; + } +#endif + struct linger ling; + ling.l_onoff = 1; + ling.l_linger = Z_TRANSPORT_LEASE / 1000; + if ((ret == _Z_RES_OK) && + (setsockopt(sock->_fd, SOL_SOCKET, SO_LINGER, (void *)&ling, sizeof(struct linger)) < 0)) { + ret = _Z_ERR_GENERIC; + } +#if defined(ZENOH_MACOS) || defined(ZENOH_BSD) + setsockopt(sock->_fd, SOL_SOCKET, SO_NOSIGPIPE, (void *)0, sizeof(int)); +#endif + if (ret != _Z_RES_OK) { + close(sock->_fd); + return ret; + } + struct addrinfo *it = NULL; + for (it = lep._iptcp; it != NULL; it = it->ai_next) { + if (bind(sock->_fd, it->ai_addr, it->ai_addrlen) < 0) { + if (it->ai_next == NULL) { + ret = _Z_ERR_GENERIC; + break; + } + } + if (listen(sock->_fd, 1) < 0) { + if (it->ai_next == NULL) { + ret = _Z_ERR_GENERIC; + break; + } + } + struct sockaddr naddr; + unsigned int nlen = sizeof(naddr); + int con_socket = accept(sock->_fd, &naddr, &nlen); + if (con_socket < 0) { + if (it->ai_next == NULL) { + ret = _Z_ERR_GENERIC; + break; + } + } else { + sock->_fd = con_socket; + break; + } + } + if (ret != _Z_RES_OK) { + close(sock->_fd); + } return ret; } @@ -535,7 +593,8 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ if (!((a->sin_port == b->sin_port) && (a->sin_addr.s_addr == b->sin_addr.s_addr))) { // If addr is not NULL, it means that the rep was requested by the upper-layers if (addr != NULL) { - *addr = _z_slice_make(sizeof(in_addr_t) + sizeof(in_port_t)); + assert(addr->len >= sizeof(in_addr_t) + sizeof(in_port_t)); + addr->len = sizeof(in_addr_t) + sizeof(in_port_t); (void)memcpy((uint8_t *)addr->start, &b->sin_addr.s_addr, sizeof(in_addr_t)); (void)memcpy((uint8_t *)(addr->start + sizeof(in_addr_t)), &b->sin_port, sizeof(in_port_t)); } @@ -548,7 +607,8 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ (memcmp(a->sin6_addr.s6_addr, b->sin6_addr.s6_addr, sizeof(struct in6_addr)) == 0))) { // If addr is not NULL, it means that the rep was requested by the upper-layers if (addr != NULL) { - *addr = _z_slice_make(sizeof(struct in6_addr) + sizeof(in_port_t)); + assert(addr->len >= sizeof(struct in6_addr) + sizeof(in_port_t)); + addr->len = sizeof(struct in6_addr) + sizeof(in_port_t); (void)memcpy((uint8_t *)addr->start, &b->sin6_addr.s6_addr, sizeof(struct in6_addr)); (void)memcpy((uint8_t *)(addr->start + sizeof(struct in6_addr)), &b->sin6_port, sizeof(in_port_t)); } diff --git a/src/system/windows/network.c b/src/system/windows/network.c index b0d9a6990..c3a8a3f27 100644 --- a/src/system/windows/network.c +++ b/src/system/windows/network.c @@ -548,7 +548,7 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ if (!((a->sin_port == b->sin_port) && (a->sin_addr.s_addr == b->sin_addr.s_addr))) { // If addr is not NULL, it means that the rep was requested by the upper-layers if (addr != NULL) { - *addr = _z_slice_make(sizeof(IN_ADDR) + sizeof(USHORT)); + addr->len = sizeof(IN_ADDR) + sizeof(USHORT); (void)memcpy((uint8_t *)addr->start, &b->sin_addr.s_addr, sizeof(IN_ADDR)); (void)memcpy((uint8_t *)(addr->start + sizeof(IN_ADDR)), &b->sin_port, sizeof(USHORT)); } @@ -561,7 +561,7 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ (memcmp(a->sin6_addr.s6_addr, b->sin6_addr.s6_addr, sizeof(struct in6_addr)) == 0))) { // If addr is not NULL, it means that the rep was requested by the upper-layers if (addr != NULL) { - *addr = _z_slice_make(sizeof(struct in6_addr) + sizeof(USHORT)); + addr->len = sizeof(struct in6_addr) + sizeof(USHORT); (void)memcpy((uint8_t *)addr->start, &b->sin6_addr.s6_addr, sizeof(struct in6_addr)); (void)memcpy((uint8_t *)(addr->start + sizeof(struct in6_addr)), &b->sin6_port, sizeof(USHORT)); } diff --git a/src/system/zephyr/network.c b/src/system/zephyr/network.c index e01b89c82..b3a0505a4 100644 --- a/src/system/zephyr/network.c +++ b/src/system/zephyr/network.c @@ -511,7 +511,7 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ // If addr is not NULL, it means that the raddr was requested by the // upper-layers if (addr != NULL) { - *addr = _z_slice_make(sizeof(uint32_t) + sizeof(uint16_t)); + addr->len = sizeof(uint32_t) + sizeof(uint16_t); (void)memcpy((uint8_t *)addr->start, &b->sin_addr.s_addr, sizeof(uint32_t)); (void)memcpy((uint8_t *)(addr->start + sizeof(uint32_t)), &b->sin_port, sizeof(uint16_t)); } @@ -525,7 +525,7 @@ size_t _z_read_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_ // If addr is not NULL, it means that the raddr was requested by the // upper-layers if (addr != NULL) { - *addr = _z_slice_make((sizeof(uint32_t) * 4UL) + sizeof(uint16_t)); + addr->len = (sizeof(uint32_t) * 4UL) + sizeof(uint16_t); (void)memcpy((uint8_t *)addr->start, &b->sin6_addr.s6_addr, sizeof(uint32_t) * 4UL); (void)memcpy((uint8_t *)(addr->start + (sizeof(uint32_t) * 4UL)), &b->sin6_port, sizeof(uint16_t)); } diff --git a/src/transport/common/rx.c b/src/transport/common/rx.c index 5c5c68073..49a39b6cb 100644 --- a/src/transport/common/rx.c +++ b/src/transport/common/rx.c @@ -72,10 +72,14 @@ z_result_t _z_link_recv_t_msg(_z_transport_message_t *t_msg, const _z_link_t *zl } if (ret == _Z_RES_OK) { _z_transport_message_t l_t_msg; - ret = _z_transport_message_decode(&l_t_msg, &zbf); + _z_arc_slice_svec_t arc_pool = _z_arc_slice_svec_make(1); + _z_network_message_svec_t msg_pool = _z_network_message_svec_make(1); + ret = _z_transport_message_decode(&l_t_msg, &zbf, &arc_pool, &msg_pool); if (ret == _Z_RES_OK) { _z_t_msg_copy(t_msg, &l_t_msg); } + _z_arc_slice_svec_clear(&arc_pool); + _z_network_message_svec_clear(&msg_pool); } _z_zbuf_clear(&zbf); diff --git a/src/transport/common/tx.c b/src/transport/common/tx.c index 6ab0af73e..9479059f3 100644 --- a/src/transport/common/tx.c +++ b/src/transport/common/tx.c @@ -16,15 +16,244 @@ #include "zenoh-pico/api/constants.h" #include "zenoh-pico/protocol/codec/core.h" +#include "zenoh-pico/protocol/codec/network.h" #include "zenoh-pico/protocol/codec/transport.h" #include "zenoh-pico/protocol/definitions/transport.h" -#include "zenoh-pico/transport/multicast/tx.h" #include "zenoh-pico/transport/raweth/tx.h" -#include "zenoh-pico/transport/unicast/tx.h" +#include "zenoh-pico/transport/utils.h" #include "zenoh-pico/utils/endianness.h" #include "zenoh-pico/utils/logging.h" /*------------------ Transmission helper ------------------*/ + +static _z_zint_t _z_transport_tx_get_sn(_z_transport_common_t *ztc, z_reliability_t reliability) { + _z_zint_t sn; + if (reliability == Z_RELIABILITY_RELIABLE) { + sn = ztc->_sn_tx_reliable; + ztc->_sn_tx_reliable = _z_sn_increment(ztc->_sn_res, ztc->_sn_tx_reliable); + } else { + sn = ztc->_sn_tx_best_effort; + ztc->_sn_tx_best_effort = _z_sn_increment(ztc->_sn_res, ztc->_sn_tx_best_effort); + } + return sn; +} + +#if Z_FEATURE_FRAGMENTATION == 1 +static z_result_t _z_transport_tx_send_fragment_inner(_z_transport_common_t *ztc, _z_wbuf_t *frag_buff, + const _z_network_message_t *n_msg, z_reliability_t reliability, + _z_zint_t first_sn) { + bool is_first = true; + _z_zint_t sn = first_sn; + // Encode message on temp buffer + _Z_RETURN_IF_ERR(_z_network_message_encode(frag_buff, n_msg)); + // Fragment message + while (_z_wbuf_len(frag_buff) > 0) { + // Get fragment sequence number + if (!is_first) { + sn = _z_transport_tx_get_sn(ztc, reliability); + } + // Serialize fragment + __unsafe_z_prepare_wbuf(&ztc->_wbuf, ztc->_link._cap._flow); + z_result_t ret = __unsafe_z_serialize_zenoh_fragment(&ztc->_wbuf, frag_buff, reliability, sn, is_first); + if (ret != _Z_RES_OK) { + _Z_ERROR("Fragment serialization failed with err %d", ret); + return ret; + } + // Send fragment + __unsafe_z_finalize_wbuf(&ztc->_wbuf, ztc->_link._cap._flow); + _Z_RETURN_IF_ERR(_z_link_send_wbuf(&ztc->_link, &ztc->_wbuf)); + ztc->_transmitted = true; // Tell session we transmitted data + is_first = false; + } + return _Z_RES_OK; +} + +static z_result_t _z_transport_tx_send_fragment(_z_transport_common_t *ztc, const _z_network_message_t *n_msg, + z_reliability_t reliability, _z_zint_t first_sn) { + // Create an expandable wbuf for fragmentation + _z_wbuf_t frag_buff = _z_wbuf_make(_Z_FRAG_BUFF_BASE_SIZE, true); + // Send message as fragments + z_result_t ret = _z_transport_tx_send_fragment_inner(ztc, &frag_buff, n_msg, reliability, first_sn); + // Clear the buffer as it's no longer required + _z_wbuf_clear(&frag_buff); + return ret; +} + +#else +static z_result_t _z_transport_tx_send_fragment(_z_transport_common_t *ztc, const _z_network_message_t *n_msg, + z_reliability_t reliability, _z_zint_t first_sn) { + _ZP_UNUSED(ztc); + _ZP_UNUSED(fbf); + _ZP_UNUSED(n_msg); + _ZP_UNUSED(reliability); + _ZP_UNUSED(first_sn); + _Z_INFO("Sending the message required fragmentation feature that is deactivated."); + return _Z_RES_OK; +} +#endif + +static inline bool _z_transport_tx_batch_has_data(_z_transport_common_t *ztc) { +#if Z_FEATURE_BATCHING == 1 + return (ztc->_batch_state == _Z_BATCHING_ACTIVE) && (ztc->_batch_count > 0); +#else + _ZP_UNUSED(ztc); + return false; +#endif +} + +static z_result_t _z_transport_tx_flush_buffer(_z_transport_common_t *ztc) { + // Send network message + __unsafe_z_finalize_wbuf(&ztc->_wbuf, ztc->_link._cap._flow); + _Z_RETURN_IF_ERR(_z_link_send_wbuf(&ztc->_link, &ztc->_wbuf)); + ztc->_transmitted = true; // Tell session we transmitted data +#if Z_FEATURE_BATCHING == 1 + ztc->_batch_count = 0; +#endif + return _Z_RES_OK; +} + +static z_result_t _z_transport_tx_flush_or_incr_batch(_z_transport_common_t *ztc) { +#if Z_FEATURE_BATCHING == 1 + if (ztc->_batch_state == _Z_BATCHING_ACTIVE) { + // Increment batch count + ztc->_batch_count++; + return _Z_RES_OK; + } else { + return _z_transport_tx_flush_buffer(ztc); + } +#else + return _z_transport_tx_flush_buffer(ztc); +#endif +} + +static z_result_t _z_transport_tx_batch_overflow(_z_transport_common_t *ztc, const _z_network_message_t *n_msg, + z_reliability_t reliability, _z_zint_t sn, size_t prev_wpos) { +#if Z_FEATURE_BATCHING == 1 + // Remove partially encoded data + _z_wbuf_set_wpos(&ztc->_wbuf, prev_wpos); + // Send batch + _Z_RETURN_IF_ERR(_z_transport_tx_flush_buffer(ztc)); + // Init buffer + __unsafe_z_prepare_wbuf(&ztc->_wbuf, ztc->_link._cap._flow); + sn = _z_transport_tx_get_sn(ztc, reliability); + _z_transport_message_t t_msg = _z_t_msg_make_frame_header(sn, reliability); + _Z_RETURN_IF_ERR(_z_transport_message_encode(&ztc->_wbuf, &t_msg)); + // Retry encode + z_result_t ret = _z_network_message_encode(&ztc->_wbuf, n_msg); + if (ret != _Z_RES_OK) { + // Message still doesn't fit in buffer, send as fragments + return _z_transport_tx_send_fragment(ztc, n_msg, reliability, sn); + } else { + // Increment batch + ztc->_batch_count++; + } + return _Z_RES_OK; +#else + _ZP_UNUSED(ztc); + _ZP_UNUSED(n_msg); + _ZP_UNUSED(reliability); + _ZP_UNUSED(sn); + _ZP_UNUSED(prev_wpos); + return _Z_RES_OK; +#endif +} + +static size_t _z_transport_tx_save_wpos(_z_wbuf_t *wbuf) { +#if Z_FEATURE_BATCHING == 1 + return _z_wbuf_get_wpos(wbuf); +#else + _ZP_UNUSED(wbuf); + return 0; +#endif +} + +static z_result_t _z_transport_tx_send_n_msg_inner(_z_transport_common_t *ztc, const _z_network_message_t *n_msg, + z_reliability_t reliability) { + // Init buffer + _z_zint_t sn = 0; + bool batch_has_data = _z_transport_tx_batch_has_data(ztc); + if (!batch_has_data) { + __unsafe_z_prepare_wbuf(&ztc->_wbuf, ztc->_link._cap._flow); + sn = _z_transport_tx_get_sn(ztc, reliability); + _z_transport_message_t t_msg = _z_t_msg_make_frame_header(sn, reliability); + _Z_RETURN_IF_ERR(_z_transport_message_encode(&ztc->_wbuf, &t_msg)); + } + // Try encoding the network message + size_t prev_wpos = _z_transport_tx_save_wpos(&ztc->_wbuf); + z_result_t ret = _z_network_message_encode(&ztc->_wbuf, n_msg); + if (ret == _Z_RES_OK) { + // Flush buffer or increase batch + return _z_transport_tx_flush_or_incr_batch(ztc); + } else if (!batch_has_data) { + // Message doesn't fit in buffer, send as fragments + return _z_transport_tx_send_fragment(ztc, n_msg, reliability, sn); + } else { + // Buffer is too full for message + return _z_transport_tx_batch_overflow(ztc, n_msg, reliability, sn, prev_wpos); + } +} + +z_result_t _z_transport_tx_send_t_msg(_z_transport_common_t *ztc, const _z_transport_message_t *t_msg) { + z_result_t ret = _Z_RES_OK; + _Z_DEBUG("Send session message"); + _z_transport_tx_mutex_lock(ztc, true); + + // Encode transport message + __unsafe_z_prepare_wbuf(&ztc->_wbuf, ztc->_link._cap._flow); + ret = _z_transport_message_encode(&ztc->_wbuf, t_msg); + if (ret == _Z_RES_OK) { + // Send message + __unsafe_z_finalize_wbuf(&ztc->_wbuf, ztc->_link._cap._flow); + ret = _z_link_send_wbuf(&ztc->_link, &ztc->_wbuf); + if (ret == _Z_RES_OK) { + ztc->_transmitted = true; // Tell session we transmitted data + } + } + _z_transport_tx_mutex_unlock(ztc); + return ret; +} + +static z_result_t _z_transport_tx_send_n_msg(_z_transport_common_t *ztc, const _z_network_message_t *n_msg, + z_reliability_t reliability, z_congestion_control_t cong_ctrl) { + z_result_t ret = _Z_RES_OK; + _Z_DEBUG("Send network message"); + + // Acquire the lock and drop the message if needed + ret = _z_transport_tx_mutex_lock(ztc, cong_ctrl == Z_CONGESTION_CONTROL_BLOCK); + if (ret != _Z_RES_OK) { + _Z_INFO("Dropping zenoh message because of congestion control"); + return ret; + } + // Process message + ret = _z_transport_tx_send_n_msg_inner(ztc, n_msg, reliability); + _z_transport_tx_mutex_unlock(ztc); + return ret; +} + +static z_result_t _z_transport_tx_send_n_batch(_z_transport_common_t *ztc, z_congestion_control_t cong_ctrl) { +#if Z_FEATURE_BATCHING == 1 + // Check batch size + if (ztc->_batch_count > 0) { + // Acquire the lock and drop the message if needed + z_result_t ret = _z_transport_tx_mutex_lock(ztc, cong_ctrl == Z_CONGESTION_CONTROL_BLOCK); + if (ret != _Z_RES_OK) { + _Z_INFO("Dropping zenoh batch because of congestion control"); + return ret; + } + // Send batch + _Z_DEBUG("Send network batch"); + ret = _z_transport_tx_flush_buffer(ztc); + _z_transport_tx_mutex_unlock(ztc); + return ret; + } + return _Z_RES_OK; +#else + _ZP_UNUSED(ztc); + _ZP_UNUSED(cong_ctrl); + return _Z_RES_OK; +#endif +} + /** * This function is unsafe because it operates in potentially concurrent data. * Make sure that the following mutexes are locked before calling this function: @@ -74,13 +303,13 @@ z_result_t _z_send_t_msg(_z_transport_t *zt, const _z_transport_message_t *t_msg z_result_t ret = _Z_RES_OK; switch (zt->_type) { case _Z_TRANSPORT_UNICAST_TYPE: - ret = _z_unicast_send_t_msg(&zt->_transport._unicast, t_msg); + ret = _z_transport_tx_send_t_msg(&zt->_transport._unicast._common, t_msg); break; case _Z_TRANSPORT_MULTICAST_TYPE: - ret = _z_multicast_send_t_msg(&zt->_transport._multicast, t_msg); + ret = _z_transport_tx_send_t_msg(&zt->_transport._multicast._common, t_msg); break; case _Z_TRANSPORT_RAWETH_TYPE: - ret = _z_raweth_send_t_msg(&zt->_transport._raweth, t_msg); + ret = _z_raweth_send_t_msg(&zt->_transport._raweth._common, t_msg); break; default: ret = _Z_ERR_TRANSPORT_NOT_AVAILABLE; @@ -165,3 +394,45 @@ z_result_t __unsafe_z_serialize_zenoh_fragment(_z_wbuf_t *dst, _z_wbuf_t *src, z return ret; } + +z_result_t _z_send_n_msg(_z_session_t *zn, const _z_network_message_t *z_msg, z_reliability_t reliability, + z_congestion_control_t cong_ctrl) { + z_result_t ret = _Z_RES_OK; + // Call transport function + switch (zn->_tp._type) { + case _Z_TRANSPORT_UNICAST_TYPE: + ret = _z_transport_tx_send_n_msg(&zn->_tp._transport._unicast._common, z_msg, reliability, cong_ctrl); + break; + case _Z_TRANSPORT_MULTICAST_TYPE: + ret = _z_transport_tx_send_n_msg(&zn->_tp._transport._multicast._common, z_msg, reliability, cong_ctrl); + break; + case _Z_TRANSPORT_RAWETH_TYPE: + ret = _z_raweth_send_n_msg(zn, z_msg, reliability, cong_ctrl); + break; + default: + ret = _Z_ERR_TRANSPORT_NOT_AVAILABLE; + break; + } + return ret; +} + +z_result_t _z_send_n_batch(_z_session_t *zn, z_congestion_control_t cong_ctrl) { + z_result_t ret = _Z_RES_OK; + // Call transport function + switch (zn->_tp._type) { + case _Z_TRANSPORT_UNICAST_TYPE: + ret = _z_transport_tx_send_n_batch(&zn->_tp._transport._unicast._common, cong_ctrl); + break; + case _Z_TRANSPORT_MULTICAST_TYPE: + ret = _z_transport_tx_send_n_batch(&zn->_tp._transport._multicast._common, cong_ctrl); + break; + case _Z_TRANSPORT_RAWETH_TYPE: + _Z_INFO("Batching not yet supported on raweth transport"); + ret = _Z_ERR_TRANSPORT_TX_FAILED; + break; + default: + ret = _Z_ERR_TRANSPORT_NOT_AVAILABLE; + break; + } + return ret; +} diff --git a/src/transport/manager.c b/src/transport/manager.c index 360f5329d..6b2447eea 100644 --- a/src/transport/manager.c +++ b/src/transport/manager.c @@ -20,7 +20,7 @@ #include "zenoh-pico/transport/multicast/transport.h" #include "zenoh-pico/transport/unicast/transport.h" -z_result_t _z_new_transport_client(_z_transport_t *zt, _z_string_t *locator, _z_id_t *local_zid) { +static z_result_t _z_new_transport_client(_z_transport_t *zt, _z_string_t *locator, _z_id_t *local_zid) { z_result_t ret = _Z_RES_OK; // Init link _z_link_t zl; @@ -62,20 +62,24 @@ z_result_t _z_new_transport_client(_z_transport_t *zt, _z_string_t *locator, _z_ return ret; } -z_result_t _z_new_transport_peer(_z_transport_t *zt, _z_string_t *locator, _z_id_t *local_zid) { +static z_result_t _z_new_transport_peer(_z_transport_t *zt, _z_string_t *locator, _z_id_t *local_zid, int peer_op) { z_result_t ret = _Z_RES_OK; // Init link _z_link_t zl; memset(&zl, 0, sizeof(_z_link_t)); // Listen link - ret = _z_listen_link(&zl, locator); + if (peer_op == _Z_PEER_OP_OPEN) { + ret = _z_open_link(&zl, locator); + } else { + ret = _z_listen_link(&zl, locator); + } if (ret != _Z_RES_OK) { return ret; } switch (zl._cap._transport) { case Z_LINK_CAP_TRANSPORT_UNICAST: { _z_transport_unicast_establish_param_t tp_param; - ret = _z_unicast_open_peer(&tp_param, &zl, local_zid); + ret = _z_unicast_open_peer(&tp_param, &zl, local_zid, peer_op); if (ret != _Z_RES_OK) { _z_link_clear(&zl); return ret; @@ -101,13 +105,13 @@ z_result_t _z_new_transport_peer(_z_transport_t *zt, _z_string_t *locator, _z_id return ret; } -z_result_t _z_new_transport(_z_transport_t *zt, _z_id_t *bs, _z_string_t *locator, z_whatami_t mode) { +z_result_t _z_new_transport(_z_transport_t *zt, _z_id_t *bs, _z_string_t *locator, z_whatami_t mode, int peer_op) { z_result_t ret; if (mode == Z_WHATAMI_CLIENT) { ret = _z_new_transport_client(zt, locator, bs); } else { - ret = _z_new_transport_peer(zt, locator, bs); + ret = _z_new_transport_peer(zt, locator, bs, peer_op); } return ret; diff --git a/src/transport/multicast.c b/src/transport/multicast.c index 6c8db89d0..eae7f7bc7 100644 --- a/src/transport/multicast.c +++ b/src/transport/multicast.c @@ -19,6 +19,14 @@ #include #include +#include "zenoh-pico/link/link.h" +#include "zenoh-pico/transport/common/lease.h" +#include "zenoh-pico/transport/common/read.h" +#include "zenoh-pico/transport/common/tx.h" +#include "zenoh-pico/transport/multicast/rx.h" +#include "zenoh-pico/transport/unicast/rx.h" +#include "zenoh-pico/transport/utils.h" +#include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/uuid.h" #if Z_FEATURE_MULTICAST_TRANSPORT == 1 diff --git a/src/transport/multicast/lease.c b/src/transport/multicast/lease.c index 50c299f3a..b8f98cd19 100644 --- a/src/transport/multicast/lease.c +++ b/src/transport/multicast/lease.c @@ -17,6 +17,7 @@ #include #include "zenoh-pico/config.h" +#include "zenoh-pico/session/query.h" #include "zenoh-pico/session/utils.h" #include "zenoh-pico/transport/multicast/lease.h" #include "zenoh-pico/utils/logging.h" @@ -26,18 +27,18 @@ z_result_t _zp_multicast_send_join(_z_transport_multicast_t *ztm) { _z_conduit_sn_list_t next_sn; next_sn._is_qos = false; - next_sn._val._plain._best_effort = ztm->_sn_tx_best_effort; - next_sn._val._plain._reliable = ztm->_sn_tx_reliable; + next_sn._val._plain._best_effort = ztm->_common._sn_tx_best_effort; + next_sn._val._plain._reliable = ztm->_common._sn_tx_reliable; - _z_id_t zid = _Z_RC_IN_VAL(ztm->_session)->_local_zid; + _z_id_t zid = _Z_RC_IN_VAL(ztm->_common._session)->_local_zid; _z_transport_message_t jsm = _z_t_msg_make_join(Z_WHATAMI_PEER, Z_TRANSPORT_LEASE, zid, next_sn); - return ztm->_send_f(ztm, &jsm); + return ztm->_send_f(&ztm->_common, &jsm); } z_result_t _zp_multicast_send_keep_alive(_z_transport_multicast_t *ztm) { _z_transport_message_t t_msg = _z_t_msg_make_keep_alive(); - return ztm->_send_f(ztm, &t_msg); + return ztm->_send_f(&ztm->_common, &t_msg); } #else @@ -90,15 +91,15 @@ static _z_zint_t _z_get_next_lease(_z_transport_peer_entry_list_t *peers) { void *_zp_multicast_lease_task(void *ztm_arg) { _z_transport_multicast_t *ztm = (_z_transport_multicast_t *)ztm_arg; - ztm->_transmitted = false; + ztm->_common._transmitted = false; // From all peers, get the next lease time (minimum) - int next_lease = (int)_z_get_minimum_lease(ztm->_peers, ztm->_lease); + int next_lease = (int)_z_get_minimum_lease(ztm->_peers, ztm->_common._lease); int next_keep_alive = (int)(next_lease / Z_TRANSPORT_LEASE_EXPIRE_FACTOR); int next_join = Z_JOIN_INTERVAL; _z_transport_peer_entry_list_t *it = NULL; - while (ztm->_lease_task_running == true) { + while (ztm->_common._lease_task_running == true) { _z_mutex_lock(&ztm->_mutex_peer); if (next_lease <= 0) { @@ -121,7 +122,7 @@ void *_zp_multicast_lease_task(void *ztm_arg) { if (next_join <= 0) { _zp_multicast_send_join(ztm); - ztm->_transmitted = true; + ztm->_common._transmitted = true; // Reset the join parameters next_join = Z_JOIN_INTERVAL; @@ -129,16 +130,18 @@ void *_zp_multicast_lease_task(void *ztm_arg) { if (next_keep_alive <= 0) { // Check if need to send a keep alive - if (ztm->_transmitted == false) { + if (ztm->_common._transmitted == false) { if (_zp_multicast_send_keep_alive(ztm) < 0) { - // TODO: Handle retransmission or error + _Z_INFO("Send keep alive failed."); } } - // Reset the keep alive parameters - ztm->_transmitted = false; - next_keep_alive = (int)(_z_get_minimum_lease(ztm->_peers, ztm->_lease) / Z_TRANSPORT_LEASE_EXPIRE_FACTOR); + ztm->_common._transmitted = false; + next_keep_alive = + (int)(_z_get_minimum_lease(ztm->_peers, ztm->_common._lease) / Z_TRANSPORT_LEASE_EXPIRE_FACTOR); } + // Query timeout process + _z_pending_query_process_timeout(_Z_RC_IN_VAL(ztm->_common._session)); // Compute the target interval to sleep int interval; @@ -189,19 +192,19 @@ void *_zp_multicast_lease_task(void *ztm_arg) { z_result_t _zp_multicast_start_lease_task(_z_transport_multicast_t *ztm, z_task_attr_t *attr, _z_task_t *task) { // Init memory (void)memset(task, 0, sizeof(_z_task_t)); - ztm->_lease_task_running = true; // Init before z_task_init for concurrency issue + ztm->_common._lease_task_running = true; // Init before z_task_init for concurrency issue // Init task if (_z_task_init(task, attr, _zp_multicast_lease_task, ztm) != _Z_RES_OK) { - ztm->_lease_task_running = false; + ztm->_common._lease_task_running = false; return _Z_ERR_SYSTEM_TASK_FAILED; } // Attach task - ztm->_lease_task = task; + ztm->_common._lease_task = task; return _Z_RES_OK; } z_result_t _zp_multicast_stop_lease_task(_z_transport_multicast_t *ztm) { - ztm->_lease_task_running = false; + ztm->_common._lease_task_running = false; return _Z_RES_OK; } #else diff --git a/src/transport/multicast/read.c b/src/transport/multicast/read.c index 965f11a32..bea0b82f3 100644 --- a/src/transport/multicast/read.c +++ b/src/transport/multicast/read.c @@ -26,17 +26,23 @@ #if Z_FEATURE_MULTICAST_TRANSPORT == 1 +#define _Z_MULTICAST_ADDR_BUFF_SIZE 32 // Arbitrary size that must be able to contain any link address. + z_result_t _zp_multicast_read(_z_transport_multicast_t *ztm) { z_result_t ret = _Z_RES_OK; - _z_slice_t addr; + static uint8_t addr_buff[_Z_MULTICAST_ADDR_BUFF_SIZE] = {0}; + _z_slice_t addr = _z_slice_alias_buf(addr_buff, sizeof(addr_buff)); _z_transport_message_t t_msg; ret = _z_multicast_recv_t_msg(ztm, &t_msg, &addr); if (ret == _Z_RES_OK) { ret = _z_multicast_handle_transport_message(ztm, &t_msg, &addr); _z_t_msg_clear(&t_msg); } - + ret = _z_multicast_update_rx_buffer(ztm); + if (ret != _Z_RES_OK) { + _Z_ERROR("Failed to allocate rx buffer"); + } return ret; } #else @@ -52,41 +58,42 @@ void *_zp_multicast_read_task(void *ztm_arg) { _z_transport_multicast_t *ztm = (_z_transport_multicast_t *)ztm_arg; // Acquire and keep the lock - _z_mutex_lock(&ztm->_mutex_rx); + _z_mutex_lock(&ztm->_common._mutex_rx); // Prepare the buffer - _z_zbuf_reset(&ztm->_zbuf); + _z_zbuf_reset(&ztm->_common._zbuf); - _z_slice_t addr = _z_slice_alias_buf(NULL, 0); - while (ztm->_read_task_running == true) { - // Read bytes from socket to the main buffer + uint8_t addr_buff[_Z_MULTICAST_ADDR_BUFF_SIZE] = {0}; + _z_slice_t addr = _z_slice_alias_buf(addr_buff, sizeof(addr_buff)); + while (ztm->_common._read_task_running == true) { size_t to_read = 0; - switch (ztm->_link._cap._flow) { + // Read bytes from socket to the main buffer + switch (ztm->_common._link._cap._flow) { case Z_LINK_CAP_FLOW_STREAM: - if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, &addr); - if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_slice_clear(&addr); - _z_zbuf_compact(&ztm->_zbuf); + if (_z_zbuf_len(&ztm->_common._zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_link_recv_zbuf(&ztm->_common._link, &ztm->_common._zbuf, &addr); + if (_z_zbuf_len(&ztm->_common._zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_zbuf_compact(&ztm->_common._zbuf); continue; } } // Get stream size - to_read = _z_read_stream_size(&ztm->_zbuf); + to_read = _z_read_stream_size(&ztm->_common._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) { - _z_zbuf_set_rpos(&ztm->_zbuf, _z_zbuf_get_rpos(&ztm->_zbuf) - _Z_MSG_LEN_ENC_SIZE); - _z_zbuf_compact(&ztm->_zbuf); + if (_z_zbuf_len(&ztm->_common._zbuf) < to_read) { + _z_link_recv_zbuf(&ztm->_common._link, &ztm->_common._zbuf, NULL); + if (_z_zbuf_len(&ztm->_common._zbuf) < to_read) { + _z_zbuf_set_rpos(&ztm->_common._zbuf, + _z_zbuf_get_rpos(&ztm->_common._zbuf) - _Z_MSG_LEN_ENC_SIZE); + _z_zbuf_compact(&ztm->_common._zbuf); continue; } } break; case Z_LINK_CAP_FLOW_DATAGRAM: - _z_zbuf_compact(&ztm->_zbuf); - to_read = _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, &addr); + _z_zbuf_compact(&ztm->_common._zbuf); + to_read = _z_link_recv_zbuf(&ztm->_common._link, &ztm->_common._zbuf, &addr); if (to_read == SIZE_MAX) { continue; } @@ -94,55 +101,56 @@ void *_zp_multicast_read_task(void *ztm_arg) { default: break; } - // Wrap the main buffer for to_read bytes - _z_zbuf_t zbuf = _z_zbuf_view(&ztm->_zbuf, to_read); + // Wrap the main buffer to_read bytes + _z_zbuf_t zbuf = _z_zbuf_view(&ztm->_common._zbuf, to_read); while (_z_zbuf_len(&zbuf) > 0) { - z_result_t ret = _Z_RES_OK; - // Decode one session message _z_transport_message_t t_msg; - ret = _z_transport_message_decode(&t_msg, &zbuf); + z_result_t ret = + _z_transport_message_decode(&t_msg, &zbuf, &ztm->_common._arc_pool, &ztm->_common._msg_pool); if (ret == _Z_RES_OK) { ret = _z_multicast_handle_transport_message(ztm, &t_msg, &addr); if (ret == _Z_RES_OK) { _z_t_msg_clear(&t_msg); - _z_slice_clear(&addr); } else { - ztm->_read_task_running = false; + _Z_ERROR("Dropping message due to processing error: %d", ret); continue; } } else { - _Z_ERROR("Connection closed due to malformed message"); - ztm->_read_task_running = false; + _Z_ERROR("Connection closed due to malformed message: %d", ret); + ztm->_common._read_task_running = false; continue; } } - // Move the read position of the read buffer - _z_zbuf_set_rpos(&ztm->_zbuf, _z_zbuf_get_rpos(&ztm->_zbuf) + to_read); + _z_zbuf_set_rpos(&ztm->_common._zbuf, _z_zbuf_get_rpos(&ztm->_common._zbuf) + to_read); + if (_z_multicast_update_rx_buffer(ztm) != _Z_RES_OK) { + _Z_ERROR("Connection closed due to lack of memory to allocate rx buffer"); + ztm->_common._read_task_running = false; + } } - _z_mutex_unlock(&ztm->_mutex_rx); + _z_mutex_unlock(&ztm->_common._mutex_rx); return NULL; } z_result_t _zp_multicast_start_read_task(_z_transport_t *zt, z_task_attr_t *attr, _z_task_t *task) { // Init memory (void)memset(task, 0, sizeof(_z_task_t)); - zt->_transport._multicast._read_task_running = true; // Init before z_task_init for concurrency issue + zt->_transport._multicast._common._read_task_running = true; // Init before z_task_init for concurrency issue // Init task if (_z_task_init(task, attr, _zp_multicast_read_task, &zt->_transport._multicast) != _Z_RES_OK) { - zt->_transport._multicast._read_task_running = false; + zt->_transport._multicast._common._read_task_running = false; return _Z_ERR_SYSTEM_TASK_FAILED; } // Attach task - zt->_transport._multicast._read_task = task; + zt->_transport._multicast._common._read_task = task; return _Z_RES_OK; } z_result_t _zp_multicast_stop_read_task(_z_transport_t *zt) { - zt->_transport._multicast._read_task_running = false; + zt->_transport._multicast._common._read_task_running = false; return _Z_RES_OK; } #else diff --git a/src/transport/multicast/rx.c b/src/transport/multicast/rx.c index 40e9f0b02..fb0205b0f 100644 --- a/src/transport/multicast/rx.c +++ b/src/transport/multicast/rx.c @@ -25,6 +25,7 @@ #include "zenoh-pico/protocol/iobuf.h" #include "zenoh-pico/session/utils.h" #include "zenoh-pico/transport/multicast/rx.h" +#include "zenoh-pico/transport/multicast/transport.h" #include "zenoh-pico/transport/utils.h" #include "zenoh-pico/utils/logging.h" @@ -34,31 +35,28 @@ static z_result_t _z_multicast_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_t _Z_DEBUG(">> recv session msg"); z_result_t ret = _Z_RES_OK; -#if Z_FEATURE_MULTI_THREAD == 1 - // Acquire the lock - _z_mutex_lock(&ztm->_mutex_rx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - + _z_transport_rx_mutex_lock(&ztm->_common); size_t to_read = 0; do { - switch (ztm->_link._cap._flow) { + switch (ztm->_common._link._cap._flow) { case Z_LINK_CAP_FLOW_STREAM: - if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); - if (_z_zbuf_len(&ztm->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_zbuf_compact(&ztm->_zbuf); + if (_z_zbuf_len(&ztm->_common._zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_link_recv_zbuf(&ztm->_common._link, &ztm->_common._zbuf, addr); + if (_z_zbuf_len(&ztm->_common._zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_zbuf_compact(&ztm->_common._zbuf); ret = _Z_ERR_TRANSPORT_NOT_ENOUGH_BYTES; break; } } // Get stream size - to_read = _z_read_stream_size(&ztm->_zbuf); + to_read = _z_read_stream_size(&ztm->_common._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) { - _z_zbuf_set_rpos(&ztm->_zbuf, _z_zbuf_get_rpos(&ztm->_zbuf) - _Z_MSG_LEN_ENC_SIZE); - _z_zbuf_compact(&ztm->_zbuf); + if (_z_zbuf_len(&ztm->_common._zbuf) < to_read) { + _z_link_recv_zbuf(&ztm->_common._link, &ztm->_common._zbuf, addr); + if (_z_zbuf_len(&ztm->_common._zbuf) < to_read) { + _z_zbuf_set_rpos(&ztm->_common._zbuf, + _z_zbuf_get_rpos(&ztm->_common._zbuf) - _Z_MSG_LEN_ENC_SIZE); + _z_zbuf_compact(&ztm->_common._zbuf); ret = _Z_ERR_TRANSPORT_NOT_ENOUGH_BYTES; break; } @@ -66,8 +64,8 @@ static z_result_t _z_multicast_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_t break; // Datagram capable links case Z_LINK_CAP_FLOW_DATAGRAM: - _z_zbuf_compact(&ztm->_zbuf); - to_read = _z_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); + _z_zbuf_compact(&ztm->_common._zbuf); + to_read = _z_link_recv_zbuf(&ztm->_common._link, &ztm->_common._zbuf, addr); if (to_read == SIZE_MAX) { ret = _Z_ERR_TRANSPORT_RX_FAILED; } @@ -78,14 +76,10 @@ static z_result_t _z_multicast_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_t } while (false); // The 1-iteration loop to use continue to break the entire loop on error if (ret == _Z_RES_OK) { - _Z_DEBUG(">> \t transport_message_decode: %ju", (uintmax_t)_z_zbuf_len(&ztm->_zbuf)); - ret = _z_transport_message_decode(t_msg, &ztm->_zbuf); + _Z_DEBUG(">> \t transport_message_decode: %ju", (uintmax_t)_z_zbuf_len(&ztm->_common._zbuf)); + ret = _z_transport_message_decode(t_msg, &ztm->_common._zbuf, &ztm->_common._arc_pool, &ztm->_common._msg_pool); } - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztm->_mutex_rx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - + _z_transport_rx_mutex_unlock(&ztm->_common); return ret; } @@ -124,23 +118,22 @@ static _z_transport_peer_entry_t *_z_find_peer_entry(_z_transport_peer_entry_lis z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_slice_t *addr) { z_result_t ret = _Z_RES_OK; -#if Z_FEATURE_MULTI_THREAD == 1 - // Acquire and keep the lock - _z_mutex_lock(&ztm->_mutex_peer); -#endif // Z_FEATURE_MULTI_THREAD == 1 - + _z_multicast_peer_mutex_lock(ztm); // Mark the session that we have received data from this peer _z_transport_peer_entry_t *entry = _z_find_peer_entry(ztm->_peers, addr); switch (_Z_MID(t_msg->_header)) { case _Z_MID_T_FRAME: { - _Z_INFO("Received _Z_FRAME message"); + _Z_DEBUG("Received _Z_FRAME message"); if (entry == NULL) { + _Z_INFO("Dropping _Z_FRAME from unknown peer"); break; } + // Note that we receive data from peer entry->_received = true; - + z_reliability_t tmsg_reliability; // Check if the SN is correct if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAME_R) == true) { + tmsg_reliability = Z_RELIABILITY_RELIABLE; // @TODO: amend once reliability is in place. For the time being only // monotonic SNs are ensured if (_z_sn_precedes(entry->_sn_res, entry->_sn_rx_sns._val._plain._reliable, t_msg->_body._frame._sn) == @@ -148,17 +141,20 @@ z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, entry->_sn_rx_sns._val._plain._reliable = t_msg->_body._frame._sn; } else { #if Z_FEATURE_FRAGMENTATION == 1 + entry->_state_reliable = _Z_DBUF_STATE_NULL; _z_wbuf_clear(&entry->_dbuf_reliable); #endif _Z_INFO("Reliable message dropped because it is out of order"); break; } } else { + tmsg_reliability = Z_RELIABILITY_BEST_EFFORT; if (_z_sn_precedes(entry->_sn_res, entry->_sn_rx_sns._val._plain._best_effort, t_msg->_body._frame._sn) == true) { entry->_sn_rx_sns._val._plain._best_effort = t_msg->_body._frame._sn; } else { #if Z_FEATURE_FRAGMENTATION == 1 + entry->_state_best_effort = _Z_DBUF_STATE_NULL; _z_wbuf_clear(&entry->_dbuf_best_effort); #endif _Z_INFO("Best effort message dropped because it is out of order"); @@ -168,30 +164,36 @@ z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, // Handle all the zenoh message, one by one uint16_t mapping = entry->_peer_id; - size_t len = _z_vec_len(&t_msg->_body._frame._messages); + size_t len = _z_svec_len(&t_msg->_body._frame._messages); for (size_t i = 0; i < len; i++) { - _z_network_message_t *zm = _z_network_message_vec_get(&t_msg->_body._frame._messages, i); - zm->_reliability = _z_t_msg_get_reliability(t_msg); + _z_network_message_t *zm = _z_network_message_svec_get(&t_msg->_body._frame._messages, i); + zm->_reliability = tmsg_reliability; _z_msg_fix_mapping(zm, mapping); - _z_handle_network_message(ztm->_session, zm, mapping); + _z_handle_network_message(ztm->_common._session, zm, mapping); } break; } case _Z_MID_T_FRAGMENT: { - _Z_INFO("Received Z_FRAGMENT message"); + _Z_DEBUG("Received Z_FRAGMENT message"); #if Z_FEATURE_FRAGMENTATION == 1 if (entry == NULL) { + _Z_INFO("Dropping Z_FRAGMENT from unknown peer"); break; } + // Note that we receive data from the peer entry->_received = true; - bool consecutive; _z_wbuf_t *dbuf; - // Check if the SN is correct and select the right defragmentation buffer + uint8_t *dbuf_state; + z_reliability_t tmsg_reliability; + bool consecutive; + // Select the right defragmentation buffer if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAME_R)) { + tmsg_reliability = Z_RELIABILITY_RELIABLE; + // Check SN // @TODO: amend once reliability is in place. For the time being only // monotonic SNs are ensured if (_z_sn_precedes(entry->_sn_res, entry->_sn_rx_sns._val._plain._reliable, @@ -200,27 +202,34 @@ z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, t_msg->_body._fragment._sn); entry->_sn_rx_sns._val._plain._reliable = t_msg->_body._fragment._sn; dbuf = &entry->_dbuf_reliable; + dbuf_state = &entry->_state_reliable; } else { _z_wbuf_clear(&entry->_dbuf_reliable); + entry->_state_reliable = _Z_DBUF_STATE_NULL; _Z_INFO("Reliable message dropped because it is out of order"); break; } } else { + tmsg_reliability = Z_RELIABILITY_BEST_EFFORT; + // Check SN if (_z_sn_precedes(entry->_sn_res, entry->_sn_rx_sns._val._plain._best_effort, t_msg->_body._fragment._sn)) { consecutive = _z_sn_consecutive(entry->_sn_res, entry->_sn_rx_sns._val._plain._best_effort, t_msg->_body._fragment._sn); entry->_sn_rx_sns._val._plain._best_effort = t_msg->_body._fragment._sn; dbuf = &entry->_dbuf_best_effort; + dbuf_state = &entry->_state_best_effort; } else { _z_wbuf_clear(&entry->_dbuf_best_effort); + entry->_state_best_effort = _Z_DBUF_STATE_NULL; _Z_INFO("Best effort message dropped because it is out of order"); break; } } - if (!consecutive && _z_wbuf_len(dbuf) > 0) { - _Z_DEBUG("Non-consecutive fragments received"); - _z_wbuf_reset(dbuf); + if (!consecutive && (_z_wbuf_len(dbuf) > 0)) { + _z_wbuf_clear(dbuf); + *dbuf_state = _Z_DBUF_STATE_NULL; + _Z_INFO("Defragmentation buffer dropped because non-consecutive fragments received"); break; } // Handle fragment markers @@ -228,7 +237,7 @@ z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, if (t_msg->_body._fragment.first) { _z_wbuf_reset(dbuf); } else if (_z_wbuf_len(dbuf) == 0) { - _Z_DEBUG("First fragment received without the first marker"); + _Z_INFO("First fragment received without the first marker"); break; } if (t_msg->_body._fragment.drop) { @@ -236,41 +245,62 @@ z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, break; } } - - bool drop = false; - if ((_z_wbuf_len(dbuf) + t_msg->_body._fragment._payload.len) > Z_FRAG_MAX_SIZE) { - // Filling the wbuf capacity as a way to signaling the last fragment to reset the dbuf - // Otherwise, last (smaller) fragments can be understood as a complete message - _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, _z_wbuf_space_left(dbuf)); - drop = true; - } else { - _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, - t_msg->_body._fragment._payload.len); + // Allocate buffer if needed + if (*dbuf_state == _Z_DBUF_STATE_NULL) { + *dbuf = _z_wbuf_make(Z_FRAG_MAX_SIZE, false); + if (_z_wbuf_capacity(dbuf) != Z_FRAG_MAX_SIZE) { + _Z_ERROR("Not enough memory to allocate peer defragmentation buffer"); + ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; + break; + } + *dbuf_state = _Z_DBUF_STATE_INIT; } - + // Process fragment data + if (*dbuf_state == _Z_DBUF_STATE_INIT) { + // Check overflow + if ((_z_wbuf_len(dbuf) + t_msg->_body._fragment._payload.len) > Z_FRAG_MAX_SIZE) { + *dbuf_state = _Z_DBUF_STATE_OVERFLOW; + } else { + // Fill buffer + _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, + t_msg->_body._fragment._payload.len); + } + } + // Process final fragment if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAGMENT_M) == false) { - if (drop == true) { // Drop message if it exceeds the fragmentation size - _z_wbuf_reset(dbuf); + // Drop message if it exceeds the fragmentation size + if (*dbuf_state == _Z_DBUF_STATE_OVERFLOW) { + _Z_INFO("Fragment dropped because defragmentation buffer has overflown"); + _z_wbuf_clear(dbuf); + *dbuf_state = _Z_DBUF_STATE_NULL; break; } - - _z_zbuf_t zbf = _z_wbuf_to_zbuf(dbuf); // Convert the defragmentation buffer into a decoding buffer - - _z_zenoh_message_t zm; - ret = _z_network_message_decode(&zm, &zbf); - zm._reliability = _z_t_msg_get_reliability(t_msg); + // Convert the defragmentation buffer into a decoding buffer + _z_zbuf_t zbf = _z_wbuf_moved_as_zbuf(dbuf); + if (_z_zbuf_capacity(&zbf) == 0) { + _Z_ERROR("Failed to convert defragmentation buffer into a decoding buffer!"); + _z_wbuf_clear(dbuf); + *dbuf_state = _Z_DBUF_STATE_NULL; + ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; + break; + } + // Decode message + _z_zenoh_message_t zm = {0}; + assert(ztm->_common._arc_pool._capacity >= 1); + _z_arc_slice_t *arcs = _z_arc_slice_svec_get_mut(&ztm->_common._arc_pool, 0); + ret = _z_network_message_decode(&zm, &zbf, arcs); + zm._reliability = tmsg_reliability; if (ret == _Z_RES_OK) { uint16_t mapping = entry->_peer_id; _z_msg_fix_mapping(&zm, mapping); - _z_handle_network_message(ztm->_session, &zm, mapping); - _z_msg_clear(&zm); // Clear must be explicitly called for fragmented zenoh messages. Non-fragmented - // zenoh messages are released when their transport message is released. + _z_handle_network_message(ztm->_common._session, &zm, mapping); + } else { + _Z_INFO("Failed to decode defragmented message"); + ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } - // Free the decoding buffer _z_zbuf_clear(&zbf); - // Reset the defragmentation buffer - _z_wbuf_reset(dbuf); + *dbuf_state = _Z_DBUF_STATE_NULL; } #else _Z_INFO("Fragment dropped because fragmentation feature is deactivated"); @@ -279,7 +309,7 @@ z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, } case _Z_MID_T_KEEP_ALIVE: { - _Z_INFO("Received _Z_KEEP_ALIVE message"); + _Z_DEBUG("Received _Z_KEEP_ALIVE message"); if (entry == NULL) { break; } @@ -299,7 +329,7 @@ z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, } case _Z_MID_T_JOIN: { - _Z_INFO("Received _Z_JOIN message"); + _Z_DEBUG("Received _Z_JOIN message"); if (t_msg->_body._join._version != Z_PROTO_VERSION) { break; } @@ -327,18 +357,10 @@ z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, #if Z_FEATURE_FRAGMENTATION == 1 entry->_patch = t_msg->_body._join._patch < _Z_CURRENT_PATCH ? t_msg->_body._join._patch : _Z_CURRENT_PATCH; -#if Z_FEATURE_DYNAMIC_MEMORY_ALLOCATION == 1 - entry->_dbuf_reliable = _z_wbuf_make(0, true); - entry->_dbuf_best_effort = _z_wbuf_make(0, true); -#else - entry->_dbuf_reliable = _z_wbuf_make(Z_FRAG_MAX_SIZE, false); - entry->_dbuf_best_effort = _z_wbuf_make(Z_FRAG_MAX_SIZE, false); - - if ((_z_wbuf_capacity(&entry->_dbuf_reliable) != Z_FRAG_MAX_SIZE) || - (_z_wbuf_capacity(&entry->_dbuf_best_effort) != Z_FRAG_MAX_SIZE)) { - _Z_ERROR("Not enough memory to allocate peer defragmentation buffers!"); - } -#endif + entry->_state_reliable = _Z_DBUF_STATE_NULL; + entry->_state_best_effort = _Z_DBUF_STATE_NULL; + entry->_dbuf_reliable = _z_wbuf_null(); + entry->_dbuf_best_effort = _z_wbuf_null(); #endif // Update lease time (set as ms during) entry->_lease = t_msg->_body._join._lease; @@ -390,13 +412,30 @@ z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, break; } } - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztm->_mutex_peer); -#endif // Z_FEATURE_MULTI_THREAD == 1 - + _z_multicast_peer_mutex_unlock(ztm); return ret; } + +z_result_t _z_multicast_update_rx_buffer(_z_transport_multicast_t *ztm) { + // Check if user or defragment buffer took ownership of buffer + if (_z_zbuf_get_ref_count(&ztm->_common._zbuf) != 1) { + // Allocate a new buffer + _z_zbuf_t new_zbuf = _z_zbuf_make(Z_BATCH_MULTICAST_SIZE); + if (_z_zbuf_capacity(&new_zbuf) != Z_BATCH_MULTICAST_SIZE) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + // Recopy leftover bytes + size_t leftovers = _z_zbuf_len(&ztm->_common._zbuf); + if (leftovers > 0) { + _z_zbuf_copy_bytes(&new_zbuf, &ztm->_common._zbuf); + } + // Drop buffer & update + _z_zbuf_clear(&ztm->_common._zbuf); + ztm->_common._zbuf = new_zbuf; + } + return _Z_RES_OK; +} + #else z_result_t _z_multicast_handle_transport_message(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_slice_t *addr) { diff --git a/src/transport/multicast/transport.c b/src/transport/multicast/transport.c index 091539fbb..bbe13bf19 100644 --- a/src/transport/multicast/transport.c +++ b/src/transport/multicast/transport.c @@ -11,6 +11,8 @@ // Contributors: // ZettaScale Zenoh Team, +#include "zenoh-pico/transport/multicast/transport.h" + #include #include #include @@ -23,7 +25,6 @@ #include "zenoh-pico/transport/common/tx.h" #include "zenoh-pico/transport/multicast.h" #include "zenoh-pico/transport/multicast/rx.h" -#include "zenoh-pico/transport/multicast/tx.h" #include "zenoh-pico/transport/raweth/tx.h" #include "zenoh-pico/transport/unicast/rx.h" #include "zenoh-pico/transport/utils.h" @@ -40,7 +41,7 @@ z_result_t _z_multicast_transport_create(_z_transport_t *zt, _z_link_t *zl, case Z_LINK_CAP_TRANSPORT_MULTICAST: zt->_type = _Z_TRANSPORT_MULTICAST_TYPE; ztm = &zt->_transport._multicast; - ztm->_send_f = _z_multicast_send_t_msg; + ztm->_send_f = _z_transport_tx_send_t_msg; break; case Z_LINK_CAP_TRANSPORT_RAWETH: zt->_type = _Z_TRANSPORT_RAWETH_TYPE; @@ -50,19 +51,26 @@ z_result_t _z_multicast_transport_create(_z_transport_t *zt, _z_link_t *zl, default: return _Z_ERR_GENERIC; } + +// Initialize batching data +#if Z_FEATURE_BATCHING == 1 + ztm->_common._batch_state = _Z_BATCHING_IDLE; + ztm->_common._batch_count = 0; +#endif + #if Z_FEATURE_MULTI_THREAD == 1 // Initialize the mutexes - ret = _z_mutex_init(&ztm->_mutex_tx); + ret = _z_mutex_init(&ztm->_common._mutex_tx); if (ret == _Z_RES_OK) { - ret = _z_mutex_init(&ztm->_mutex_rx); + ret = _z_mutex_init(&ztm->_common._mutex_rx); if (ret == _Z_RES_OK) { ret = _z_mutex_init(&ztm->_mutex_peer); if (ret != _Z_RES_OK) { - _z_mutex_drop(&ztm->_mutex_tx); - _z_mutex_drop(&ztm->_mutex_rx); + _z_mutex_drop(&ztm->_common._mutex_tx); + _z_mutex_drop(&ztm->_common._mutex_rx); } } else { - _z_mutex_drop(&ztm->_mutex_tx); + _z_mutex_drop(&ztm->_common._mutex_tx); } } #endif // Z_FEATURE_MULTI_THREAD == 1 @@ -70,51 +78,57 @@ z_result_t _z_multicast_transport_create(_z_transport_t *zt, _z_link_t *zl, // Initialize the read and write buffers if (ret == _Z_RES_OK) { uint16_t mtu = (zl->_mtu < Z_BATCH_MULTICAST_SIZE) ? zl->_mtu : Z_BATCH_MULTICAST_SIZE; - ztm->_wbuf = _z_wbuf_make(mtu, false); - ztm->_zbuf = _z_zbuf_make(Z_BATCH_MULTICAST_SIZE); + ztm->_common._wbuf = _z_wbuf_make(mtu, false); + ztm->_common._zbuf = _z_zbuf_make(Z_BATCH_MULTICAST_SIZE); + + // Initialize resource pool + ztm->_common._arc_pool = _z_arc_slice_svec_make(_Z_RES_POOL_INIT_SIZE); + ztm->_common._msg_pool = _z_network_message_svec_make(_Z_RES_POOL_INIT_SIZE); // Clean up the buffers if one of them failed to be allocated - if ((_z_wbuf_capacity(&ztm->_wbuf) != mtu) || (_z_zbuf_capacity(&ztm->_zbuf) != Z_BATCH_MULTICAST_SIZE)) { + if ((ztm->_common._msg_pool._capacity == 0) || (ztm->_common._arc_pool._capacity == 0) || + (_z_wbuf_capacity(&ztm->_common._wbuf) != mtu) || + (_z_zbuf_capacity(&ztm->_common._zbuf) != Z_BATCH_MULTICAST_SIZE)) { ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; _Z_ERROR("Not enough memory to allocate transport tx rx buffers!"); #if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_drop(&ztm->_mutex_tx); - _z_mutex_drop(&ztm->_mutex_rx); + _z_mutex_drop(&ztm->_common._mutex_tx); + _z_mutex_drop(&ztm->_common._mutex_rx); _z_mutex_drop(&ztm->_mutex_peer); #endif // Z_FEATURE_MULTI_THREAD == 1 - _z_wbuf_clear(&ztm->_wbuf); - _z_zbuf_clear(&ztm->_zbuf); + _z_wbuf_clear(&ztm->_common._wbuf); + _z_zbuf_clear(&ztm->_common._zbuf); } } if (ret == _Z_RES_OK) { // Set default SN resolution - ztm->_sn_res = _z_sn_max(param->_seq_num_res); + ztm->_common._sn_res = _z_sn_max(param->_seq_num_res); // The initial SN at TX side - ztm->_sn_tx_reliable = param->_initial_sn_tx._val._plain._reliable; - ztm->_sn_tx_best_effort = param->_initial_sn_tx._val._plain._best_effort; + ztm->_common._sn_tx_reliable = param->_initial_sn_tx._val._plain._reliable; + ztm->_common._sn_tx_best_effort = param->_initial_sn_tx._val._plain._best_effort; // Initialize peer list ztm->_peers = _z_transport_peer_entry_list_new(); #if Z_FEATURE_MULTI_THREAD == 1 // Tasks - ztm->_read_task_running = false; - ztm->_read_task = NULL; - ztm->_lease_task_running = false; - ztm->_lease_task = NULL; + ztm->_common._read_task_running = false; + ztm->_common._read_task = NULL; + ztm->_common._lease_task_running = false; + ztm->_common._lease_task = NULL; #endif // Z_FEATURE_MULTI_THREAD == 1 - ztm->_lease = Z_TRANSPORT_LEASE; + ztm->_common._lease = Z_TRANSPORT_LEASE; // Notifiers - ztm->_transmitted = false; + ztm->_common._transmitted = false; // Transport link for multicast - ztm->_link = *zl; + ztm->_common._link = *zl; } return ret; } @@ -136,7 +150,7 @@ z_result_t _z_multicast_open_peer(_z_transport_multicast_establish_param_t *para _z_transport_message_t jsm = _z_t_msg_make_join(Z_WHATAMI_PEER, Z_TRANSPORT_LEASE, zid, next_sn); // Encode and send the message - _Z_INFO("Sending Z_JOIN message"); + _Z_DEBUG("Sending Z_JOIN message"); switch (zl->_cap._transport) { case Z_LINK_CAP_TRANSPORT_MULTICAST: ret = _z_link_send_t_msg(zl, &jsm); @@ -170,7 +184,7 @@ z_result_t _z_multicast_send_close(_z_transport_multicast_t *ztm, uint8_t reason z_result_t ret = _Z_RES_OK; // Send and clear message _z_transport_message_t cm = _z_t_msg_make_close(reason, link_only); - ret = ztm->_send_f(ztm, &cm); + ret = ztm->_send_f(&ztm->_common, &cm); _z_t_msg_clear(&cm); return ret; } @@ -183,27 +197,29 @@ void _z_multicast_transport_clear(_z_transport_t *zt) { _z_transport_multicast_t *ztm = &zt->_transport._multicast; #if Z_FEATURE_MULTI_THREAD == 1 // Clean up tasks - if (ztm->_read_task != NULL) { - _z_task_join(ztm->_read_task); - z_free(ztm->_read_task); + if (ztm->_common._read_task != NULL) { + _z_task_join(ztm->_common._read_task); + z_free(ztm->_common._read_task); } - if (ztm->_lease_task != NULL) { - _z_task_join(ztm->_lease_task); - z_free(ztm->_lease_task); + if (ztm->_common._lease_task != NULL) { + _z_task_join(ztm->_common._lease_task); + z_free(ztm->_common._lease_task); } // Clean up the mutexes - _z_mutex_drop(&ztm->_mutex_tx); - _z_mutex_drop(&ztm->_mutex_rx); + _z_mutex_drop(&ztm->_common._mutex_tx); + _z_mutex_drop(&ztm->_common._mutex_rx); _z_mutex_drop(&ztm->_mutex_peer); #endif // Z_FEATURE_MULTI_THREAD == 1 // Clean up the buffers - _z_wbuf_clear(&ztm->_wbuf); - _z_zbuf_clear(&ztm->_zbuf); + _z_wbuf_clear(&ztm->_common._wbuf); + _z_zbuf_clear(&ztm->_common._zbuf); + _z_arc_slice_svec_release(&ztm->_common._arc_pool); + _z_network_message_svec_release(&ztm->_common._msg_pool); // Clean up peer list _z_transport_peer_entry_list_free(&ztm->_peers); - _z_link_clear(&ztm->_link); + _z_link_clear(&ztm->_common._link); } #else @@ -243,7 +259,6 @@ z_result_t _z_multicast_transport_close(_z_transport_multicast_t *ztm, uint8_t r _ZP_UNUSED(ztm); _ZP_UNUSED(reason); return _Z_ERR_TRANSPORT_NOT_AVAILABLE; - ; } void _z_multicast_transport_clear(_z_transport_t *zt) { _ZP_UNUSED(zt); } diff --git a/src/transport/multicast/tx.c b/src/transport/multicast/tx.c deleted file mode 100644 index bbb555edd..000000000 --- a/src/transport/multicast/tx.c +++ /dev/null @@ -1,180 +0,0 @@ -// -// 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, -// - -#include "zenoh-pico/transport/common/tx.h" - -#include "zenoh-pico/config.h" -#include "zenoh-pico/protocol/codec/network.h" -#include "zenoh-pico/protocol/codec/transport.h" -#include "zenoh-pico/transport/multicast/tx.h" -#include "zenoh-pico/transport/utils.h" -#include "zenoh-pico/utils/logging.h" - -#if Z_FEATURE_MULTICAST_TRANSPORT == 1 - -/** - * This function is unsafe because it operates in potentially concurrent data. - * Make sure that the following mutexes are locked before calling this function: - * - ztm->_mutex_inner - */ -_z_zint_t __unsafe_z_multicast_get_sn(_z_transport_multicast_t *ztm, z_reliability_t reliability) { - _z_zint_t sn; - if (reliability == Z_RELIABILITY_RELIABLE) { - sn = ztm->_sn_tx_reliable; - ztm->_sn_tx_reliable = _z_sn_increment(ztm->_sn_res, ztm->_sn_tx_reliable); - } else { - sn = ztm->_sn_tx_best_effort; - ztm->_sn_tx_best_effort = _z_sn_increment(ztm->_sn_res, ztm->_sn_tx_best_effort); - } - return sn; -} - -z_result_t _z_multicast_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg) { - z_result_t ret = _Z_RES_OK; - _Z_DEBUG(">> send session message"); - -#if Z_FEATURE_MULTI_THREAD == 1 - // Acquire the lock - _z_mutex_lock(&ztm->_mutex_tx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - - // Prepare the buffer eventually reserving space for the message length - __unsafe_z_prepare_wbuf(&ztm->_wbuf, ztm->_link._cap._flow); - - // Encode the session message - ret = _z_transport_message_encode(&ztm->_wbuf, t_msg); - if (ret == _Z_RES_OK) { - // Write the message length in the reserved space if needed - __unsafe_z_finalize_wbuf(&ztm->_wbuf, ztm->_link._cap._flow); - // Send the wbuf on the socket - ret = _z_link_send_wbuf(&ztm->_link, &ztm->_wbuf); - if (ret == _Z_RES_OK) { - ztm->_transmitted = true; // Mark the session that we have transmitted data - } - } - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztm->_mutex_tx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - - return ret; -} - -z_result_t _z_multicast_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl) { - z_result_t ret = _Z_RES_OK; - _Z_DEBUG(">> send network message"); - - _z_transport_multicast_t *ztm = &zn->_tp._transport._multicast; - - // Acquire the lock and drop the message if needed - bool drop = false; - if (cong_ctrl == Z_CONGESTION_CONTROL_BLOCK) { -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_lock(&ztm->_mutex_tx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - } else { -#if Z_FEATURE_MULTI_THREAD == 1 - z_result_t locked = _z_mutex_try_lock(&ztm->_mutex_tx); - if (locked != 0) { - _Z_INFO("Dropping zenoh message because of congestion control"); - // We failed to acquire the lock, drop the message - drop = true; - } -#endif // Z_FEATURE_MULTI_THREAD == 1 - } - - if (drop == false) { - // Prepare the buffer eventually reserving space for the message length - __unsafe_z_prepare_wbuf(&ztm->_wbuf, ztm->_link._cap._flow); - - _z_zint_t sn = __unsafe_z_multicast_get_sn(ztm, reliability); // Get the next sequence number - - _z_transport_message_t t_msg = _z_t_msg_make_frame_header(sn, reliability); - ret = _z_transport_message_encode(&ztm->_wbuf, &t_msg); // Encode the frame header - if (ret == _Z_RES_OK) { - ret = _z_network_message_encode(&ztm->_wbuf, n_msg); // Encode the network message - if (ret == _Z_RES_OK) { - // Write the message length in the reserved space if needed - __unsafe_z_finalize_wbuf(&ztm->_wbuf, ztm->_link._cap._flow); - - ret = _z_link_send_wbuf(&ztm->_link, &ztm->_wbuf); // Send the wbuf on the socket - if (ret == _Z_RES_OK) { - ztm->_transmitted = true; // Mark the session that we have transmitted data - } - } else { -#if Z_FEATURE_FRAGMENTATION == 1 - // The message does not fit in the current batch, let's fragment it - // Create an expandable wbuf for fragmentation - _z_wbuf_t fbf = _z_wbuf_make(_Z_FRAG_BUFF_BASE_SIZE, true); - - ret = _z_network_message_encode(&fbf, n_msg); // Encode the message on the expandable wbuf - if (ret == _Z_RES_OK) { - bool is_first = true; // Fragment and send the message - while (_z_wbuf_len(&fbf) > 0) { - if (is_first == false) { // Get the fragment sequence number - sn = __unsafe_z_multicast_get_sn(ztm, reliability); - } - - // Clear the buffer for serialization - __unsafe_z_prepare_wbuf(&ztm->_wbuf, ztm->_link._cap._flow); - - // Serialize one fragment - ret = __unsafe_z_serialize_zenoh_fragment(&ztm->_wbuf, &fbf, reliability, sn, is_first); - if (ret == _Z_RES_OK) { - // Write the message length in the reserved space if needed - __unsafe_z_finalize_wbuf(&ztm->_wbuf, ztm->_link._cap._flow); - - ret = _z_link_send_wbuf(&ztm->_link, &ztm->_wbuf); // Send the wbuf on the socket - if (ret == _Z_RES_OK) { - ztm->_transmitted = true; // Mark the session that we have transmitted data - } - } else { - _Z_ERROR("Fragment serialization failed with err %d", ret); - } - is_first = false; - } - } - // Clear the buffer as it's no longer required - _z_wbuf_clear(&fbf); -#else - _Z_INFO("Sending the message required fragmentation feature that is deactivated."); -#endif - } - } - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztm->_mutex_tx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - } - - return ret; -} - -#else -z_result_t _z_multicast_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg) { - _ZP_UNUSED(ztm); - _ZP_UNUSED(t_msg); - return _Z_ERR_TRANSPORT_NOT_AVAILABLE; -} - -z_result_t _z_multicast_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl) { - _ZP_UNUSED(zn); - _ZP_UNUSED(n_msg); - _ZP_UNUSED(reliability); - _ZP_UNUSED(cong_ctrl); - return _Z_ERR_TRANSPORT_NOT_AVAILABLE; -} -#endif // Z_FEATURE_MULTICAST_TRANSPORT == 1 diff --git a/src/transport/peer_entry.c b/src/transport/peer_entry.c index b363b2eab..38180e5ed 100644 --- a/src/transport/peer_entry.c +++ b/src/transport/peer_entry.c @@ -28,9 +28,10 @@ void _z_transport_peer_entry_clear(_z_transport_peer_entry_t *src) { void _z_transport_peer_entry_copy(_z_transport_peer_entry_t *dst, const _z_transport_peer_entry_t *src) { #if Z_FEATURE_FRAGMENTATION == 1 + dst->_state_reliable = src->_state_reliable; + dst->_state_best_effort = src->_state_best_effort; _z_wbuf_copy(&dst->_dbuf_reliable, &src->_dbuf_reliable); _z_wbuf_copy(&dst->_dbuf_best_effort, &src->_dbuf_best_effort); - dst->_patch = src->_patch; #endif diff --git a/src/transport/raweth/read.c b/src/transport/raweth/read.c index a2f080e7b..aa9995562 100644 --- a/src/transport/raweth/read.c +++ b/src/transport/raweth/read.c @@ -37,6 +37,10 @@ z_result_t _zp_raweth_read(_z_transport_multicast_t *ztm) { _z_t_msg_clear(&t_msg); } _z_slice_clear(&addr); + ret = _z_raweth_update_rx_buff(ztm); + if (ret != _Z_RES_OK) { + _Z_ERROR("Failed to allocate rx buffer"); + } return ret; } #else @@ -55,7 +59,7 @@ void *_zp_raweth_read_task(void *ztm_arg) { _z_slice_t addr = _z_slice_alias_buf(NULL, 0); // Task loop - while (ztm->_read_task_running == true) { + while (ztm->_common._read_task_running == true) { // Read message from link z_result_t ret = _z_raweth_recv_t_msg(ztm, &t_msg, &addr); switch (ret) { @@ -69,20 +73,26 @@ void *_zp_raweth_read_task(void *ztm_arg) { break; default: // Drop message & stop task - _Z_ERROR("Connection closed due to malformed message"); - ztm->_read_task_running = false; + _Z_ERROR("Connection closed due to malformed message: %d", ret); + ztm->_common._read_task_running = false; _z_slice_clear(&addr); continue; break; } // Process message - if (_z_multicast_handle_transport_message(ztm, &t_msg, &addr) != _Z_RES_OK) { - ztm->_read_task_running = false; + ret = _z_multicast_handle_transport_message(ztm, &t_msg, &addr); + if (ret != _Z_RES_OK) { + _Z_ERROR("Connection closed due to message processing error: %d", ret); + ztm->_common._read_task_running = false; _z_slice_clear(&addr); continue; } _z_t_msg_clear(&t_msg); _z_slice_clear(&addr); + if (_z_raweth_update_rx_buff(ztm) != _Z_RES_OK) { + _Z_ERROR("Connection closed due to lack of memory to allocate rx buffer"); + ztm->_common._read_task_running = false; + } } return NULL; } @@ -90,19 +100,19 @@ void *_zp_raweth_read_task(void *ztm_arg) { z_result_t _zp_raweth_start_read_task(_z_transport_t *zt, z_task_attr_t *attr, _z_task_t *task) { // Init memory (void)memset(task, 0, sizeof(_z_task_t)); - zt->_transport._unicast._lease_task_running = true; // Init before z_task_init for concurrency issue + zt->_transport._unicast._common._lease_task_running = true; // Init before z_task_init for concurrency issue // Init task if (_z_task_init(task, attr, _zp_raweth_read_task, &zt->_transport._raweth) != _Z_RES_OK) { - zt->_transport._unicast._lease_task_running = false; + zt->_transport._unicast._common._lease_task_running = false; return _Z_ERR_SYSTEM_TASK_FAILED; } // Attach task - zt->_transport._raweth._read_task = task; + zt->_transport._raweth._common._read_task = task; return _Z_RES_OK; } z_result_t _zp_raweth_stop_read_task(_z_transport_t *zt) { - zt->_transport._raweth._read_task_running = false; + zt->_transport._raweth._common._read_task_running = false; return _Z_RES_OK; } #else diff --git a/src/transport/raweth/rx.c b/src/transport/raweth/rx.c index 59b57e512..fdec282f8 100644 --- a/src/transport/raweth/rx.c +++ b/src/transport/raweth/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/multicast/transport.h" #include "zenoh-pico/transport/utils.h" #include "zenoh-pico/utils/logging.h" @@ -77,20 +78,16 @@ z_result_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_m _Z_DEBUG(">> recv session msg"); z_result_t ret = _Z_RES_OK; -#if Z_FEATURE_MULTI_THREAD == 1 - // Acquire the lock - _z_mutex_lock(&ztm->_mutex_rx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - + _z_transport_rx_mutex_lock(&ztm->_common); // Prepare the buffer - _z_zbuf_reset(&ztm->_zbuf); + _z_zbuf_reset(&ztm->_common._zbuf); - switch (ztm->_link._cap._flow) { + switch (ztm->_common._link._cap._flow) { // Datagram capable links case Z_LINK_CAP_FLOW_DATAGRAM: { - _z_zbuf_compact(&ztm->_zbuf); + _z_zbuf_compact(&ztm->_common._zbuf); // Read from link - size_t to_read = _z_raweth_link_recv_zbuf(&ztm->_link, &ztm->_zbuf, addr); + size_t to_read = _z_raweth_link_recv_zbuf(&ztm->_common._link, &ztm->_common._zbuf, addr); if (to_read == SIZE_MAX) { ret = _Z_ERR_TRANSPORT_RX_FAILED; } @@ -102,14 +99,10 @@ z_result_t _z_raweth_recv_t_msg_na(_z_transport_multicast_t *ztm, _z_transport_m } // Decode message if (ret == _Z_RES_OK) { - _Z_DEBUG(">> \t transport_message_decode: %ju", (uintmax_t)_z_zbuf_len(&ztm->_zbuf)); - ret = _z_transport_message_decode(t_msg, &ztm->_zbuf); + _Z_DEBUG(">> \t transport_message_decode: %ju", (uintmax_t)_z_zbuf_len(&ztm->_common._zbuf)); + ret = _z_transport_message_decode(t_msg, &ztm->_common._zbuf, &ztm->_common._arc_pool, &ztm->_common._msg_pool); } - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztm->_mutex_rx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - + _z_transport_rx_mutex_unlock(&ztm->_common); return ret; } @@ -117,6 +110,26 @@ z_result_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_mess return _z_raweth_recv_t_msg_na(ztm, t_msg, addr); } +z_result_t _z_raweth_update_rx_buff(_z_transport_multicast_t *ztm) { + // Check if user or defragment buffer took ownership of buffer + if (_z_zbuf_get_ref_count(&ztm->_common._zbuf) != 1) { + // Allocate a new buffer + _z_zbuf_t new_zbuf = _z_zbuf_make(Z_BATCH_MULTICAST_SIZE); + if (_z_zbuf_capacity(&new_zbuf) != Z_BATCH_MULTICAST_SIZE) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + // Recopy leftover bytes + size_t leftovers = _z_zbuf_len(&ztm->_common._zbuf); + if (leftovers > 0) { + _z_zbuf_copy_bytes(&new_zbuf, &ztm->_common._zbuf); + } + // Drop buffer & update + _z_zbuf_clear(&ztm->_common._zbuf); + ztm->_common._zbuf = new_zbuf; + } + return _Z_RES_OK; +} + #else z_result_t _z_raweth_recv_t_msg(_z_transport_multicast_t *ztm, _z_transport_message_t *t_msg, _z_slice_t *addr) { _ZP_UNUSED(ztm); diff --git a/src/transport/raweth/tx.c b/src/transport/raweth/tx.c index 538c52e1a..e8f9be4c4 100644 --- a/src/transport/raweth/tx.c +++ b/src/transport/raweth/tx.c @@ -24,18 +24,13 @@ #include "zenoh-pico/protocol/iobuf.h" #include "zenoh-pico/protocol/keyexpr.h" #include "zenoh-pico/system/link/raweth.h" +#include "zenoh-pico/transport/multicast/transport.h" #include "zenoh-pico/transport/transport.h" #include "zenoh-pico/transport/utils.h" #include "zenoh-pico/utils/logging.h" #if Z_FEATURE_RAWETH_TRANSPORT == 1 -#if Z_FEATURE_MULTI_THREAD == 1 -static void _zp_raweth_unlock_tx_mutex(_z_transport_multicast_t *ztm) { _z_mutex_unlock(&ztm->_mutex_tx); } -#else -static void _zp_raweth_unlock_tx_mutex(_z_transport_multicast_t *ztm) { _ZP_UNUSED(ztm); } -#endif - static int _zp_raweth_find_map_entry(const _z_keyexpr_t *keyexpr, _z_raweth_socket_t *sock) { for (size_t i = 0; i < _zp_raweth_mapping_array_len(&sock->_mapping); i++) { // Find matching keyexpr @@ -92,11 +87,11 @@ static z_result_t _zp_raweth_set_socket(const _z_keyexpr_t *keyexpr, _z_raweth_s static _z_zint_t __unsafe_z_raweth_get_sn(_z_transport_multicast_t *ztm, z_reliability_t reliability) { _z_zint_t sn; if (reliability == Z_RELIABILITY_RELIABLE) { - sn = ztm->_sn_tx_reliable; - ztm->_sn_tx_reliable = _z_sn_increment(ztm->_sn_res, ztm->_sn_tx_reliable); + sn = ztm->_common._sn_tx_reliable; + ztm->_common._sn_tx_reliable = _z_sn_increment(ztm->_common._sn_res, ztm->_common._sn_tx_reliable); } else { - sn = ztm->_sn_tx_best_effort; - ztm->_sn_tx_best_effort = _z_sn_increment(ztm->_sn_res, ztm->_sn_tx_best_effort); + sn = ztm->_common._sn_tx_best_effort; + ztm->_common._sn_tx_best_effort = _z_sn_increment(ztm->_common._sn_res, ztm->_common._sn_tx_best_effort); } return sn; } @@ -192,32 +187,26 @@ z_result_t _z_raweth_link_send_t_msg(const _z_link_t *zl, const _z_transport_mes return ret; } -z_result_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg) { +z_result_t _z_raweth_send_t_msg(_z_transport_common_t *ztc, const _z_transport_message_t *t_msg) { z_result_t ret = _Z_RES_OK; _Z_DEBUG(">> send session message"); -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_lock(&ztm->_mutex_tx); -#endif + _z_transport_tx_mutex_lock(ztc, true); // Reset wbuf - _z_wbuf_reset(&ztm->_wbuf); + _z_wbuf_reset(&ztc->_wbuf); // Set socket info - _Z_CLEAN_RETURN_IF_ERR(_zp_raweth_set_socket(NULL, &ztm->_link._socket._raweth), _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(_zp_raweth_set_socket(NULL, &ztc->_link._socket._raweth), _z_transport_tx_mutex_unlock(ztc)); // Prepare buff - __unsafe_z_raweth_prepare_header(&ztm->_link, &ztm->_wbuf); + __unsafe_z_raweth_prepare_header(&ztc->_link, &ztc->_wbuf); // Encode the session message - _Z_CLEAN_RETURN_IF_ERR(_z_transport_message_encode(&ztm->_wbuf, t_msg), _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(_z_transport_message_encode(&ztc->_wbuf, t_msg), _z_transport_tx_mutex_unlock(ztc)); // Write the message header - _Z_CLEAN_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_link, &ztm->_wbuf), _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztc->_link, &ztc->_wbuf), _z_transport_tx_mutex_unlock(ztc)); // Send the wbuf on the socket - _Z_CLEAN_RETURN_IF_ERR(_z_raweth_link_send_wbuf(&ztm->_link, &ztm->_wbuf), _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(_z_raweth_link_send_wbuf(&ztc->_link, &ztc->_wbuf), _z_transport_tx_mutex_unlock(ztc)); // Mark the session that we have transmitted data - ztm->_transmitted = true; - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztm->_mutex_tx); -#endif - + ztc->_transmitted = true; + _z_transport_tx_mutex_unlock(ztc); return ret; } @@ -228,20 +217,11 @@ z_result_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_ _Z_DEBUG(">> send network message"); // Acquire the lock and drop the message if needed -#if Z_FEATURE_MULTI_THREAD == 1 - if (cong_ctrl == Z_CONGESTION_CONTROL_BLOCK) { - _z_mutex_lock(&ztm->_mutex_tx); - } else { - if (_z_mutex_try_lock(&ztm->_mutex_tx) != 0) { - _Z_INFO("Dropping zenoh message because of congestion control"); - // We failed to acquire the lock, drop the message - return ret; - } + ret = _z_transport_tx_mutex_lock(&ztm->_common, cong_ctrl == Z_CONGESTION_CONTROL_BLOCK); + if (ret != _Z_RES_OK) { + _Z_INFO("Dropping zenoh message because of congestion control"); + return ret; } -#else - _ZP_UNUSED(cong_ctrl); -#endif // Z_FEATURE_MULTI_THREAD == 1 - const _z_keyexpr_t *keyexpr = NULL; switch (n_msg->_tag) { case _Z_N_PUSH: @@ -259,32 +239,34 @@ z_result_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_ break; } // Reset wbuf - _z_wbuf_reset(&ztm->_wbuf); + _z_wbuf_reset(&ztm->_common._wbuf); // Set socket info - _Z_CLEAN_RETURN_IF_ERR(_zp_raweth_set_socket(keyexpr, &ztm->_link._socket._raweth), - _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(_zp_raweth_set_socket(keyexpr, &ztm->_common._link._socket._raweth), + _z_transport_tx_mutex_unlock(&ztm->_common)); // Prepare buff - __unsafe_z_raweth_prepare_header(&ztm->_link, &ztm->_wbuf); + __unsafe_z_raweth_prepare_header(&ztm->_common._link, &ztm->_common._wbuf); // Set the frame header _z_zint_t sn = __unsafe_z_raweth_get_sn(ztm, reliability); _z_transport_message_t t_msg = _z_t_msg_make_frame_header(sn, reliability); // Encode the frame header - _Z_CLEAN_RETURN_IF_ERR(_z_transport_message_encode(&ztm->_wbuf, &t_msg), _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(_z_transport_message_encode(&ztm->_common._wbuf, &t_msg), + _z_transport_tx_mutex_unlock(&ztm->_common)); // Encode the network message - if (_z_network_message_encode(&ztm->_wbuf, n_msg) == _Z_RES_OK) { + if (_z_network_message_encode(&ztm->_common._wbuf, n_msg) == _Z_RES_OK) { // Write the eth header - _Z_CLEAN_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_link, &ztm->_wbuf), - _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_common._link, &ztm->_common._wbuf), + _z_transport_tx_mutex_unlock(&ztm->_common)); // Send the wbuf on the socket - _Z_CLEAN_RETURN_IF_ERR(_z_raweth_link_send_wbuf(&ztm->_link, &ztm->_wbuf), _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(_z_raweth_link_send_wbuf(&ztm->_common._link, &ztm->_common._wbuf), + _z_transport_tx_mutex_unlock(&ztm->_common)); // Mark the session that we have transmitted data - ztm->_transmitted = true; + ztm->_common._transmitted = true; } else { // The message does not fit in the current batch, let's fragment it #if Z_FEATURE_FRAGMENTATION == 1 // Create an expandable wbuf for fragmentation _z_wbuf_t fbf = _z_wbuf_make(_Z_FRAG_BUFF_BASE_SIZE, true); // Encode the message on the expandable wbuf - _Z_CLEAN_RETURN_IF_ERR(_z_network_message_encode(&fbf, n_msg), _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(_z_network_message_encode(&fbf, n_msg), _z_transport_tx_mutex_unlock(&ztm->_common)); // Fragment and send the message bool is_first = true; while (_z_wbuf_len(&fbf) > 0) { @@ -293,19 +275,21 @@ z_result_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_ sn = __unsafe_z_raweth_get_sn(ztm, reliability); } // Reset wbuf - _z_wbuf_reset(&ztm->_wbuf); + _z_wbuf_reset(&ztm->_common._wbuf); // Prepare buff - __unsafe_z_raweth_prepare_header(&ztm->_link, &ztm->_wbuf); + __unsafe_z_raweth_prepare_header(&ztm->_common._link, &ztm->_common._wbuf); // Serialize one fragment - _Z_CLEAN_RETURN_IF_ERR(__unsafe_z_serialize_zenoh_fragment(&ztm->_wbuf, &fbf, reliability, sn, is_first), - _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR( + __unsafe_z_serialize_zenoh_fragment(&ztm->_common._wbuf, &fbf, reliability, sn, is_first), + _z_transport_tx_mutex_unlock(&ztm->_common)); // Write the eth header - _Z_CLEAN_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_link, &ztm->_wbuf), - _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(__unsafe_z_raweth_write_header(&ztm->_common._link, &ztm->_common._wbuf), + _z_transport_tx_mutex_unlock(&ztm->_common)); // Send the wbuf on the socket - _Z_CLEAN_RETURN_IF_ERR(_z_raweth_link_send_wbuf(&ztm->_link, &ztm->_wbuf), _zp_raweth_unlock_tx_mutex(ztm)); + _Z_CLEAN_RETURN_IF_ERR(_z_raweth_link_send_wbuf(&ztm->_common._link, &ztm->_common._wbuf), + _z_transport_tx_mutex_unlock(&ztm->_common)); // Mark the session that we have transmitted data - ztm->_transmitted = true; + ztm->_common._transmitted = true; is_first = false; } // Clear the expandable buffer @@ -314,9 +298,7 @@ z_result_t _z_raweth_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_ _Z_INFO("Sending the message required fragmentation feature that is deactivated."); #endif } -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztm->_mutex_tx); -#endif // Z_FEATURE_MULTI_THREAD == 1 + _z_transport_tx_mutex_unlock(&ztm->_common); return ret; } @@ -326,8 +308,8 @@ z_result_t _z_raweth_link_send_t_msg(const _z_link_t *zl, const _z_transport_mes _ZP_UNUSED(t_msg); return _Z_ERR_TRANSPORT_NOT_AVAILABLE; } -z_result_t _z_raweth_send_t_msg(_z_transport_multicast_t *ztm, const _z_transport_message_t *t_msg) { - _ZP_UNUSED(ztm); +z_result_t _z_raweth_send_t_msg(_z_transport_common_t *ztc, const _z_transport_message_t *t_msg) { + _ZP_UNUSED(ztc); _ZP_UNUSED(t_msg); return _Z_ERR_TRANSPORT_NOT_AVAILABLE; } diff --git a/src/transport/transport.c b/src/transport/transport.c index 5aee622c6..2540ec5ac 100644 --- a/src/transport/transport.c +++ b/src/transport/transport.c @@ -22,13 +22,11 @@ #include "zenoh-pico/link/link.h" #include "zenoh-pico/protocol/core.h" #include "zenoh-pico/transport/multicast/rx.h" -#include "zenoh-pico/transport/multicast/tx.h" #include "zenoh-pico/transport/raweth/rx.h" #include "zenoh-pico/transport/raweth/tx.h" #include "zenoh-pico/transport/transport.h" #include "zenoh-pico/transport/unicast/rx.h" #include "zenoh-pico/transport/unicast/transport.h" -#include "zenoh-pico/transport/unicast/tx.h" #include "zenoh-pico/transport/utils.h" #include "zenoh-pico/utils/logging.h" @@ -78,6 +76,53 @@ void _z_transport_free(_z_transport_t **zt) { *zt = NULL; } +#if Z_FEATURE_BATCHING == 1 +bool _z_transport_start_batching(_z_transport_t *zt) { + uint8_t *batch_state = NULL; + size_t *batch_count = NULL; + switch (zt->_type) { + case _Z_TRANSPORT_UNICAST_TYPE: + batch_state = &zt->_transport._unicast._common._batch_state; + batch_count = &zt->_transport._unicast._common._batch_count; + break; + case _Z_TRANSPORT_MULTICAST_TYPE: + batch_state = &zt->_transport._multicast._common._batch_state; + batch_count = &zt->_transport._multicast._common._batch_count; + break; + case _Z_TRANSPORT_RAWETH_TYPE: + batch_state = &zt->_transport._raweth._common._batch_state; + batch_count = &zt->_transport._raweth._common._batch_count; + break; + default: + break; + } + if (*batch_state == _Z_BATCHING_ACTIVE) { + return false; + } + *batch_count = 0; + *batch_state = _Z_BATCHING_ACTIVE; + return true; +} + +void _z_transport_stop_batching(_z_transport_t *zt) { + uint8_t *batch_state = NULL; + switch (zt->_type) { + case _Z_TRANSPORT_UNICAST_TYPE: + batch_state = &zt->_transport._unicast._common._batch_state; + break; + case _Z_TRANSPORT_MULTICAST_TYPE: + batch_state = &zt->_transport._multicast._common._batch_state; + break; + case _Z_TRANSPORT_RAWETH_TYPE: + batch_state = &zt->_transport._raweth._common._batch_state; + break; + default: + break; + } + *batch_state = _Z_BATCHING_IDLE; +} +#endif + /** * @brief Inserts an entry into `root`, allocating it a `_peer_id` * diff --git a/src/transport/unicast.c b/src/transport/unicast.c index f194b4ee3..2387565cc 100644 --- a/src/transport/unicast.c +++ b/src/transport/unicast.c @@ -17,6 +17,15 @@ #include #include +#include "zenoh-pico/link/link.h" +#include "zenoh-pico/transport/common/rx.h" +#include "zenoh-pico/transport/common/tx.h" +#include "zenoh-pico/transport/multicast/rx.h" +#include "zenoh-pico/transport/unicast/lease.h" +#include "zenoh-pico/transport/unicast/read.h" +#include "zenoh-pico/transport/unicast/rx.h" +#include "zenoh-pico/transport/utils.h" +#include "zenoh-pico/utils/logging.h" #include "zenoh-pico/utils/uuid.h" #if Z_FEATURE_UNICAST_TRANSPORT == 1 diff --git a/src/transport/unicast/lease.c b/src/transport/unicast/lease.c index 32358eaba..c4cd362f1 100644 --- a/src/transport/unicast/lease.c +++ b/src/transport/unicast/lease.c @@ -14,8 +14,10 @@ #include "zenoh-pico/transport/unicast/lease.h" +#include "zenoh-pico/session/query.h" +#include "zenoh-pico/session/utils.h" +#include "zenoh-pico/transport/common/tx.h" #include "zenoh-pico/transport/unicast/transport.h" -#include "zenoh-pico/transport/unicast/tx.h" #include "zenoh-pico/utils/logging.h" #if Z_FEATURE_UNICAST_TRANSPORT == 1 @@ -24,7 +26,7 @@ z_result_t _zp_unicast_send_keep_alive(_z_transport_unicast_t *ztu) { z_result_t ret = _Z_RES_OK; _z_transport_message_t t_msg = _z_t_msg_make_keep_alive(); - ret = _z_unicast_send_t_msg(ztu, &t_msg); + ret = _z_transport_tx_send_t_msg(&ztu->_common, &t_msg); return ret; } @@ -42,11 +44,11 @@ void *_zp_unicast_lease_task(void *ztu_arg) { _z_transport_unicast_t *ztu = (_z_transport_unicast_t *)ztu_arg; ztu->_received = false; - ztu->_transmitted = false; + ztu->_common._transmitted = false; - int next_lease = (int)ztu->_lease; - int next_keep_alive = (int)(ztu->_lease / Z_TRANSPORT_LEASE_EXPIRE_FACTOR); - while (ztu->_lease_task_running == true) { + int next_lease = (int)ztu->_common._lease; + int next_keep_alive = (int)(ztu->_common._lease / Z_TRANSPORT_LEASE_EXPIRE_FACTOR); + while (ztu->_common._lease_task_running == true) { // Next lease process if (next_lease <= 0) { // Check if received data @@ -54,26 +56,27 @@ void *_zp_unicast_lease_task(void *ztu_arg) { // Reset the lease parameters ztu->_received = false; } else { - _Z_INFO("Closing session because it has expired after %zums", ztu->_lease); - ztu->_lease_task_running = false; + _Z_INFO("Closing session because it has expired after %zums", ztu->_common._lease); + ztu->_common._lease_task_running = false; _z_unicast_transport_close(ztu, _Z_CLOSE_EXPIRED); break; } - next_lease = (int)ztu->_lease; + next_lease = (int)ztu->_common._lease; } // Next keep alive process if (next_keep_alive <= 0) { // Check if need to send a keep alive - if (ztu->_transmitted == false) { + if (ztu->_common._transmitted == false) { if (_zp_unicast_send_keep_alive(ztu) < 0) { - // TODO: Handle retransmission or error + _Z_INFO("Send keep alive failed."); } } - // Reset the keep alive parameters - ztu->_transmitted = false; - next_keep_alive = (int)(ztu->_lease / Z_TRANSPORT_LEASE_EXPIRE_FACTOR); + ztu->_common._transmitted = false; + next_keep_alive = (int)(ztu->_common._lease / Z_TRANSPORT_LEASE_EXPIRE_FACTOR); } + // Query timeout process + _z_pending_query_process_timeout(_Z_RC_IN_VAL(ztu->_common._session)); // Compute the target interval int interval; @@ -98,18 +101,18 @@ void *_zp_unicast_lease_task(void *ztu_arg) { z_result_t _zp_unicast_start_lease_task(_z_transport_t *zt, z_task_attr_t *attr, _z_task_t *task) { // Init memory (void)memset(task, 0, sizeof(_z_task_t)); - zt->_transport._unicast._lease_task_running = true; // Init before z_task_init for concurrency issue + zt->_transport._unicast._common._lease_task_running = true; // Init before z_task_init for concurrency issue // Init task if (_z_task_init(task, attr, _zp_unicast_lease_task, &zt->_transport._unicast) != _Z_RES_OK) { return _Z_ERR_SYSTEM_TASK_FAILED; } // Attach task - zt->_transport._unicast._lease_task = task; + zt->_transport._unicast._common._lease_task = task; return _Z_RES_OK; } z_result_t _zp_unicast_stop_lease_task(_z_transport_t *zt) { - zt->_transport._unicast._lease_task_running = false; + zt->_transport._unicast._common._lease_task_running = false; return _Z_RES_OK; } #else diff --git a/src/transport/unicast/read.c b/src/transport/unicast/read.c index a43f1404c..5851f3146 100644 --- a/src/transport/unicast/read.c +++ b/src/transport/unicast/read.c @@ -33,7 +33,10 @@ z_result_t _zp_unicast_read(_z_transport_unicast_t *ztu) { ret = _z_unicast_handle_transport_message(ztu, &t_msg); _z_t_msg_clear(&t_msg); } - + ret = _z_unicast_update_rx_buffer(ztu); + if (ret != _Z_RES_OK) { + _Z_ERROR("Failed to allocate rx buffer"); + } return ret; } #else @@ -50,38 +53,39 @@ void *_zp_unicast_read_task(void *ztu_arg) { _z_transport_unicast_t *ztu = (_z_transport_unicast_t *)ztu_arg; // Acquire and keep the lock - _z_mutex_lock(&ztu->_mutex_rx); + _z_mutex_lock(&ztu->_common._mutex_rx); // Prepare the buffer - _z_zbuf_reset(&ztu->_zbuf); + _z_zbuf_reset(&ztu->_common._zbuf); - while (ztu->_read_task_running == true) { + while (ztu->_common._read_task_running == true) { // Read bytes from socket to the main buffer size_t to_read = 0; - switch (ztu->_link._cap._flow) { + switch (ztu->_common._link._cap._flow) { case Z_LINK_CAP_FLOW_STREAM: - if (_z_zbuf_len(&ztu->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_link_recv_zbuf(&ztu->_link, &ztu->_zbuf, NULL); - if (_z_zbuf_len(&ztu->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_zbuf_compact(&ztu->_zbuf); + if (_z_zbuf_len(&ztu->_common._zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_link_recv_zbuf(&ztu->_common._link, &ztu->_common._zbuf, NULL); + if (_z_zbuf_len(&ztu->_common._zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_zbuf_compact(&ztu->_common._zbuf); continue; } } // Get stream size - to_read = _z_read_stream_size(&ztu->_zbuf); + to_read = _z_read_stream_size(&ztu->_common._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) { - _z_zbuf_set_rpos(&ztu->_zbuf, _z_zbuf_get_rpos(&ztu->_zbuf) - _Z_MSG_LEN_ENC_SIZE); - _z_zbuf_compact(&ztu->_zbuf); + if (_z_zbuf_len(&ztu->_common._zbuf) < to_read) { + _z_link_recv_zbuf(&ztu->_common._link, &ztu->_common._zbuf, NULL); + if (_z_zbuf_len(&ztu->_common._zbuf) < to_read) { + _z_zbuf_set_rpos(&ztu->_common._zbuf, + _z_zbuf_get_rpos(&ztu->_common._zbuf) - _Z_MSG_LEN_ENC_SIZE); + _z_zbuf_compact(&ztu->_common._zbuf); continue; } } break; case Z_LINK_CAP_FLOW_DATAGRAM: - _z_zbuf_compact(&ztu->_zbuf); - to_read = _z_link_recv_zbuf(&ztu->_link, &ztu->_zbuf, NULL); + _z_zbuf_compact(&ztu->_common._zbuf); + to_read = _z_link_recv_zbuf(&ztu->_common._link, &ztu->_common._zbuf, NULL); if (to_read == SIZE_MAX) { continue; } @@ -90,52 +94,62 @@ void *_zp_unicast_read_task(void *ztu_arg) { break; } // Wrap the main buffer for to_read bytes - _z_zbuf_t zbuf = _z_zbuf_view(&ztu->_zbuf, to_read); + _z_zbuf_t zbuf = _z_zbuf_view(&ztu->_common._zbuf, to_read); // Mark the session that we have received data ztu->_received = true; - // Decode one session message - _z_transport_message_t t_msg; - z_result_t ret = _z_transport_message_decode(&t_msg, &zbuf); + while (_z_zbuf_len(&zbuf) > 0) { + // Decode one session message + _z_transport_message_t t_msg; + z_result_t ret = + _z_transport_message_decode(&t_msg, &zbuf, &ztu->_common._arc_pool, &ztu->_common._msg_pool); - if (ret == _Z_RES_OK) { - ret = _z_unicast_handle_transport_message(ztu, &t_msg); if (ret == _Z_RES_OK) { - _z_t_msg_clear(&t_msg); + ret = _z_unicast_handle_transport_message(ztu, &t_msg); + if (ret == _Z_RES_OK) { + _z_t_msg_clear(&t_msg); + } else { + if (ret != _Z_ERR_CONNECTION_CLOSED) { + _Z_ERROR("Connection closed due to message processing error: %d", ret); + } + ztu->_common._read_task_running = false; + continue; + } } else { - ztu->_read_task_running = false; + _Z_ERROR("Connection closed due to malformed message: %d", ret); + ztu->_common._read_task_running = false; continue; } - } else { - _Z_ERROR("Connection closed due to malformed message"); - ztu->_read_task_running = false; - continue; } - // Move the read position of the read buffer - _z_zbuf_set_rpos(&ztu->_zbuf, _z_zbuf_get_rpos(&ztu->_zbuf) + to_read); + _z_zbuf_set_rpos(&ztu->_common._zbuf, _z_zbuf_get_rpos(&ztu->_common._zbuf) + to_read); + + if (_z_unicast_update_rx_buffer(ztu) != _Z_RES_OK) { + _Z_ERROR("Connection closed due to lack of memory to allocate rx buffer"); + ztu->_common._read_task_running = false; + } } - _z_mutex_unlock(&ztu->_mutex_rx); + _z_mutex_unlock(&ztu->_common._mutex_rx); return NULL; } z_result_t _zp_unicast_start_read_task(_z_transport_t *zt, z_task_attr_t *attr, _z_task_t *task) { // Init memory (void)memset(task, 0, sizeof(_z_task_t)); - zt->_transport._unicast._read_task_running = true; // Init before z_task_init for concurrency issue + zt->_transport._unicast._common._read_task_running = true; // Init before z_task_init for concurrency issue // Init task if (_z_task_init(task, attr, _zp_unicast_read_task, &zt->_transport._unicast) != _Z_RES_OK) { - zt->_transport._unicast._read_task_running = false; + zt->_transport._unicast._common._read_task_running = false; return _Z_ERR_SYSTEM_TASK_FAILED; } // Attach task - zt->_transport._unicast._read_task = task; + zt->_transport._unicast._common._read_task = task; return _Z_RES_OK; } z_result_t _zp_unicast_stop_read_task(_z_transport_t *zt) { - zt->_transport._unicast._read_task_running = false; + zt->_transport._unicast._common._read_task_running = false; return _Z_RES_OK; } diff --git a/src/transport/unicast/rx.c b/src/transport/unicast/rx.c index e0b8e0c68..3f374f5af 100644 --- a/src/transport/unicast/rx.c +++ b/src/transport/unicast/rx.c @@ -23,6 +23,7 @@ #include "zenoh-pico/protocol/iobuf.h" #include "zenoh-pico/session/utils.h" #include "zenoh-pico/transport/unicast/rx.h" +#include "zenoh-pico/transport/unicast/transport.h" #include "zenoh-pico/transport/utils.h" #include "zenoh-pico/utils/logging.h" @@ -31,32 +32,29 @@ z_result_t _z_unicast_recv_t_msg_na(_z_transport_unicast_t *ztu, _z_transport_message_t *t_msg) { _Z_DEBUG(">> recv session msg"); z_result_t ret = _Z_RES_OK; -#if Z_FEATURE_MULTI_THREAD == 1 - // Acquire the lock - _z_mutex_lock(&ztu->_mutex_rx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - + _z_transport_rx_mutex_lock(&ztu->_common); size_t to_read = 0; do { - switch (ztu->_link._cap._flow) { + switch (ztu->_common._link._cap._flow) { // Stream capable links case Z_LINK_CAP_FLOW_STREAM: - if (_z_zbuf_len(&ztu->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_link_recv_zbuf(&ztu->_link, &ztu->_zbuf, NULL); - if (_z_zbuf_len(&ztu->_zbuf) < _Z_MSG_LEN_ENC_SIZE) { - _z_zbuf_compact(&ztu->_zbuf); + if (_z_zbuf_len(&ztu->_common._zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_link_recv_zbuf(&ztu->_common._link, &ztu->_common._zbuf, NULL); + if (_z_zbuf_len(&ztu->_common._zbuf) < _Z_MSG_LEN_ENC_SIZE) { + _z_zbuf_compact(&ztu->_common._zbuf); ret = _Z_ERR_TRANSPORT_NOT_ENOUGH_BYTES; continue; } } // Get stream size - to_read = _z_read_stream_size(&ztu->_zbuf); + to_read = _z_read_stream_size(&ztu->_common._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) { - _z_zbuf_set_rpos(&ztu->_zbuf, _z_zbuf_get_rpos(&ztu->_zbuf) - _Z_MSG_LEN_ENC_SIZE); - _z_zbuf_compact(&ztu->_zbuf); + if (_z_zbuf_len(&ztu->_common._zbuf) < to_read) { + _z_link_recv_zbuf(&ztu->_common._link, &ztu->_common._zbuf, NULL); + if (_z_zbuf_len(&ztu->_common._zbuf) < to_read) { + _z_zbuf_set_rpos(&ztu->_common._zbuf, + _z_zbuf_get_rpos(&ztu->_common._zbuf) - _Z_MSG_LEN_ENC_SIZE); + _z_zbuf_compact(&ztu->_common._zbuf); ret = _Z_ERR_TRANSPORT_NOT_ENOUGH_BYTES; continue; } @@ -64,8 +62,8 @@ z_result_t _z_unicast_recv_t_msg_na(_z_transport_unicast_t *ztu, _z_transport_me break; // Datagram capable links case Z_LINK_CAP_FLOW_DATAGRAM: - _z_zbuf_compact(&ztu->_zbuf); - to_read = _z_link_recv_zbuf(&ztu->_link, &ztu->_zbuf, NULL); + _z_zbuf_compact(&ztu->_common._zbuf); + to_read = _z_link_recv_zbuf(&ztu->_common._link, &ztu->_common._zbuf, NULL); if (to_read == SIZE_MAX) { ret = _Z_ERR_TRANSPORT_RX_FAILED; } @@ -77,18 +75,14 @@ z_result_t _z_unicast_recv_t_msg_na(_z_transport_unicast_t *ztu, _z_transport_me if (ret == _Z_RES_OK) { _Z_DEBUG(">> \t transport_message_decode"); - ret = _z_transport_message_decode(t_msg, &ztu->_zbuf); + ret = _z_transport_message_decode(t_msg, &ztu->_common._zbuf, &ztu->_common._arc_pool, &ztu->_common._msg_pool); // Mark the session that we have received data if (ret == _Z_RES_OK) { ztu->_received = true; } } - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztu->_mutex_rx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - + _z_transport_rx_mutex_unlock(&ztu->_common); return ret; } @@ -101,26 +95,31 @@ z_result_t _z_unicast_handle_transport_message(_z_transport_unicast_t *ztu, _z_t switch (_Z_MID(t_msg->_header)) { case _Z_MID_T_FRAME: { - _Z_INFO("Received Z_FRAME message"); + _Z_DEBUG("Received Z_FRAME message"); + z_reliability_t tmsg_reliability; // Check if the SN is correct if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAME_R) == true) { + tmsg_reliability = Z_RELIABILITY_RELIABLE; // @TODO: amend once reliability is in place. For the time being only // monotonic SNs are ensured - if (_z_sn_precedes(ztu->_sn_res, ztu->_sn_rx_reliable, t_msg->_body._frame._sn) == true) { + if (_z_sn_precedes(ztu->_common._sn_res, ztu->_sn_rx_reliable, t_msg->_body._frame._sn) == true) { ztu->_sn_rx_reliable = t_msg->_body._frame._sn; } else { #if Z_FEATURE_FRAGMENTATION == 1 _z_wbuf_clear(&ztu->_dbuf_reliable); + ztu->_state_reliable = _Z_DBUF_STATE_NULL; #endif _Z_INFO("Reliable message dropped because it is out of order"); break; } } else { - if (_z_sn_precedes(ztu->_sn_res, ztu->_sn_rx_best_effort, t_msg->_body._frame._sn) == true) { + tmsg_reliability = Z_RELIABILITY_BEST_EFFORT; + if (_z_sn_precedes(ztu->_common._sn_res, ztu->_sn_rx_best_effort, t_msg->_body._frame._sn) == true) { ztu->_sn_rx_best_effort = t_msg->_body._frame._sn; } else { #if Z_FEATURE_FRAGMENTATION == 1 _z_wbuf_clear(&ztu->_dbuf_best_effort); + ztu->_state_best_effort = _Z_DBUF_STATE_NULL; #endif _Z_INFO("Best effort message dropped because it is out of order"); break; @@ -128,48 +127,63 @@ z_result_t _z_unicast_handle_transport_message(_z_transport_unicast_t *ztu, _z_t } // Handle all the zenoh message, one by one - size_t len = _z_vec_len(&t_msg->_body._frame._messages); + size_t len = _z_svec_len(&t_msg->_body._frame._messages); for (size_t i = 0; i < len; i++) { - _z_network_message_t *zm = _z_network_message_vec_get(&t_msg->_body._frame._messages, i); - zm->_reliability = _z_t_msg_get_reliability(t_msg); - _z_handle_network_message(ztu->_session, zm, _Z_KEYEXPR_MAPPING_UNKNOWN_REMOTE); + _z_network_message_t *zm = _z_network_message_svec_get(&t_msg->_body._frame._messages, i); + zm->_reliability = tmsg_reliability; + _z_handle_network_message(ztu->_common._session, zm, _Z_KEYEXPR_MAPPING_UNKNOWN_REMOTE); } break; } case _Z_MID_T_FRAGMENT: { - _Z_INFO("Received Z_FRAGMENT message"); + _Z_DEBUG("Received Z_FRAGMENT message"); #if Z_FEATURE_FRAGMENTATION == 1 - bool consecutive; _z_wbuf_t *dbuf; - // Check if the SN is correct and select the right defragmentation buffer - if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAME_R)) { + uint8_t *dbuf_state; + z_reliability_t tmsg_reliability; + bool consecutive; + + // Select the right defragmentation buffer + if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAGMENT_R)) { + tmsg_reliability = Z_RELIABILITY_RELIABLE; + // Check SN // @TODO: amend once reliability is in place. For the time being only // monotonic SNs are ensured - if (_z_sn_precedes(ztu->_sn_res, ztu->_sn_rx_reliable, t_msg->_body._fragment._sn)) { - consecutive = _z_sn_consecutive(ztu->_sn_res, ztu->_sn_rx_reliable, t_msg->_body._fragment._sn); + if (_z_sn_precedes(ztu->_common._sn_res, ztu->_sn_rx_reliable, t_msg->_body._fragment._sn)) { + consecutive = + _z_sn_consecutive(ztu->_common._sn_res, ztu->_sn_rx_reliable, t_msg->_body._fragment._sn); ztu->_sn_rx_reliable = t_msg->_body._fragment._sn; dbuf = &ztu->_dbuf_reliable; + dbuf_state = &ztu->_state_reliable; } else { _z_wbuf_clear(&ztu->_dbuf_reliable); + ztu->_state_reliable = _Z_DBUF_STATE_NULL; _Z_INFO("Reliable message dropped because it is out of order"); break; } } else { - if (_z_sn_precedes(ztu->_sn_res, ztu->_sn_rx_best_effort, t_msg->_body._fragment._sn)) { - consecutive = _z_sn_consecutive(ztu->_sn_res, ztu->_sn_rx_best_effort, t_msg->_body._fragment._sn); + tmsg_reliability = Z_RELIABILITY_BEST_EFFORT; + // Check SN + if (_z_sn_precedes(ztu->_common._sn_res, ztu->_sn_rx_best_effort, t_msg->_body._fragment._sn)) { + consecutive = + _z_sn_consecutive(ztu->_common._sn_res, ztu->_sn_rx_best_effort, t_msg->_body._fragment._sn); ztu->_sn_rx_best_effort = t_msg->_body._fragment._sn; dbuf = &ztu->_dbuf_best_effort; + dbuf_state = &ztu->_state_best_effort; } else { _z_wbuf_clear(&ztu->_dbuf_best_effort); + ztu->_state_best_effort = _Z_DBUF_STATE_NULL; _Z_INFO("Best effort message dropped because it is out of order"); break; } } + // Check consecutive SN if (!consecutive && _z_wbuf_len(dbuf) > 0) { - _Z_DEBUG("Non-consecutive fragments received"); - _z_wbuf_reset(dbuf); + _z_wbuf_clear(dbuf); + *dbuf_state = _Z_DBUF_STATE_NULL; + _Z_INFO("Defragmentation buffer dropped because non-consecutive fragments received"); break; } // Handle fragment markers @@ -177,7 +191,7 @@ z_result_t _z_unicast_handle_transport_message(_z_transport_unicast_t *ztu, _z_t if (t_msg->_body._fragment.first) { _z_wbuf_reset(dbuf); } else if (_z_wbuf_len(dbuf) == 0) { - _Z_DEBUG("First fragment received without the start marker"); + _Z_INFO("First fragment received without the start marker"); break; } if (t_msg->_body._fragment.drop) { @@ -185,40 +199,60 @@ z_result_t _z_unicast_handle_transport_message(_z_transport_unicast_t *ztu, _z_t break; } } - bool drop = false; - if ((_z_wbuf_len(dbuf) + t_msg->_body._fragment._payload.len) > Z_FRAG_MAX_SIZE) { - // Filling the wbuf capacity as a way to signal the last fragment to reset the dbuf - // Otherwise, last (smaller) fragments can be understood as a complete message - _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, _z_wbuf_space_left(dbuf)); - drop = true; - } else { - _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, - t_msg->_body._fragment._payload.len); + // Allocate buffer if needed + if (*dbuf_state == _Z_DBUF_STATE_NULL) { + *dbuf = _z_wbuf_make(Z_FRAG_MAX_SIZE, false); + if (_z_wbuf_capacity(dbuf) != Z_FRAG_MAX_SIZE) { + _Z_ERROR("Not enough memory to allocate transport defragmentation buffer"); + ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; + break; + } + *dbuf_state = _Z_DBUF_STATE_INIT; } - + // Process fragment data + if (*dbuf_state == _Z_DBUF_STATE_INIT) { + // Check overflow + if ((_z_wbuf_len(dbuf) + t_msg->_body._fragment._payload.len) > Z_FRAG_MAX_SIZE) { + *dbuf_state = _Z_DBUF_STATE_OVERFLOW; + } else { + // Fill buffer + _z_wbuf_write_bytes(dbuf, t_msg->_body._fragment._payload.start, 0, + t_msg->_body._fragment._payload.len); + } + } + // Process final fragment if (_Z_HAS_FLAG(t_msg->_header, _Z_FLAG_T_FRAGMENT_M) == false) { - if (drop == true) { // Drop message if it exceeds the fragmentation size - _z_wbuf_reset(dbuf); + // Drop message if it exceeds the fragmentation size + if (*dbuf_state == _Z_DBUF_STATE_OVERFLOW) { + _Z_INFO("Fragment dropped because defragmentation buffer has overflown"); + _z_wbuf_clear(dbuf); + *dbuf_state = _Z_DBUF_STATE_NULL; break; } - - _z_zbuf_t zbf = _z_wbuf_to_zbuf(dbuf); // Convert the defragmentation buffer into a decoding buffer - - _z_zenoh_message_t zm; - ret = _z_network_message_decode(&zm, &zbf); - zm._reliability = _z_t_msg_get_reliability(t_msg); + // Convert the defragmentation buffer into a decoding buffer + _z_zbuf_t zbf = _z_wbuf_moved_as_zbuf(dbuf); + if (_z_zbuf_capacity(&zbf) == 0) { + _Z_ERROR("Failed to convert defragmentation buffer into a decoding buffer!"); + _z_wbuf_clear(dbuf); + *dbuf_state = _Z_DBUF_STATE_NULL; + ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; + break; + } + // Decode message + _z_zenoh_message_t zm = {0}; + assert(ztu->_common._arc_pool._capacity >= 1); + _z_arc_slice_t *arcs = _z_arc_slice_svec_get_mut(&ztu->_common._arc_pool, 0); + ret = _z_network_message_decode(&zm, &zbf, arcs); + zm._reliability = tmsg_reliability; if (ret == _Z_RES_OK) { - _z_handle_network_message(ztu->_session, &zm, _Z_KEYEXPR_MAPPING_UNKNOWN_REMOTE); - _z_msg_clear(&zm); // Clear must be explicitly called for fragmented zenoh messages. Non-fragmented - // zenoh messages are released when their transport message is released. + _z_handle_network_message(ztu->_common._session, &zm, _Z_KEYEXPR_MAPPING_UNKNOWN_REMOTE); } else { - _Z_DEBUG("Failed to decode defragmented message"); + _Z_INFO("Failed to decode defragmented message"); + ret = _Z_ERR_MESSAGE_DESERIALIZATION_FAILED; } - // Free the decoding buffer _z_zbuf_clear(&zbf); - // Reset the defragmentation buffer - _z_wbuf_reset(dbuf); + *dbuf_state = _Z_DBUF_STATE_NULL; } #else _Z_INFO("Fragment dropped because fragmentation feature is deactivated"); @@ -227,7 +261,7 @@ z_result_t _z_unicast_handle_transport_message(_z_transport_unicast_t *ztu, _z_t } case _Z_MID_T_KEEP_ALIVE: { - _Z_INFO("Received Z_KEEP_ALIVE message"); + _Z_DEBUG("Received Z_KEEP_ALIVE message"); break; } @@ -255,6 +289,28 @@ z_result_t _z_unicast_handle_transport_message(_z_transport_unicast_t *ztu, _z_t return ret; } + +z_result_t _z_unicast_update_rx_buffer(_z_transport_unicast_t *ztu) { + // Check if user or defragment buffer took ownership of buffer + if (_z_zbuf_get_ref_count(&ztu->_common._zbuf) != 1) { + // Allocate a new buffer + size_t buff_capacity = _z_zbuf_capacity(&ztu->_common._zbuf); + _z_zbuf_t new_zbuf = _z_zbuf_make(buff_capacity); + if (_z_zbuf_capacity(&new_zbuf) != buff_capacity) { + return _Z_ERR_SYSTEM_OUT_OF_MEMORY; + } + // Recopy leftover bytes + size_t leftovers = _z_zbuf_len(&ztu->_common._zbuf); + if (leftovers > 0) { + _z_zbuf_copy_bytes(&new_zbuf, &ztu->_common._zbuf); + } + // Drop buffer & update + _z_zbuf_clear(&ztu->_common._zbuf); + ztu->_common._zbuf = new_zbuf; + } + return _Z_RES_OK; +} + #else z_result_t _z_unicast_recv_t_msg(_z_transport_unicast_t *ztu, _z_transport_message_t *t_msg) { _ZP_UNUSED(ztu); diff --git a/src/transport/unicast/transport.c b/src/transport/unicast/transport.c index c7ef12aca..7ab1578fa 100644 --- a/src/transport/unicast/transport.c +++ b/src/transport/unicast/transport.c @@ -25,7 +25,6 @@ #include "zenoh-pico/transport/unicast/lease.h" #include "zenoh-pico/transport/unicast/read.h" #include "zenoh-pico/transport/unicast/rx.h" -#include "zenoh-pico/transport/unicast/tx.h" #include "zenoh-pico/transport/utils.h" #include "zenoh-pico/utils/logging.h" @@ -36,18 +35,25 @@ z_result_t _z_unicast_transport_create(_z_transport_t *zt, _z_link_t *zl, z_result_t ret = _Z_RES_OK; zt->_type = _Z_TRANSPORT_UNICAST_TYPE; + _z_transport_unicast_t *ztu = &zt->_transport._unicast; #if Z_FEATURE_FRAGMENTATION == 1 // Patch zt->_transport._unicast._patch = param->_patch; #endif +// Initialize batching data +#if Z_FEATURE_BATCHING == 1 + ztu->_common._batch_state = _Z_BATCHING_IDLE; + ztu->_common._batch_count = 0; +#endif + #if Z_FEATURE_MULTI_THREAD == 1 // Initialize the mutexes - ret = _z_mutex_init(&zt->_transport._unicast._mutex_tx); + ret = _z_mutex_init(&ztu->_common._mutex_tx); if (ret == _Z_RES_OK) { - ret = _z_mutex_init(&zt->_transport._unicast._mutex_rx); + ret = _z_mutex_init(&ztu->_common._mutex_rx); if (ret != _Z_RES_OK) { - _z_mutex_drop(&zt->_transport._unicast._mutex_tx); + _z_mutex_drop(&ztu->_common._mutex_tx); } } #endif // Z_FEATURE_MULTI_THREAD == 1 @@ -55,106 +61,75 @@ z_result_t _z_unicast_transport_create(_z_transport_t *zt, _z_link_t *zl, // Initialize the read and write buffers if (ret == _Z_RES_OK) { uint16_t mtu = (zl->_mtu < param->_batch_size) ? zl->_mtu : param->_batch_size; - size_t dbuf_size = 0; size_t wbuf_size = mtu; size_t zbuf_size = param->_batch_size; - bool expandable = false; - - // Set expandable on stream link - if (zl->_cap._flow == Z_LINK_CAP_FLOW_STREAM) { - expandable = true; - } -#if Z_FEATURE_DYNAMIC_MEMORY_ALLOCATION == 0 - expandable = false; - dbuf_size = Z_FRAG_MAX_SIZE; -#endif // Initialize tx rx buffers - zt->_transport._unicast._wbuf = _z_wbuf_make(wbuf_size, false); - zt->_transport._unicast._zbuf = _z_zbuf_make(zbuf_size); + ztu->_common._wbuf = _z_wbuf_make(wbuf_size, false); + ztu->_common._zbuf = _z_zbuf_make(zbuf_size); + + // Initialize resources pool + ztu->_common._arc_pool = _z_arc_slice_svec_make(_Z_RES_POOL_INIT_SIZE); + ztu->_common._msg_pool = _z_network_message_svec_make(_Z_RES_POOL_INIT_SIZE); // Clean up the buffers if one of them failed to be allocated - if ((_z_wbuf_capacity(&zt->_transport._unicast._wbuf) != wbuf_size) || - (_z_zbuf_capacity(&zt->_transport._unicast._zbuf) != zbuf_size)) { + if ((ztu->_common._msg_pool._capacity == 0) || (ztu->_common._arc_pool._capacity == 0) || + (_z_wbuf_capacity(&ztu->_common._wbuf) != wbuf_size) || + (_z_zbuf_capacity(&ztu->_common._zbuf) != zbuf_size)) { ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; _Z_ERROR("Not enough memory to allocate transport tx rx buffers!"); #if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_drop(&zt->_transport._unicast._mutex_tx); - _z_mutex_drop(&zt->_transport._unicast._mutex_rx); + _z_mutex_drop(&ztu->_common._mutex_tx); + _z_mutex_drop(&ztu->_common._mutex_rx); #endif // Z_FEATURE_MULTI_THREAD == 1 - _z_wbuf_clear(&zt->_transport._unicast._wbuf); - _z_zbuf_clear(&zt->_transport._unicast._zbuf); + _z_wbuf_clear(&ztu->_common._wbuf); + _z_zbuf_clear(&ztu->_common._zbuf); } #if Z_FEATURE_FRAGMENTATION == 1 // Initialize the defragmentation buffers - zt->_transport._unicast._dbuf_reliable = _z_wbuf_make(dbuf_size, expandable); - zt->_transport._unicast._dbuf_best_effort = _z_wbuf_make(dbuf_size, expandable); - - // Clean up the buffers if one of them failed to be allocated - if ( -#if Z_FEATURE_DYNAMIC_MEMORY_ALLOCATION == 0 - (_z_wbuf_capacity(&zt->_transport._unicast._dbuf_reliable) != dbuf_size) || - (_z_wbuf_capacity(&zt->_transport._unicast._dbuf_best_effort) != dbuf_size)) { -#else - (_z_wbuf_capacity(&zt->_transport._unicast._dbuf_reliable) != Z_IOSLICE_SIZE) || - (_z_wbuf_capacity(&zt->_transport._unicast._dbuf_best_effort) != Z_IOSLICE_SIZE)) { -#endif - ret = _Z_ERR_SYSTEM_OUT_OF_MEMORY; - _Z_ERROR("Not enough memory to allocate transport defragmentation buffers!"); - - _z_wbuf_clear(&zt->_transport._unicast._dbuf_reliable); - _z_wbuf_clear(&zt->_transport._unicast._dbuf_best_effort); - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_drop(&zt->_transport._unicast._mutex_tx); - _z_mutex_drop(&zt->_transport._unicast._mutex_rx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - - _z_wbuf_clear(&zt->_transport._unicast._wbuf); - _z_zbuf_clear(&zt->_transport._unicast._zbuf); - } -#else - _ZP_UNUSED(dbuf_size); - _ZP_UNUSED(expandable); + ztu->_state_reliable = _Z_DBUF_STATE_NULL; + ztu->_state_best_effort = _Z_DBUF_STATE_NULL; + ztu->_dbuf_reliable = _z_wbuf_null(); + ztu->_dbuf_best_effort = _z_wbuf_null(); #endif // Z_FEATURE_FRAGMENTATION == 1 } if (ret == _Z_RES_OK) { // Set default SN resolution - zt->_transport._unicast._sn_res = _z_sn_max(param->_seq_num_res); + ztu->_common._sn_res = _z_sn_max(param->_seq_num_res); // The initial SN at TX side - zt->_transport._unicast._sn_tx_reliable = param->_initial_sn_tx; - zt->_transport._unicast._sn_tx_best_effort = param->_initial_sn_tx; + ztu->_common._sn_tx_reliable = param->_initial_sn_tx; + ztu->_common._sn_tx_best_effort = param->_initial_sn_tx; // The initial SN at RX side - _z_zint_t initial_sn_rx = _z_sn_decrement(zt->_transport._unicast._sn_res, param->_initial_sn_rx); - zt->_transport._unicast._sn_rx_reliable = initial_sn_rx; - zt->_transport._unicast._sn_rx_best_effort = initial_sn_rx; + _z_zint_t initial_sn_rx = _z_sn_decrement(ztu->_common._sn_res, param->_initial_sn_rx); + ztu->_sn_rx_reliable = initial_sn_rx; + ztu->_sn_rx_best_effort = initial_sn_rx; #if Z_FEATURE_MULTI_THREAD == 1 // Tasks - zt->_transport._unicast._read_task_running = false; - zt->_transport._unicast._read_task = NULL; - zt->_transport._unicast._lease_task_running = false; - zt->_transport._unicast._lease_task = NULL; + ztu->_common._read_task_running = false; + ztu->_common._read_task = NULL; + ztu->_common._lease_task_running = false; + ztu->_common._lease_task = NULL; #endif // Z_FEATURE_MULTI_THREAD == 1 // Notifiers - zt->_transport._unicast._received = 0; - zt->_transport._unicast._transmitted = 0; + ztu->_received = 0; + ztu->_common._transmitted = 0; // Transport lease - zt->_transport._unicast._lease = param->_lease; + ztu->_common._lease = param->_lease; // Transport link for unicast - zt->_transport._unicast._link = *zl; + ztu->_common._link = *zl; // Remote peer PID - zt->_transport._unicast._remote_zid = param->_remote_zid; + ztu->_remote_zid = param->_remote_zid; } else { param->_remote_zid = _z_id_empty(); } @@ -162,114 +137,205 @@ z_result_t _z_unicast_transport_create(_z_transport_t *zt, _z_link_t *zl, return ret; } -z_result_t _z_unicast_open_client(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, - const _z_id_t *local_zid) { - z_result_t ret = _Z_RES_OK; - - _z_id_t zid = *local_zid; - _z_transport_message_t ism = _z_t_msg_make_init_syn(Z_WHATAMI_CLIENT, zid); +static z_result_t _z_unicast_handshake_client(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid, enum z_whatami_t whatami) { + _z_transport_message_t ism = _z_t_msg_make_init_syn(whatami, *local_zid); param->_seq_num_res = ism._body._init._seq_num_res; // The announced sn resolution param->_req_id_res = ism._body._init._req_id_res; // The announced req id resolution param->_batch_size = ism._body._init._batch_size; // The announced batch size // Encode and send the message - _Z_INFO("Sending Z_INIT(Syn)"); - ret = _z_link_send_t_msg(zl, &ism); + _Z_DEBUG("Sending Z_INIT(Syn)"); + z_result_t ret = _z_link_send_t_msg(zl, &ism); _z_t_msg_clear(&ism); - if (ret == _Z_RES_OK) { - _z_transport_message_t iam; - ret = _z_link_recv_t_msg(&iam, zl); - if (ret == _Z_RES_OK) { - if ((_Z_MID(iam._header) == _Z_MID_T_INIT) && (_Z_HAS_FLAG(iam._header, _Z_FLAG_T_INIT_A) == true)) { - _Z_INFO("Received Z_INIT(Ack)"); - - // Any of the size parameters in the InitAck must be less or equal than the one in the InitSyn, - // otherwise the InitAck message is considered invalid and it should be treated as a - // CLOSE message with L==0 by the Initiating Peer -- the recipient of the InitAck message. - if (iam._body._init._seq_num_res <= param->_seq_num_res) { - param->_seq_num_res = iam._body._init._seq_num_res; - } else { - ret = _Z_ERR_TRANSPORT_OPEN_SN_RESOLUTION; - } - - if (iam._body._init._req_id_res <= param->_req_id_res) { - param->_req_id_res = iam._body._init._req_id_res; - } else { - ret = _Z_ERR_TRANSPORT_OPEN_SN_RESOLUTION; - } - - if (iam._body._init._batch_size <= param->_batch_size) { - param->_batch_size = iam._body._init._batch_size; - } else { - ret = _Z_ERR_TRANSPORT_OPEN_SN_RESOLUTION; - } + if (ret != _Z_RES_OK) { + return ret; + } + // Try to receive response + _z_transport_message_t iam; + _Z_RETURN_IF_ERR(_z_link_recv_t_msg(&iam, zl)); + if ((_Z_MID(iam._header) != _Z_MID_T_INIT) || !_Z_HAS_FLAG(iam._header, _Z_FLAG_T_INIT_A)) { + _z_t_msg_clear(&iam); + return _Z_ERR_MESSAGE_UNEXPECTED; + } + _Z_DEBUG("Received Z_INIT(Ack)"); + // Any of the size parameters in the InitAck must be less or equal than the one in the InitSyn, + // otherwise the InitAck message is considered invalid and it should be treated as a + // CLOSE message with L==0 by the Initiating Peer -- the recipient of the InitAck message. + if (iam._body._init._seq_num_res <= param->_seq_num_res) { + param->_seq_num_res = iam._body._init._seq_num_res; + } else { + ret = _Z_ERR_TRANSPORT_OPEN_SN_RESOLUTION; + } + if (iam._body._init._req_id_res <= param->_req_id_res) { + param->_req_id_res = iam._body._init._req_id_res; + } else { + ret = _Z_ERR_TRANSPORT_OPEN_SN_RESOLUTION; + } + if (iam._body._init._batch_size <= param->_batch_size) { + param->_batch_size = iam._body._init._batch_size; + } else { + ret = _Z_ERR_TRANSPORT_OPEN_SN_RESOLUTION; + } #if Z_FEATURE_FRAGMENTATION == 1 - if (iam._body._init._patch > ism._body._init._patch) { - // TODO: Use a better error code? - ret = _Z_ERR_GENERIC; - } - param->_patch = iam._body._init._patch; + if (iam._body._init._patch <= ism._body._init._patch) { + param->_patch = iam._body._init._patch; + } else { + // TODO: Use a better error code? + ret = _Z_ERR_GENERIC; + } #endif + if (ret != _Z_RES_OK) { + _z_t_msg_clear(&iam); + return ret; + } + param->_key_id_res = 0x08 << param->_key_id_res; + param->_req_id_res = 0x08 << param->_req_id_res; + + // The initial SN at TX side + z_random_fill(¶m->_initial_sn_tx, sizeof(param->_initial_sn_tx)); + param->_initial_sn_tx = param->_initial_sn_tx & !_z_sn_modulo_mask(param->_seq_num_res); + + // Initialize the Local and Remote Peer IDs + param->_remote_zid = iam._body._init._zid; + + // Create the OpenSyn message + _z_zint_t lease = Z_TRANSPORT_LEASE; + _z_zint_t initial_sn = param->_initial_sn_tx; + _z_slice_t cookie; + _z_slice_copy(&cookie, &iam._body._init._cookie); + _z_transport_message_t osm = _z_t_msg_make_open_syn(lease, initial_sn, cookie); + _z_t_msg_clear(&iam); + // Encode and send the message + _Z_DEBUG("Sending Z_OPEN(Syn)"); + ret = _z_link_send_t_msg(zl, &osm); + _z_t_msg_clear(&osm); + if (ret != _Z_RES_OK) { + return ret; + } + // Try to receive response + _z_transport_message_t oam; + _Z_RETURN_IF_ERR(_z_link_recv_t_msg(&oam, zl)); + if ((_Z_MID(oam._header) != _Z_MID_T_OPEN) || !_Z_HAS_FLAG(oam._header, _Z_FLAG_T_OPEN_A)) { + _z_t_msg_clear(&oam); + ret = _Z_ERR_MESSAGE_UNEXPECTED; + } + _Z_DEBUG("Received Z_OPEN(Ack)"); + param->_lease = oam._body._open._lease; // The session lease + // The initial SN at RX side. Initialize the session as we had already received + // a message with a SN equal to initial_sn - 1. + param->_initial_sn_rx = oam._body._open._initial_sn; + _z_t_msg_clear(&oam); + return _Z_RES_OK; +} - if (ret == _Z_RES_OK) { - param->_key_id_res = 0x08 << param->_key_id_res; - param->_req_id_res = 0x08 << param->_req_id_res; - - // The initial SN at TX side - z_random_fill(¶m->_initial_sn_tx, sizeof(param->_initial_sn_tx)); - param->_initial_sn_tx = param->_initial_sn_tx & !_z_sn_modulo_mask(param->_seq_num_res); - - // Initialize the Local and Remote Peer IDs - param->_remote_zid = iam._body._init._zid; - - // Create the OpenSyn message - _z_zint_t lease = Z_TRANSPORT_LEASE; - _z_zint_t initial_sn = param->_initial_sn_tx; - _z_slice_t cookie; - _z_slice_copy(&cookie, &iam._body._init._cookie); - - _z_transport_message_t osm = _z_t_msg_make_open_syn(lease, initial_sn, cookie); - - // Encode and send the message - _Z_INFO("Sending Z_OPEN(Syn)"); - ret = _z_link_send_t_msg(zl, &osm); - if (ret == _Z_RES_OK) { - _z_transport_message_t oam; - ret = _z_link_recv_t_msg(&oam, zl); - if (ret == _Z_RES_OK) { - if ((_Z_MID(oam._header) == _Z_MID_T_OPEN) && - (_Z_HAS_FLAG(oam._header, _Z_FLAG_T_OPEN_A) == true)) { - _Z_INFO("Received Z_OPEN(Ack)"); - param->_lease = oam._body._open._lease; // The session lease - - // The initial SN at RX side. Initialize the session as we had already received - // a message with a SN equal to initial_sn - 1. - param->_initial_sn_rx = oam._body._open._initial_sn; - } else { - ret = _Z_ERR_MESSAGE_UNEXPECTED; - } - _z_t_msg_clear(&oam); - } - } - _z_t_msg_clear(&osm); - } - } else { - ret = _Z_ERR_MESSAGE_UNEXPECTED; - } - _z_t_msg_clear(&iam); - } +// TODO: Activate if we add peer unicast support +#if 0 +static z_result_t _z_unicast_handshake_listener(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid, enum z_whatami_t whatami) { + // Read t message from link + _z_transport_message_t tmsg; + z_result_t ret = _z_link_recv_t_msg(&tmsg, zl); + if (ret != _Z_RES_OK) { + return ret; + } + // Receive InitSyn + if (_Z_MID(tmsg._header) != _Z_MID_T_INIT || _Z_HAS_FLAG(tmsg._header, _Z_FLAG_T_INIT_A)) { + _z_t_msg_clear(&tmsg); + return _Z_ERR_MESSAGE_UNEXPECTED; + } + _Z_DEBUG("Received Z_INIT(Syn)"); + // Encode InitAck + _z_slice_t cookie = _z_slice_null(); + _z_transport_message_t iam = _z_t_msg_make_init_ack(whatami, *local_zid, cookie); + // Any of the size parameters in the InitAck must be less or equal than the one in the InitSyn, + if (iam._body._init._seq_num_res > tmsg._body._init._seq_num_res) { + iam._body._init._seq_num_res = tmsg._body._init._seq_num_res; + } + if (iam._body._init._req_id_res > tmsg._body._init._req_id_res) { + iam._body._init._req_id_res = tmsg._body._init._req_id_res; + } + if (iam._body._init._batch_size > tmsg._body._init._batch_size) { + iam._body._init._batch_size = tmsg._body._init._batch_size; } +#if Z_FEATURE_FRAGMENTATION == 1 + if (iam._body._init._patch > tmsg._body._init._patch) { + iam._body._init._patch = tmsg._body._init._patch; + } +#endif + param->_remote_zid = tmsg._body._init._zid; + param->_seq_num_res = iam._body._init._seq_num_res; + param->_req_id_res = iam._body._init._req_id_res; + param->_batch_size = iam._body._init._batch_size; + param->_key_id_res = 0x08 << param->_key_id_res; + param->_req_id_res = 0x08 << param->_req_id_res; + _z_t_msg_clear(&tmsg); + // Send InitAck + _Z_DEBUG("Sending Z_INIT(Ack)"); + ret = _z_link_send_t_msg(zl, &iam); + _z_t_msg_clear(&iam); + if (ret != _Z_RES_OK) { + return ret; + } + // Read t message from link + ret = _z_link_recv_t_msg(&tmsg, zl); + if (ret != _Z_RES_OK) { + return ret; + } + // Receive OpenSyn + if (_Z_MID(tmsg._header) != _Z_MID_T_OPEN || _Z_HAS_FLAG(tmsg._header, _Z_FLAG_T_INIT_A)) { + _z_t_msg_clear(&tmsg); + return _Z_ERR_MESSAGE_UNEXPECTED; + } + _Z_DEBUG("Received Z_OPEN(Syn)"); + // Process message + param->_lease = tmsg._body._open._lease; + param->_initial_sn_rx = tmsg._body._open._initial_sn; + _z_t_msg_clear(&tmsg); - return ret; -} + // Init sn, tx side + z_random_fill(¶m->_initial_sn_tx, sizeof(param->_initial_sn_tx)); + param->_initial_sn_tx = param->_initial_sn_tx & !_z_sn_modulo_mask(param->_seq_num_res); -z_result_t _z_unicast_open_peer(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, - const _z_id_t *local_zid) { + // Encode OpenAck + _z_zint_t lease = Z_TRANSPORT_LEASE; + _z_zint_t initial_sn = param->_initial_sn_tx; + _z_transport_message_t oam = _z_t_msg_make_open_ack(lease, initial_sn); + + // Encode and send the message + _Z_DEBUG("Sending Z_OPEN(Ack)"); + ret = _z_link_send_t_msg(zl, &oam); + _z_t_msg_clear(&oam); + if (ret != _Z_RES_OK) { + return ret; + } + // Handshake finished + return _Z_RES_OK; +} +#else +static z_result_t _z_unicast_handshake_listener(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid, enum z_whatami_t whatami) { _ZP_UNUSED(param); _ZP_UNUSED(zl); _ZP_UNUSED(local_zid); - z_result_t ret = _Z_ERR_CONFIG_UNSUPPORTED_PEER_UNICAST; - // @TODO: not implemented + _ZP_UNUSED(whatami); + return _Z_ERR_TRANSPORT_OPEN_FAILED; +} +#endif + +z_result_t _z_unicast_open_client(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid) { + return _z_unicast_handshake_client(param, zl, local_zid, Z_WHATAMI_CLIENT); +} + +z_result_t _z_unicast_open_peer(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, + const _z_id_t *local_zid, int peer_op) { + z_result_t ret = _Z_RES_OK; + if (peer_op == _Z_PEER_OP_OPEN) { + ret = _z_unicast_handshake_client(param, zl, local_zid, Z_WHATAMI_PEER); + } else { + ret = _z_unicast_handshake_listener(param, zl, local_zid, Z_WHATAMI_PEER); + } return ret; } @@ -277,7 +343,7 @@ z_result_t _z_unicast_send_close(_z_transport_unicast_t *ztu, uint8_t reason, bo z_result_t ret = _Z_RES_OK; // Send and clear message _z_transport_message_t cm = _z_t_msg_make_close(reason, link_only); - ret = _z_unicast_send_t_msg(ztu, &cm); + ret = _z_transport_tx_send_t_msg(&ztu->_common, &cm); _z_t_msg_clear(&cm); return ret; } @@ -290,23 +356,25 @@ void _z_unicast_transport_clear(_z_transport_t *zt) { _z_transport_unicast_t *ztu = &zt->_transport._unicast; #if Z_FEATURE_MULTI_THREAD == 1 // Clean up tasks - if (ztu->_read_task != NULL) { - _z_task_join(ztu->_read_task); - z_free(ztu->_read_task); + if (ztu->_common._read_task != NULL) { + _z_task_join(ztu->_common._read_task); + z_free(ztu->_common._read_task); } - if (ztu->_lease_task != NULL) { - _z_task_join(ztu->_lease_task); - z_free(ztu->_lease_task); + if (ztu->_common._lease_task != NULL) { + _z_task_join(ztu->_common._lease_task); + z_free(ztu->_common._lease_task); } // Clean up the mutexes - _z_mutex_drop(&ztu->_mutex_tx); - _z_mutex_drop(&ztu->_mutex_rx); + _z_mutex_drop(&ztu->_common._mutex_tx); + _z_mutex_drop(&ztu->_common._mutex_rx); #endif // Z_FEATURE_MULTI_THREAD == 1 // Clean up the buffers - _z_wbuf_clear(&ztu->_wbuf); - _z_zbuf_clear(&ztu->_zbuf); + _z_wbuf_clear(&ztu->_common._wbuf); + _z_zbuf_clear(&ztu->_common._zbuf); + _z_arc_slice_svec_release(&ztu->_common._arc_pool); + _z_network_message_svec_release(&ztu->_common._msg_pool); #if Z_FEATURE_FRAGMENTATION == 1 _z_wbuf_clear(&ztu->_dbuf_reliable); _z_wbuf_clear(&ztu->_dbuf_best_effort); @@ -314,7 +382,7 @@ void _z_unicast_transport_clear(_z_transport_t *zt) { // Clean up PIDs ztu->_remote_zid = _z_id_empty(); - _z_link_clear(&ztu->_link); + _z_link_clear(&ztu->_common._link); } #else @@ -336,10 +404,11 @@ z_result_t _z_unicast_open_client(_z_transport_unicast_establish_param_t *param, } z_result_t _z_unicast_open_peer(_z_transport_unicast_establish_param_t *param, const _z_link_t *zl, - const _z_id_t *local_zid) { + const _z_id_t *local_zid, int peer_op) { _ZP_UNUSED(param); _ZP_UNUSED(zl); _ZP_UNUSED(local_zid); + _ZP_UNUSED(peer_op); return _Z_ERR_TRANSPORT_NOT_AVAILABLE; } diff --git a/src/transport/unicast/tx.c b/src/transport/unicast/tx.c deleted file mode 100644 index 561c232c7..000000000 --- a/src/transport/unicast/tx.c +++ /dev/null @@ -1,189 +0,0 @@ -// -// 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, -// - -#include "zenoh-pico/transport/common/tx.h" - -#include - -#include "zenoh-pico/config.h" -#include "zenoh-pico/protocol/codec/network.h" -#include "zenoh-pico/protocol/codec/transport.h" -#include "zenoh-pico/protocol/iobuf.h" -#include "zenoh-pico/transport/unicast/tx.h" -#include "zenoh-pico/transport/utils.h" -#include "zenoh-pico/utils/logging.h" - -#if Z_FEATURE_UNICAST_TRANSPORT == 1 - -/** - * This function is unsafe because it operates in potentially concurrent data. - * Make sure that the following mutexes are locked before calling this function: - * - ztu->_mutex_inner - */ -_z_zint_t __unsafe_z_unicast_get_sn(_z_transport_unicast_t *ztu, z_reliability_t reliability) { - _z_zint_t sn; - if (reliability == Z_RELIABILITY_RELIABLE) { - sn = ztu->_sn_tx_reliable; - ztu->_sn_tx_reliable = _z_sn_increment(ztu->_sn_res, ztu->_sn_tx_reliable); - } else { - sn = ztu->_sn_tx_best_effort; - ztu->_sn_tx_best_effort = _z_sn_increment(ztu->_sn_res, ztu->_sn_tx_best_effort); - } - return sn; -} - -z_result_t _z_unicast_send_t_msg(_z_transport_unicast_t *ztu, const _z_transport_message_t *t_msg) { - z_result_t ret = _Z_RES_OK; - _Z_DEBUG(">> send session message"); - -#if Z_FEATURE_MULTI_THREAD == 1 - // Acquire the lock - _z_mutex_lock(&ztu->_mutex_tx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - - // Prepare the buffer eventually reserving space for the message length - __unsafe_z_prepare_wbuf(&ztu->_wbuf, ztu->_link._cap._flow); - - // Encode the session message - ret = _z_transport_message_encode(&ztu->_wbuf, t_msg); - if (ret == _Z_RES_OK) { - // Write the message length in the reserved space if needed - __unsafe_z_finalize_wbuf(&ztu->_wbuf, ztu->_link._cap._flow); - // Send the wbuf on the socket - ret = _z_link_send_wbuf(&ztu->_link, &ztu->_wbuf); - if (ret == _Z_RES_OK) { - ztu->_transmitted = true; // Mark the session that we have transmitted data - } - } - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztu->_mutex_tx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - - return ret; -} - -z_result_t _z_unicast_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl) { - z_result_t ret = _Z_RES_OK; - _Z_DEBUG(">> send network message"); - - _z_transport_unicast_t *ztu = &zn->_tp._transport._unicast; - - // Acquire the lock and drop the message if needed - bool drop = false; - if (cong_ctrl == Z_CONGESTION_CONTROL_BLOCK) { -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_lock(&ztu->_mutex_tx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - } else { -#if Z_FEATURE_MULTI_THREAD == 1 - z_result_t locked = _z_mutex_try_lock(&ztu->_mutex_tx); - if (locked != 0) { - _Z_INFO("Dropping zenoh message because of congestion control"); - // We failed to acquire the lock, drop the message - drop = true; - } -#endif // Z_FEATURE_MULTI_THREAD == 1 - } - - if (drop == false) { - // Prepare the buffer eventually reserving space for the message length - __unsafe_z_prepare_wbuf(&ztu->_wbuf, ztu->_link._cap._flow); - - _z_zint_t sn = __unsafe_z_unicast_get_sn(ztu, reliability); // Get the next sequence number - - _z_transport_message_t t_msg = _z_t_msg_make_frame_header(sn, reliability); - ret = _z_transport_message_encode(&ztu->_wbuf, &t_msg); // Encode the frame header - if (ret == _Z_RES_OK) { - ret = _z_network_message_encode(&ztu->_wbuf, n_msg); // Encode the network message - if (ret == _Z_RES_OK) { - // Write the message length in the reserved space if needed - __unsafe_z_finalize_wbuf(&ztu->_wbuf, ztu->_link._cap._flow); - - if (ztu->_wbuf._ioss._len == 1) { - ret = _z_link_send_wbuf(&ztu->_link, &ztu->_wbuf); // Send the wbuf on the socket - } else { - // Change the MID - - // for () - } - if (ret == _Z_RES_OK) { - ztu->_transmitted = true; // Mark the session that we have transmitted data - } - } else { -#if Z_FEATURE_FRAGMENTATION == 1 - // The message does not fit in the current batch, let's fragment it - // Create an expandable wbuf for fragmentation - _z_wbuf_t fbf = _z_wbuf_make(_Z_FRAG_BUFF_BASE_SIZE, true); - - ret = _z_network_message_encode(&fbf, n_msg); // Encode the message on the expandable wbuf - if (ret == _Z_RES_OK) { - bool is_first = true; // Fragment and send the message - while (_z_wbuf_len(&fbf) > 0) { - if (is_first == false) { // Get the fragment sequence number - sn = __unsafe_z_unicast_get_sn(ztu, reliability); - } - - // Clear the buffer for serialization - __unsafe_z_prepare_wbuf(&ztu->_wbuf, ztu->_link._cap._flow); - - // Serialize one fragment - ret = __unsafe_z_serialize_zenoh_fragment(&ztu->_wbuf, &fbf, reliability, sn, is_first); - if (ret == _Z_RES_OK) { - // Write the message length in the reserved space if needed - __unsafe_z_finalize_wbuf(&ztu->_wbuf, ztu->_link._cap._flow); - - ret = _z_link_send_wbuf(&ztu->_link, &ztu->_wbuf); // Send the wbuf on the socket - if (ret == _Z_RES_OK) { - ztu->_transmitted = true; // Mark the session that we have transmitted data - } - } else { - _Z_ERROR("Fragment serialization failed with err %d", ret); - } - is_first = false; - } - } - - // Clear the buffer as it's no longer required - _z_wbuf_clear(&fbf); -#else - _Z_INFO("Sending the message required fragmentation feature that is deactivated."); -#endif - } - } - -#if Z_FEATURE_MULTI_THREAD == 1 - _z_mutex_unlock(&ztu->_mutex_tx); -#endif // Z_FEATURE_MULTI_THREAD == 1 - } - - return ret; -} -#else -z_result_t _z_unicast_send_t_msg(_z_transport_unicast_t *ztu, const _z_transport_message_t *t_msg) { - _ZP_UNUSED(ztu); - _ZP_UNUSED(t_msg); - return _Z_ERR_TRANSPORT_NOT_AVAILABLE; -} - -z_result_t _z_unicast_send_n_msg(_z_session_t *zn, const _z_network_message_t *n_msg, z_reliability_t reliability, - z_congestion_control_t cong_ctrl) { - _ZP_UNUSED(zn); - _ZP_UNUSED(n_msg); - _ZP_UNUSED(reliability); - _ZP_UNUSED(cong_ctrl); - return _Z_ERR_TRANSPORT_NOT_AVAILABLE; -} -#endif // Z_FEATURE_UNICAST_TRANSPORT == 1 diff --git a/tests/modularity.py b/tests/modularity.py index 7350ae70a..be18d86c1 100644 --- a/tests/modularity.py +++ b/tests/modularity.py @@ -47,8 +47,8 @@ def pub_and_sub(args): # Expected z_sub output & status if args.sub == 1: - z_sub_expected_status = -2 if args.pub == 1: + z_sub_expected_status = [0, -2] z_sub_expected_output = """Opening session... Declaring Subscriber on 'demo/example/**'... Press CTRL-C to quit... @@ -63,16 +63,17 @@ def pub_and_sub(args): >> [Subscriber] Received ('demo/example/zenoh-pico-pub': '[ 8] Pub from Pico!') >> [Subscriber] Received ('demo/example/zenoh-pico-pub': '[ 9] Pub from Pico!')""" else: + z_sub_expected_status = [-2] z_sub_expected_output = """Opening session... Declaring Subscriber on 'demo/example/**'... Press CTRL-C to quit...""" else: - z_sub_expected_status = 254 + z_sub_expected_status = [254] z_sub_expected_output = "ERROR: Zenoh pico was compiled without " "Z_FEATURE_SUBSCRIPTION but this example requires it." print("Start subscriber") # Start z_sub in the background - z_sub_command = f"stdbuf -oL -eL ./{DIR_EXAMPLES}/z_sub" + z_sub_command = f"stdbuf -oL -eL ./{DIR_EXAMPLES}/z_sub -n 10" z_sub_process = subprocess.Popen( z_sub_command, shell=True, @@ -132,7 +133,7 @@ def pub_and_sub(args): print("Check subscriber status & output") # Check the exit status of z_sub z_sub_status = z_sub_process.returncode - if z_sub_status == z_sub_expected_status: + if z_sub_status in z_sub_expected_status: print("z_sub status valid") else: print(f"z_sub status invalid, expected: {z_sub_expected_status}, received: {z_sub_status}") @@ -175,24 +176,25 @@ def query_and_queryable(args): # Expected z_queryable output & status if args.queryable == 1: - z_queryable_expected_status = -2 if args.query == 1: + z_queryable_expected_status = [0, -2] z_queryable_expected_output = """Opening session... Creating Queryable on 'demo/example/zenoh-pico-queryable'... Press CTRL-C to quit... >> [Queryable handler] Received Query 'demo/example/**' """ else: + z_queryable_expected_status = [-2] z_queryable_expected_output = """Opening session... Creating Queryable on 'demo/example/zenoh-pico-queryable'... Press CTRL-C to quit...""" else: - z_queryable_expected_status = 254 + z_queryable_expected_status = [254] z_queryable_expected_output = "ERROR: Zenoh pico was compiled without " "Z_FEATURE_QUERYABLE but this example requires it." print("Start queryable") # Start z_queryable in the background - z_queryable_command = f"stdbuf -oL -eL ./{DIR_EXAMPLES}/z_queryable" + z_queryable_command = f"stdbuf -oL -eL ./{DIR_EXAMPLES}/z_queryable -n 1" z_queryable_process = subprocess.Popen( z_queryable_command, shell=True, @@ -252,7 +254,7 @@ def query_and_queryable(args): print("Check queryable status & output") # Check the exit status of z_queryable z_queryable_status = z_queryable_process.returncode - if z_queryable_status == z_queryable_expected_status: + if z_queryable_status in z_queryable_expected_status: print("z_queryable status valid") else: print(f"z_queryable status invalid, expected: {z_queryable_expected_status}," f" received: {z_queryable_status}") diff --git a/tests/z_client_test.c b/tests/z_client_test.c index ca230002c..559505de5 100644 --- a/tests/z_client_test.c +++ b/tests/z_client_test.c @@ -134,7 +134,7 @@ int main(int argc, char **argv) { z_owned_session_t s1; assert(z_open(&s1, z_move(config), NULL) == Z_OK); _z_string_t zid1 = _z_id_to_string(&(_Z_RC_IN_VAL(z_loan(s1))->_local_zid)); - printf("Session 1 with PID: %*.s\n", (int)_z_string_len(&zid1), _z_string_data(&zid1)); + printf("Session 1 with PID: %.*s\n", (int)_z_string_len(&zid1), _z_string_data(&zid1)); _z_string_clear(&zid1); // Start the read session session lease loops @@ -150,7 +150,7 @@ int main(int argc, char **argv) { assert(z_open(&s2, z_move(config), NULL) == Z_OK); assert(z_internal_check(s2)); _z_string_t zid2 = _z_id_to_string(&(_Z_RC_IN_VAL(z_loan(s2))->_local_zid)); - printf("Session 2 with PID: %*.s\n", (int)_z_string_len(&zid2), _z_string_data(&zid2)); + printf("Session 2 with PID: %.*s\n", (int)_z_string_len(&zid2), _z_string_data(&zid2)); _z_string_clear(&zid2); // Start the read session session lease loops diff --git a/tests/z_msgcodec_test.c b/tests/z_msgcodec_test.c index 1c227d194..cb7b9a927 100644 --- a/tests/z_msgcodec_test.c +++ b/tests/z_msgcodec_test.c @@ -172,7 +172,7 @@ _z_wbuf_t gen_wbuf(size_t len) { _z_slice_t gen_slice(size_t len) { if (len == 0) { - return _z_slice_empty(); + return _z_slice_null(); } uint8_t *p = (uint8_t *)z_malloc(sizeof(uint8_t) * len); @@ -232,7 +232,7 @@ _z_string_svec_t gen_str_array(size_t size) { _z_string_svec_t sa = _z_string_svec_make(size); for (size_t i = 0; i < size; i++) { _z_string_t s = gen_string(16); - _z_string_svec_append(&sa, &s); + _z_string_svec_append(&sa, &s, true); } return sa; @@ -559,8 +559,9 @@ void payload_field(void) { // Decode _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); - _z_bytes_t d_pld; - res = _z_bytes_decode(&d_pld, &zbf); + _z_bytes_t d_pld = _z_bytes_null(); + _z_arc_slice_t arcs = {0}; + res = _z_bytes_decode(&d_pld, &zbf, &arcs); assert(res == _Z_RES_OK); printf(" "); assert_eq_bytes(&e_pld, &d_pld); @@ -594,6 +595,7 @@ void assert_eq_value(const _z_value_t *left, const _z_value_t *right) { /*------------------ Timestamp field ------------------*/ _z_timestamp_t gen_timestamp(void) { _z_timestamp_t ts; + ts.valid = true; ts.time = gen_uint64(); for (size_t i = 0; i < 16; i++) { ts.id.id[i] = gen_uint8() & 0x7f; // 0b01111111 @@ -1167,7 +1169,8 @@ void declare_message(void) { // Decode _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); _z_network_message_t d_dcl = {0}; - res = _z_network_message_decode(&d_dcl, &zbf); + _z_arc_slice_t arcs = {0}; + res = _z_network_message_decode(&d_dcl, &zbf, &arcs); assert(res == _Z_RES_OK); assert_eq_declare_message(&n_msg._body._declare, &d_dcl._body._declare); @@ -1239,7 +1242,7 @@ void interest_message(void) { // Encode assert(_z_n_interest_encode(&wbf, &expected._body._interest) == _Z_RES_OK); // Decode - _z_n_msg_interest_t decoded; + _z_n_msg_interest_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); uint8_t header = _z_zbuf_read(&zbf); assert(_z_n_interest_decode(&decoded, &zbf, header) == _Z_RES_OK); @@ -1301,8 +1304,9 @@ void push_body_message(void) { // Decode _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); _z_push_body_t d_da = {0}; + _z_arc_slice_t arcs = {0}; uint8_t header = _z_zbuf_read(&zbf); - res = _z_push_body_decode(&d_da, &zbf, header); + res = _z_push_body_decode(&d_da, &zbf, header, &arcs); assert(res == _Z_RES_OK); assert_eq_push_body(&e_da, &d_da); @@ -1335,7 +1339,7 @@ void query_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_msg_query_t expected = gen_query(); assert(_z_query_encode(&wbf, &expected) == _Z_RES_OK); - _z_msg_query_t decoded; + _z_msg_query_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); uint8_t header = _z_zbuf_read(&zbf); z_result_t res = _z_query_decode(&decoded, &zbf, header); @@ -1367,10 +1371,11 @@ void err_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_msg_err_t expected = gen_err(); assert(_z_err_encode(&wbf, &expected) == _Z_RES_OK); - _z_msg_err_t decoded; + _z_msg_err_t decoded = {0}; + _z_arc_slice_t arcs = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); uint8_t header = _z_zbuf_read(&zbf); - assert(_Z_RES_OK == _z_err_decode(&decoded, &zbf, header)); + assert(_Z_RES_OK == _z_err_decode(&decoded, &zbf, header, &arcs)); assert_eq_err(&expected, &decoded); _z_msg_err_clear(&decoded); _z_msg_err_clear(&expected); @@ -1395,10 +1400,11 @@ void reply_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_msg_reply_t expected = gen_reply(); assert(_z_reply_encode(&wbf, &expected) == _Z_RES_OK); - _z_msg_reply_t decoded; + _z_msg_reply_t decoded = {0}; + _z_arc_slice_t arcs = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); uint8_t header = _z_zbuf_read(&zbf); - assert(_Z_RES_OK == _z_reply_decode(&decoded, &zbf, header)); + assert(_Z_RES_OK == _z_reply_decode(&decoded, &zbf, header, &arcs)); assert_eq_reply(&expected, &decoded); _z_msg_reply_clear(&decoded); _z_msg_reply_clear(&expected); @@ -1427,10 +1433,11 @@ void push_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_n_msg_push_t expected = gen_push(); assert(_z_push_encode(&wbf, &expected) == _Z_RES_OK); - _z_n_msg_push_t decoded; + _z_n_msg_push_t decoded = {0}; + _z_arc_slice_t arcs = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); uint8_t header = _z_zbuf_read(&zbf); - assert(_Z_RES_OK == _z_push_decode(&decoded, &zbf, header)); + assert(_Z_RES_OK == _z_push_decode(&decoded, &zbf, header, &arcs)); assert_eq_push(&expected, &decoded); _z_n_msg_push_clear(&decoded); _z_n_msg_push_clear(&expected); @@ -1439,10 +1446,11 @@ void push_message(void) { } _z_n_msg_request_t gen_request(void) { + _z_qos_t qos_default = {._val = 5}; _z_n_msg_request_t request = { ._rid = gen_uint64(), ._key = gen_keyexpr(), - ._ext_qos = gen_bool() ? _z_n_qos_make(gen_bool(), gen_bool(), gen_uint8() % 8) : _Z_N_QOS_DEFAULT, + ._ext_qos = gen_bool() ? _z_n_qos_make(gen_bool(), gen_bool(), gen_uint8() % 8) : qos_default, ._ext_timestamp = gen_bool() ? gen_timestamp() : _z_timestamp_null(), ._ext_target = gen_uint8() % 3, ._ext_budget = gen_bool() ? (uint32_t)gen_uint64() : 0, @@ -1497,10 +1505,11 @@ void request_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_n_msg_request_t expected = gen_request(); assert(_z_request_encode(&wbf, &expected) == _Z_RES_OK); - _z_n_msg_request_t decoded; + _z_n_msg_request_t decoded = {0}; + _z_arc_slice_t arcs = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); uint8_t header = _z_zbuf_read(&zbf); - z_result_t ret = _z_request_decode(&decoded, &zbf, header); + z_result_t ret = _z_request_decode(&decoded, &zbf, header, &arcs); assert(_Z_RES_OK == ret); assert_eq_request(&expected, &decoded); _z_n_msg_request_clear(&decoded); @@ -1555,10 +1564,11 @@ void response_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_n_msg_response_t expected = gen_response(); assert(_z_response_encode(&wbf, &expected) == _Z_RES_OK); - _z_n_msg_response_t decoded; + _z_n_msg_response_t decoded = {0}; + _z_arc_slice_t arcs = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); uint8_t header = _z_zbuf_read(&zbf); - z_result_t ret = _z_response_decode(&decoded, &zbf, header); + z_result_t ret = _z_response_decode(&decoded, &zbf, header, &arcs); assert(_Z_RES_OK == ret); assert_eq_response(&expected, &decoded); _z_n_msg_response_clear(&decoded); @@ -1576,7 +1586,7 @@ void response_final_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_n_msg_response_final_t expected = gen_response_final(); assert(_z_response_final_encode(&wbf, &expected) == _Z_RES_OK); - _z_n_msg_response_final_t decoded; + _z_n_msg_response_final_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); uint8_t header = _z_zbuf_read(&zbf); z_result_t ret = _z_response_final_decode(&decoded, &zbf, header); @@ -1625,7 +1635,7 @@ void join_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_transport_message_t expected = gen_join(); assert(_z_join_encode(&wbf, expected._header, &expected._body._join) == _Z_RES_OK); - _z_t_msg_join_t decoded; + _z_t_msg_join_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); z_result_t ret = _z_join_decode(&decoded, &zbf, expected._header); assert(_Z_RES_OK == ret); @@ -1657,7 +1667,7 @@ void init_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_transport_message_t expected = gen_init(); assert(_z_init_encode(&wbf, expected._header, &expected._body._init) == _Z_RES_OK); - _z_t_msg_init_t decoded; + _z_t_msg_init_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); z_result_t ret = _z_init_decode(&decoded, &zbf, expected._header); assert(_Z_RES_OK == ret); @@ -1685,7 +1695,7 @@ void open_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_transport_message_t expected = gen_open(); assert(_z_open_encode(&wbf, expected._header, &expected._body._open) == _Z_RES_OK); - _z_t_msg_open_t decoded; + _z_t_msg_open_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); z_result_t ret = _z_open_decode(&decoded, &zbf, expected._header); assert(_Z_RES_OK == ret); @@ -1705,7 +1715,7 @@ void close_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_transport_message_t expected = gen_close(); assert(_z_close_encode(&wbf, expected._header, &expected._body._close) == _Z_RES_OK); - _z_t_msg_close_t decoded; + _z_t_msg_close_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); z_result_t ret = _z_close_decode(&decoded, &zbf, expected._header); assert(_Z_RES_OK == ret); @@ -1726,7 +1736,7 @@ void keep_alive_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_transport_message_t expected = gen_keep_alive(); assert(_z_keep_alive_encode(&wbf, expected._header, &expected._body._keep_alive) == _Z_RES_OK); - _z_t_msg_keep_alive_t decoded; + _z_t_msg_keep_alive_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); z_result_t ret = _z_keep_alive_decode(&decoded, &zbf, expected._header); assert(_Z_RES_OK == ret); @@ -1787,13 +1797,11 @@ void assert_eq_net_msg(const _z_network_message_t *left, const _z_network_messag break; } } -_z_network_message_vec_t gen_net_msgs(size_t n) { - _z_network_message_vec_t ret = _z_network_message_vec_make(n); +_z_network_message_svec_t gen_net_msgs(size_t n) { + _z_network_message_svec_t ret = _z_network_message_svec_make(n); for (size_t i = 0; i < n; i++) { - _z_network_message_t *msg = (_z_network_message_t *)z_malloc(sizeof(_z_network_message_t)); - memset(msg, 0, sizeof(_z_network_message_t)); - *msg = gen_net_msg(); - _z_network_message_vec_append(&ret, msg); + _z_network_message_t msg = gen_net_msg(); + _z_network_message_svec_append(&ret, &msg, false); } return ret; } @@ -1805,7 +1813,8 @@ void assert_eq_frame(const _z_t_msg_frame_t *left, const _z_t_msg_frame_t *right assert(left->_sn == right->_sn); assert(left->_messages._len == right->_messages._len); for (size_t i = 0; i < left->_messages._len; i++) { - assert_eq_net_msg(left->_messages._val[i], right->_messages._val[i]); + assert_eq_net_msg(_z_network_message_svec_get(&left->_messages, i), + _z_network_message_svec_get(&right->_messages, i)); } } void frame_message(void) { @@ -1813,11 +1822,15 @@ void frame_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_transport_message_t expected = gen_frame(); assert(_z_frame_encode(&wbf, expected._header, &expected._body._frame) == _Z_RES_OK); - _z_t_msg_frame_t decoded; + _z_t_msg_frame_t decoded = {0}; + _z_arc_slice_svec_t arcs = _z_arc_slice_svec_make(1); + _z_network_message_svec_t msg = _z_network_message_svec_make(1); _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); - z_result_t ret = _z_frame_decode(&decoded, &zbf, expected._header); + z_result_t ret = _z_frame_decode(&decoded, &zbf, expected._header, &arcs, &msg); assert(_Z_RES_OK == ret); assert_eq_frame(&expected._body._frame, &decoded); + _z_network_message_svec_clear(&msg); + _z_arc_slice_svec_release(&arcs); _z_t_msg_frame_clear(&decoded); _z_t_msg_clear(&expected); _z_zbuf_clear(&zbf); @@ -1836,7 +1849,7 @@ void fragment_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_transport_message_t expected = gen_fragment(); assert(_z_fragment_encode(&wbf, expected._header, &expected._body._fragment) == _Z_RES_OK); - _z_t_msg_fragment_t decoded; + _z_t_msg_fragment_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); z_result_t ret = _z_fragment_decode(&decoded, &zbf, expected._header); assert(_Z_RES_OK == ret); @@ -1906,11 +1919,15 @@ void transport_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_transport_message_t expected = gen_transport(); assert(_z_transport_message_encode(&wbf, &expected) == _Z_RES_OK); - _z_transport_message_t decoded; + _z_transport_message_t decoded = {0}; + _z_arc_slice_svec_t arcs = _z_arc_slice_svec_make(1); + _z_network_message_svec_t msg = _z_network_message_svec_make(1); _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); - z_result_t ret = _z_transport_message_decode(&decoded, &zbf); + z_result_t ret = _z_transport_message_decode(&decoded, &zbf, &arcs, &msg); assert(_Z_RES_OK == ret); assert_eq_transport(&expected, &decoded); + _z_network_message_svec_clear(&msg); + _z_arc_slice_svec_release(&arcs); _z_t_msg_clear(&decoded); _z_t_msg_clear(&expected); _z_zbuf_clear(&zbf); @@ -1948,7 +1965,7 @@ void scouting_message(void) { _z_wbuf_t wbf = gen_wbuf(UINT16_MAX); _z_scouting_message_t expected = gen_scouting(); assert(_z_scouting_message_encode(&wbf, &expected) == _Z_RES_OK); - _z_scouting_message_t decoded; + _z_scouting_message_t decoded = {0}; _z_zbuf_t zbf = _z_wbuf_to_zbuf(&wbf); z_result_t ret = _z_scouting_message_decode(&decoded, &zbf); assert(_Z_RES_OK == ret); diff --git a/tests/z_peer_multicast_test.c b/tests/z_peer_multicast_test.c index d558eccc8..7d350eeb5 100644 --- a/tests/z_peer_multicast_test.c +++ b/tests/z_peer_multicast_test.c @@ -78,7 +78,7 @@ int main(int argc, char **argv) { z_owned_session_t s1; assert(z_open(&s1, z_move(config), NULL) == Z_OK); _z_string_t zid1 = _z_id_to_string(&(_Z_RC_IN_VAL(z_loan(s1))->_local_zid)); - printf("Session 1 with PID: %*.s\n", (int)_z_string_len(&zid1), _z_string_data(&zid1)); + printf("Session 1 with PID: %.*s\n", (int)_z_string_len(&zid1), _z_string_data(&zid1)); _z_string_clear(&zid1); // Start the read session session lease loops @@ -89,13 +89,13 @@ int main(int argc, char **argv) { z_config_default(&config); zp_config_insert(z_loan_mut(config), Z_CONFIG_MODE_KEY, "peer"); - zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, argv[1]); + zp_config_insert(z_loan_mut(config), Z_CONFIG_LISTEN_KEY, argv[1]); z_owned_session_t s2; assert(z_open(&s2, z_move(config), NULL) == Z_OK); _z_string_t zid2 = _z_id_to_string(&(_Z_RC_IN_VAL(z_loan(s2))->_local_zid)); - printf("Session 2 with PID: %*.s\n", (int)_z_string_len(&zid2), _z_string_data(&zid2)); + printf("Session 2 with PID: %.*s\n", (int)_z_string_len(&zid2), _z_string_data(&zid2)); _z_string_clear(&zid2); // Start the read session session lease loops diff --git a/tests/z_refcount_test.c b/tests/z_refcount_test.c index fc8b322cc..ded735f02 100644 --- a/tests/z_refcount_test.c +++ b/tests/z_refcount_test.c @@ -36,6 +36,11 @@ void _dummy_clear(_dummy_t *val) { _Z_REFCOUNT_DEFINE(_dummy, _dummy) +typedef struct { + unsigned int _strong_cnt; + unsigned int _weak_cnt; +} _dummy_inner_rc_t; + void test_rc_null(void) { _dummy_rc_t drc = _dummy_rc_null(); assert(drc._cnt == NULL); @@ -231,9 +236,9 @@ void test_overflow(void) { _dummy_t val = {.foo = 42}; _dummy_rc_t drc1 = _dummy_rc_new_from_val(&val); // Artificially set weak count to max value - for (size_t i = 0; i < INT32_MAX; i++) { - _z_rc_increase_strong(drc1._cnt); - } + _dummy_inner_rc_t *dcnt = (_dummy_inner_rc_t *)drc1._cnt; + dcnt->_strong_cnt = INT32_MAX; + dcnt->_weak_cnt = INT32_MAX; _dummy_rc_t drc2 = _dummy_rc_clone(&drc1); assert(_Z_RC_IS_NULL(&drc2)); @@ -255,6 +260,107 @@ void test_decr(void) { free(drc1._val); } +_Z_SIMPLE_REFCOUNT_DEFINE(_dummy, _dummy) + +void test_simple_rc_null(void) { + _dummy_simple_rc_t drc = _dummy_simple_rc_null(); + assert(drc._cnt == NULL); + assert(drc._val == NULL); +} + +void test_simple_rc_size(void) { assert(_dummy_simple_rc_size(NULL) == sizeof(_dummy_simple_rc_t)); } + +void test_simple_rc_drop(void) { + _dummy_simple_rc_t drc = _dummy_simple_rc_null(); + assert(!_dummy_simple_rc_drop(NULL)); + assert(!_dummy_simple_rc_drop(&drc)); +} + +void test_simple_rc_new(void) { + _dummy_t *val = z_malloc(sizeof(_dummy_t)); + val->foo = 42; + _dummy_simple_rc_t drc = _dummy_simple_rc_new(val); + assert(!_Z_RC_IS_NULL(&drc)); + assert(_z_simple_rc_strong_count(drc._cnt) == 1); + assert(drc._val->foo == 42); + drc._val->foo = 0; + assert(val->foo == 0); + assert(_dummy_simple_rc_drop(&drc)); +} + +void test_simple_rc_new_from_val(void) { + _dummy_t val = {.foo = 42}; + _dummy_simple_rc_t drc = _dummy_simple_rc_new_from_val(&val); + assert(!_Z_RC_IS_NULL(&drc)); + assert(_z_simple_rc_strong_count(drc._cnt) == 1); + assert(drc._val->foo == 42); + drc._val->foo = 0; + assert(val.foo == 42); + assert(_dummy_simple_rc_drop(&drc)); +} + +void test_simple_rc_clone(void) { + _dummy_t val = {.foo = 42}; + _dummy_simple_rc_t drc1 = _dummy_simple_rc_new_from_val(&val); + assert(_z_simple_rc_strong_count(drc1._cnt) == 1); + + _dummy_simple_rc_t drc2 = _dummy_simple_rc_clone(&drc1); + assert(!_Z_RC_IS_NULL(&drc2)); + assert(_z_simple_rc_strong_count(drc2._cnt) == 2); + assert(_z_simple_rc_strong_count(drc2._cnt) == _z_simple_rc_strong_count(drc1._cnt)); + assert(drc2._val->foo == drc1._val->foo); + + assert(!_dummy_simple_rc_drop(&drc1)); + assert(_z_simple_rc_strong_count(drc2._cnt) == 1); + assert(drc2._val->foo == 42); + assert(_dummy_simple_rc_drop(&drc2)); +} + +void test_simple_rc_eq(void) { + _dummy_t val = {.foo = 42}; + _dummy_simple_rc_t drc1 = _dummy_simple_rc_new_from_val(&val); + _dummy_simple_rc_t drc2 = _dummy_simple_rc_clone(&drc1); + assert(_dummy_simple_rc_eq(&drc1, &drc2)); + assert(!_dummy_simple_rc_drop(&drc1)); + assert(_dummy_simple_rc_drop(&drc2)); +} + +void test_simple_rc_clone_as_ptr(void) { + _dummy_t val = {.foo = 42}; + _dummy_simple_rc_t drc1 = _dummy_simple_rc_new_from_val(&val); + _dummy_simple_rc_t *drc2 = _dummy_simple_rc_clone_as_ptr(&drc1); + assert(drc2->_val != NULL); + assert(!_Z_RC_IS_NULL(drc2)); + assert(_dummy_simple_rc_count(drc2) == 2); + assert(_dummy_simple_rc_eq(&drc1, drc2)); + assert(!_dummy_simple_rc_drop(&drc1)); + assert(_dummy_simple_rc_count(drc2) == 1); + assert(_dummy_simple_rc_drop(drc2)); + z_free(drc2); +} + +void test_simple_rc_copy(void) { + _dummy_t val = {.foo = 42}; + _dummy_simple_rc_t drc1 = _dummy_simple_rc_new_from_val(&val); + _dummy_simple_rc_t drc2 = _dummy_simple_rc_null(); + assert(!_dummy_simple_rc_eq(&drc1, &drc2)); + _dummy_simple_rc_copy(&drc2, &drc1); + assert(_dummy_simple_rc_count(&drc2) == 2); + assert(_dummy_simple_rc_eq(&drc1, &drc2)); + assert(!_dummy_simple_rc_drop(&drc2)); + assert(_dummy_simple_rc_drop(&drc1)); +} + +void test_simple_rc_decr(void) { + _dummy_t val = {.foo = 42}; + _dummy_simple_rc_t drc1 = _dummy_simple_rc_new_from_val(&val); + _dummy_simple_rc_t drc2 = _dummy_simple_rc_clone(&drc1); + assert(!_dummy_simple_rc_decr(&drc2)); + assert(_dummy_simple_rc_decr(&drc1)); + // free manualy, to make asan happy, because counter already zero + z_free(drc1._val); +} + int main(void) { test_rc_null(); test_rc_size(); @@ -273,5 +379,17 @@ int main(void) { test_weak_upgrade(); test_overflow(); test_decr(); + + test_simple_rc_null(); + test_simple_rc_size(); + test_simple_rc_drop(); + test_simple_rc_new(); + test_simple_rc_new_from_val(); + test_simple_rc_clone(); + test_simple_rc_eq(); + test_simple_rc_clone_as_ptr(); + test_simple_rc_copy(); + test_simple_rc_decr(); + return 0; }