diff --git a/include/zenoh-pico/collections/lru_cache.h b/include/zenoh-pico/collections/lru_cache.h
index 0f935d642..f60eca4c1 100644
--- a/include/zenoh-pico/collections/lru_cache.h
+++ b/include/zenoh-pico/collections/lru_cache.h
@@ -23,9 +23,6 @@
 extern "C" {
 #endif
 
-// TODO: move to config
-#define Z_FEATURE_CACHE_TREE 0
-
 // Three way comparison function pointer
 typedef int (*_z_lru_val_cmp_f)(const void *first, const void *second);
 
diff --git a/include/zenoh-pico/config.h b/include/zenoh-pico/config.h
index 3f24bbd9e..5d7c07e12 100644
--- a/include/zenoh-pico/config.h
+++ b/include/zenoh-pico/config.h
@@ -179,6 +179,15 @@
  */
 #define Z_RX_CACHE_SIZE 10
 
+/**
+ * Use binary tree to speed up rx cache search
+ */
+#if Z_RX_CACHE_SIZE > 20
+#define Z_FEATURE_CACHE_TREE 1
+#else
+#define Z_FEATURE_CACHE_TREE 0
+#endif
+
 /**
  * Default get timeout in milliseconds.
  */
diff --git a/include/zenoh-pico/config.h.in b/include/zenoh-pico/config.h.in
index 800548b93..6c5d71e54 100644
--- a/include/zenoh-pico/config.h.in
+++ b/include/zenoh-pico/config.h.in
@@ -179,6 +179,15 @@
  */
 #define Z_RX_CACHE_SIZE 10
 
+/**
+ * Use binary tree to speed up rx cache search
+ */
+#if Z_RX_CACHE_SIZE > 20
+#define Z_FEATURE_CACHE_TREE 1
+#else
+#define Z_FEATURE_CACHE_TREE 0
+#endif
+
 /**
  * Default get timeout in milliseconds.
  */
diff --git a/tests/z_lru_cache_test.c b/tests/z_lru_cache_test.c
index b556d4584..64dc4e7e8 100644
--- a/tests/z_lru_cache_test.c
+++ b/tests/z_lru_cache_test.c
@@ -184,11 +184,51 @@ void test_lru_cache_random_val(void) {
     _dummy_lru_cache_delete(&dcache);
 }
 
+#if 0
+void *stop_task(void *ctx) {
+    z_sleep_s(10);
+    bool *stop_flag = (bool *)ctx;
+    *stop_flag = true;
+    return NULL;
+}
+// Results
+// size 10: 287720681 list, 294626805 tree (1.024)
+// size 20: 207124561 list, 282312820 tree (1.363)
+// size 50: 104164238 list, 212367113 tree (2.039)
+// size 100: 51666425 list, 198541036 tree (3.843)
+// size 1000: 5838653 list, 122717170 tree (21.018)
+void test_search_benchmark(void) {
+    _dummy_lru_cache_t dcache = _dummy_lru_cache_init(10);
+    _dummy_t data[10] = {0};
+
+    srand(0x55);
+    // Insert data
+    for (size_t i = 0; i < _ZP_ARRAY_SIZE(data); i++) {
+        data[i].foo = rand();
+        assert(_dummy_lru_cache_insert(&dcache, &data[i]) == 0);
+    }
+    bool stop_flag = false;
+    pthread_t task;
+    pthread_create(&task, NULL, stop_task, &stop_flag);
+    size_t get_cnt = 0;
+
+    while (!stop_flag) {
+        int i = rand() % _ZP_ARRAY_SIZE(data);
+        _dummy_t *src = &data[i];
+        _dummy_t *res = _dummy_lru_cache_get(&dcache, src);
+        get_cnt++;
+        assert(res != NULL);
+    }
+    printf("Mode: %d, Get count: %ld\n", Z_FEATURE_CACHE_TREE, get_cnt);
+}
+#endif
+
 int main(void) {
     test_lru_init();
     test_lru_cache_insert();
     test_lru_cache_deletion();
     test_lru_cache_update();
     test_lru_cache_random_val();
+    // test_search_benchmark();
     return 0;
 }