Add unicast interest feature

include/zenoh-pico/collections/refcount.h

@@ -124,6 +124,10 @@
         }                                                                                 \
         return c;                                                                         \
     }                                                                                     \
+    static inline void name##_rc_copy(name##_rc_t *dst, const name##_rc_t *p) {           \
+        dst->in = p->in;                                                                  \
+        _ZP_RC_OP_INCR_CNT                                                                \
+    }                                                                                     \
     static inline _Bool name##_rc_eq(const name##_rc_t *left, const name##_rc_t *right) { \
         return (left->in == right->in);                                                   \
     }                                                                                     \

include/zenoh-pico/config.h

@@ -239,6 +239,13 @@
 #define Z_FEATURE_ATTACHMENT 1
 #endif
 
+/**
+ * Enable interests.
+ */
+#ifndef Z_FEATURE_INTEREST
+#define Z_FEATURE_INTEREST 0
+#endif
+
 /*------------------ Compile-time configuration properties ------------------*/
 /**
  * Default length for Zenoh ID. Maximum size is 16 bytes.

include/zenoh-pico/net/filtering.h

@@ -0,0 +1,48 @@
+//
+// Copyright (c) 2022 ZettaScale Technology
+//
+// This program and the accompanying materials are made available under the
+// terms of the Eclipse Public License 2.0 which is available at
+// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0
+// which is available at https://www.apache.org/licenses/LICENSE-2.0.
+//
+// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0
+//
+// Contributors:
+//   ZettaScale Zenoh Team, <[email protected]>
+
+#ifndef ZENOH_PICO_FILTERING_NETAPI_H
+#define ZENOH_PICO_FILTERING_NETAPI_H
+
+#include <stdint.h>
+
+#include "zenoh-pico/api/constants.h"
+#include "zenoh-pico/net/session.h"
+#include "zenoh-pico/protocol/core.h"
+
+typedef enum {
+    WRITE_FILTER_INIT = 0,
+    WRITE_FILTER_ACTIVE = 1,
+    WRITE_FILTER_OFF = 2,
+} _z_write_filter_state_t;
+
+typedef struct {
+    uint32_t decl_id;
+    uint8_t state;
+} _z_writer_filter_ctx_t;
+
+/**
+ * Return type when declaring a queryable.
+ */
+typedef struct _z_interest_t {
+    uint32_t _interest_id;
+    _z_writer_filter_ctx_t *ctx;
+} _z_write_filter_t;
+
+typedef struct _z_publisher_t _z_publisher_t;
+
+int8_t _z_write_filter_create(_z_publisher_t *pub);
+int8_t _z_write_filter_destroy(const _z_publisher_t *pub);
+_Bool _z_write_filter_active(const _z_publisher_t *pub);
+
+#endif /* ZENOH_PICO_FILTERING_NETAPI_H */

include/zenoh-pico/net/primitives.h

@@ -241,4 +241,10 @@
 );
 #endif
 
+#if Z_FEATURE_INTEREST == 1
+uint32_t _z_declare_interest(_z_session_t *zn, _z_keyexpr_t keyexpr, _z_interest_handler_t callback, uint8_t flags,
+                             void *arg);
+int8_t _z_undeclare_interest(_z_session_t *zn, uint32_t interest_id);
+#endif
+
 #endif /* INCLUDE_ZENOH_PICO_NET_PRIMITIVES_H */

include/zenoh-pico/net/publish.h

@@ -15,18 +15,22 @@
 #ifndef INCLUDE_ZENOH_PICO_NET_PUBLISH_H
 #define INCLUDE_ZENOH_PICO_NET_PUBLISH_H
 
+#include "zenoh-pico/net/filtering.h"
 #include "zenoh-pico/net/session.h"
 #include "zenoh-pico/protocol/core.h"
 
 /**
  * Return type when declaring a publisher.
  */
-typedef struct {
+typedef struct _z_publisher_t {
     _z_keyexpr_t _key;
     _z_zint_t _id;
     _z_session_rc_t _zn;
     z_congestion_control_t _congestion_control;
     z_priority_t _priority;
+#if Z_FEATURE_INTEREST == 1
+    _z_write_filter_t _filter;
+#endif
 } _z_publisher_t;
 
 #if Z_FEATURE_PUBLICATION == 1

include/zenoh-pico/net/session.h

@@ -62,6 +62,11 @@ typedef struct _z_session_t {
 #if Z_FEATURE_QUERY == 1
     _z_pending_query_list_t *_pending_queries;
 #endif
+
+    // Session interests
+#if Z_FEATURE_INTEREST == 1
+    _z_session_interest_rc_list_t *_local_interests;
+#endif
 } _z_session_t;
 
 extern void _z_session_clear(_z_session_t *zn);  // Forward type declaration to avoid cyclical include

include/zenoh-pico/protocol/definitions/declarations.h

@@ -137,8 +137,8 @@ _z_declaration_t _z_make_undecl_queryable(uint32_t id, _Z_OPTIONAL const _z_keye
 _z_declaration_t _z_make_decl_token(_Z_MOVE(_z_keyexpr_t) key, uint32_t id);
 _z_declaration_t _z_make_undecl_token(uint32_t id, _Z_OPTIONAL const _z_keyexpr_t* key);
 
-_z_declaration_t _z_make_decl_interest(_Z_MOVE(_z_keyexpr_t) key, uint32_t id);
+_z_declaration_t _z_make_decl_interest(_Z_MOVE(_z_keyexpr_t) key, uint32_t id, uint8_t interest_flags);
 _z_declaration_t _z_make_undecl_interest(uint32_t id, _Z_OPTIONAL const _z_keyexpr_t* key);
-_z_declaration_t _z_make_final_decl(uint32_t id);
+_z_declaration_t _z_make_final_interest(uint32_t id);
 
 #endif /* INCLUDE_ZENOH_PICO_PROTOCOL_DEFINITIONS_DECLARATIONS_H */

include/zenoh-pico/session/interest.h

@@ -0,0 +1,35 @@
+//
+// Copyright (c) 2022 ZettaScale Technology
+//
+// This program and the accompanying materials are made available under the
+// terms of the Eclipse Public License 2.0 which is available at
+// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0
+// which is available at https://www.apache.org/licenses/LICENSE-2.0.
+//
+// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0
+//
+// Contributors:
+//   ZettaScale Zenoh Team, <[email protected]>
+//
+
+#ifndef ZENOH_PICO_SESSION_INTEREST_H
+#define ZENOH_PICO_SESSION_INTEREST_H
+
+#include <stdbool.h>
+
+#include "zenoh-pico/net/session.h"
+
+#if Z_FEATURE_INTEREST == 1
+_z_session_interest_rc_t *_z_get_interest_by_id(_z_session_t *zn, const _z_zint_t id);
+_z_session_interest_rc_list_t *_z_get_interest_by_key(_z_session_t *zn, const _z_keyexpr_t key);
+_z_session_interest_rc_t *_z_register_interest(_z_session_t *zn, _z_session_interest_t *intr);
+void _z_unregister_interest(_z_session_t *zn, _z_session_interest_rc_t *intr);
+void _z_flush_interest(_z_session_t *zn);
+#endif  // Z_FEATURE_INTEREST == 1
+
+int8_t _z_interest_process_declares(_z_session_t *zn, const _z_declaration_t *decl);
+int8_t _z_interest_process_final_interest(_z_session_t *zn, uint32_t id);
+int8_t _z_interest_process_undeclare_interest(_z_session_t *zn, uint32_t id);
+int8_t _z_interest_process_declare_interest(_z_session_t *zn, _z_keyexpr_t key, uint32_t id, uint8_t flags);
+
+#endif /* ZENOH_PICO_SESSION_INTEREST_H */

include/zenoh-pico/session/session.h

@@ -77,9 +77,10 @@ typedef struct {
 
 _Bool _z_resource_eq(const _z_resource_t *one, const _z_resource_t *two);
 void _z_resource_clear(_z_resource_t *res);
+void _z_resource_copy(_z_resource_t *dst, const _z_resource_t *src);
 void _z_resource_free(_z_resource_t **res);
 
-_Z_ELEM_DEFINE(_z_resource, _z_resource_t, _z_noop_size, _z_resource_clear, _z_noop_copy)
+_Z_ELEM_DEFINE(_z_resource, _z_resource_t, _z_noop_size, _z_resource_clear, _z_resource_copy)
 _Z_LIST_DEFINE(_z_resource, _z_resource_t)
 
 /**
@@ -102,7 +103,7 @@ 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_subscription_rc, _z_subscription_rc_t, _z_noop_size, _z_subscription_rc_drop, _z_noop_copy)
+_Z_ELEM_DEFINE(_z_subscription_rc, _z_subscription_rc_t, _z_noop_size, _z_subscription_rc_drop, _z_subscription_rc_copy)
 _Z_LIST_DEFINE(_z_subscription_rc, _z_subscription_rc_t)
 
 typedef struct {
@@ -187,4 +188,41 @@ typedef void (*_z_hello_handler_t)(_z_hello_t *hello, struct __z_hello_handler_w
 
 int8_t _z_session_generate_zid(_z_id_t *bs, uint8_t size);
 
+typedef enum {
+    _Z_INTEREST_MSG_TYPE_FINAL = 0,
+    _Z_INTEREST_MSG_TYPE_DECL_SUBSCRIBER,
+    _Z_INTEREST_MSG_TYPE_DECL_QUERYABLE,
+    _Z_INTEREST_MSG_TYPE_DECL_TOKEN,
+    _Z_INTEREST_MSG_TYPE_UNDECL_SUBSCRIBER,
+    _Z_INTEREST_MSG_TYPE_UNDECL_QUERYABLE,
+    _Z_INTEREST_MSG_TYPE_UNDECL_TOKEN,
+} _z_interest_msg_type_t;
+
+typedef struct _z_interest_msg_t {
+    uint8_t type;
+    uint32_t id;
+} _z_interest_msg_t;
+
+/**
+ * The callback signature of the functions handling interest messages.
+ */
+typedef void (*_z_interest_handler_t)(const _z_interest_msg_t *msg, void *arg);
+
+typedef struct {
+    _z_keyexpr_t _key;
+    uint32_t _id;
+    _z_interest_handler_t _callback;
+    void *_arg;
+    uint8_t _flags;
+} _z_session_interest_t;
+
+_Bool _z_session_interest_eq(const _z_session_interest_t *one, const _z_session_interest_t *two);
+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_rc, _z_session_interest_rc_t, _z_noop_size, _z_session_interest_rc_drop,
+               _z_noop_copy)
+_Z_LIST_DEFINE(_z_session_interest_rc, _z_session_interest_rc_t)
+
 #endif /* INCLUDE_ZENOH_PICO_SESSION_SESSION_H */

include/zenoh-pico/session/utils.h

@@ -34,4 +34,7 @@ int8_t _z_handle_network_message(_z_session_t *zn, _z_zenoh_message_t *z_msg, ui
 int8_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);
+
 #endif /* INCLUDE_ZENOH_PICO_SESSION_UTILS_H */

include/zenoh-pico/system/link/raweth.h

@@ -58,7 +58,6 @@ typedef struct {
     _Bool _has_vlan;
 } _z_raweth_socket_t;
 
-int8_t _z_get_smac_raweth(_z_raweth_socket_t *resock);
 int8_t _z_open_raweth(_z_sys_net_socket_t *sock, const char *interface);
 size_t _z_send_raweth(const _z_sys_net_socket_t *sock, const void *buff, size_t buff_len);
 size_t _z_receive_raweth(const _z_sys_net_socket_t *sock, void *buff, size_t buff_len, _z_bytes_t *addr);

src/api/api.c

@@ -23,6 +23,7 @@
 #include "zenoh-pico/collections/bytes.h"
 #include "zenoh-pico/config.h"
 #include "zenoh-pico/net/config.h"
+#include "zenoh-pico/net/filtering.h"
 #include "zenoh-pico/net/logger.h"
 #include "zenoh-pico/net/memory.h"
 #include "zenoh-pico/net/primitives.h"
@@ -694,14 +695,28 @@
             key = _z_rid_with_suffix(id, NULL);
         }
     }
-
+    // Set options
     z_publisher_options_t opt = z_publisher_options_default();
     if (options != NULL) {
         opt.congestion_control = options->congestion_control;
         opt.priority = options->priority;
     }
-
-    return (z_owned_publisher_t){._value = _z_declare_publisher(&zs._val, key, opt.congestion_control, opt.priority)};
+    // Set publisher
+    _z_publisher_t *pub = _z_declare_publisher(&zs._val, key, opt.congestion_control, opt.priority);
+    if (pub == NULL) {
+        if (key._id != Z_RESOURCE_ID_NONE) {
+            _z_undeclare_resource(&zs._val.in->val, key._id);
+        }
+        return (z_owned_publisher_t){._value = NULL};
+    }
+    // Create write filter
+    if (_z_write_filter_create(pub) != _Z_RES_OK) {
+        if (key._id != Z_RESOURCE_ID_NONE) {
+            _z_undeclare_resource(&zs._val.in->val, key._id);
+        }
+        return (z_owned_publisher_t){._value = NULL};
+    }
+    return (z_owned_publisher_t){._value = pub};
 }
 
 int8_t z_undeclare_publisher(z_owned_publisher_t *pub) {
@@ -728,33 +743,33 @@
 
 int8_t z_publisher_put(const z_publisher_t pub, const uint8_t *payload, size_t len,
                        const z_publisher_put_options_t *options) {
-    int8_t ret = _Z_RES_OK;
-
+    // Build options
     z_publisher_put_options_t opt = z_publisher_put_options_default();
     if (options != NULL) {
         opt.encoding = options->encoding;
 #if Z_FEATURE_ATTACHMENT == 1
         opt.attachment = options->attachment;
 #endif
     }
-
-    ret = _z_write(&pub._val->_zn.in->val, pub._val->_key, payload, len, opt.encoding, Z_SAMPLE_KIND_PUT,
-                   pub._val->_congestion_control, pub._val->_priority
-#if Z_FEATURE_ATTACHMENT == 1
-                   ,
-                   opt.attachment
-#endif
-    );
-
     // Trigger local subscriptions
     _z_trigger_local_subscriptions(&pub._val->_zn.in->val, pub._val->_key, payload, len
 #if Z_FEATURE_ATTACHMENT == 1
                                    ,
                                    opt.attachment
 #endif
     );
-
-    return ret;
+    // Check if write filter is active before writing
+    if (_z_write_filter_active(pub._val)) {
+        return _Z_RES_OK;
+    }
+    // Write value
+    return _z_write(&pub._val->_zn.in->val, pub._val->_key, payload, len, opt.encoding, Z_SAMPLE_KIND_PUT,
+                    pub._val->_congestion_control, pub._val->_priority
+#if Z_FEATURE_ATTACHMENT == 1
+                    ,
+                    opt.attachment
+#endif
+    );
 }
 
 int8_t z_publisher_delete(const z_publisher_t pub, const z_publisher_delete_options_t *options) {