tree data OPTIMIZE added first_sibling parameter

This change avoids unnecessary finding of the first sibling in case
the parent node is not available.
diff --git a/src/tree_data.c b/src/tree_data.c
index 0b1dbcb..5ff1715 100644
--- a/src/tree_data.c
+++ b/src/tree_data.c
@@ -543,27 +543,32 @@
 }
 
 void
-lyd_insert_after_node(struct lyd_node *sibling, struct lyd_node *node)
+lyd_insert_after_node(struct lyd_node **first_sibling_p, struct lyd_node *sibling, struct lyd_node *node)
 {
     struct lyd_node_inner *par;
+    struct lyd_node *first_sibling;
 
     assert(!node->next && (node->prev == node));
 
-    node->next = sibling->next;
-    node->prev = sibling;
-    sibling->next = node;
-    if (node->next) {
+    if (sibling->next) {
         /* sibling had a succeeding node */
-        node->next->prev = node;
+        sibling->next->prev = node;
+        node->next = sibling->next;
     } else {
         /* sibling was last, find first sibling and change its prev */
-        if (sibling->parent) {
-            sibling = sibling->parent->child;
+        if (first_sibling_p && *first_sibling_p) {
+            assert(!(*first_sibling_p)->prev->next);
+            (*first_sibling_p)->prev = node;
         } else {
-            for ( ; sibling->prev->next != node; sibling = sibling->prev) {}
+            first_sibling = lyd_first_sibling(sibling);
+            first_sibling->prev = node;
+            if (first_sibling_p) {
+                *first_sibling_p = first_sibling;
+            }
         }
-        sibling->prev = node;
     }
+    node->prev = sibling;
+    sibling->next = node;
     node->parent = sibling->parent;
 
     for (par = node->parent; par; par = par->parent) {
@@ -692,33 +697,42 @@
  * @brief Insert @p node as the last node.
  *
  * @param[in] parent Parent to insert into, NULL for top-level sibling.
- * @param[in] first_sibling First sibling, NULL if no top-level sibling exist yet. Can be also NULL if @p parent is set.
+ * @param[in,out] first_sibling First sibling, NULL if no top-level sibling exist yet.
+ * Can be also NULL if @p parent is set.
  * @param[in] node Individual node (without siblings) to insert.
  */
 static void
-lyd_insert_node_last(struct lyd_node *parent, struct lyd_node *first_sibling, struct lyd_node *node)
+lyd_insert_node_last(struct lyd_node *parent, struct lyd_node **first_sibling, struct lyd_node *node)
 {
-    if (first_sibling) {
+    assert(first_sibling && node);
+
+    if (*first_sibling) {
 #ifndef NDEBUG
-        if (lyds_is_supported(node) && (first_sibling->prev->schema == node->schema) &&
-                (lyds_compare_single(first_sibling->prev, node) > 0)) {
+        if (lyds_is_supported(node) && ((*first_sibling)->prev->schema == node->schema) &&
+                (lyds_compare_single((*first_sibling)->prev, node) > 0)) {
             LOGWRN(LYD_CTX(node), "Data in \"%s\" are not sorted, inserted node should not be added to the end.",
                     node->schema->name);
         }
 #endif
-        lyd_insert_after_node(first_sibling->prev, node);
+        lyd_insert_after_node(first_sibling, (*first_sibling)->prev, node);
     } else if (parent) {
         lyd_insert_only_child(parent, node);
+        *first_sibling = node;
+    } else {
+        *first_sibling = node;
     }
 }
 
 void
-lyd_insert_node_ordby_schema(struct lyd_node *parent, struct lyd_node *first_sibling, struct lyd_node *node)
+lyd_insert_node_ordby_schema(struct lyd_node *parent, struct lyd_node **first_sibling, struct lyd_node *node)
 {
     struct lyd_node *anchor;
 
-    if ((anchor = lyd_insert_node_find_anchor(first_sibling, node))) {
+    assert(first_sibling && node);
+
+    if ((anchor = lyd_insert_node_find_anchor(*first_sibling, node))) {
         lyd_insert_before_node(anchor, node);
+        *first_sibling = *first_sibling != anchor ? *first_sibling : node;
     } else {
         lyd_insert_node_last(parent, first_sibling, node);
     }
@@ -741,19 +755,19 @@
     first_sibling = parent ? lyd_child(parent) : *first_sibling_p;
 
     if (last) {
-        lyd_insert_node_last(parent, first_sibling, node);
+        lyd_insert_node_last(parent, &first_sibling, node);
     } else if (lyds_is_supported(node) &&
             (lyd_find_sibling_schema(first_sibling, node->schema, &leader) == LY_SUCCESS)) {
-        ret = lyds_insert(&leader, node);
+        ret = lyds_insert(&first_sibling, &leader, node);
         if (ret) {
             /* The operation on the sorting tree unexpectedly failed due to some internal issue,
              * but insert the node anyway although the nodes will not be sorted.
              */
             LOGWRN(LYD_CTX(node), "Data in \"%s\" are not sorted.", node->schema->name);
-            lyd_insert_node_ordby_schema(parent, first_sibling, node);
+            lyd_insert_node_ordby_schema(parent, &first_sibling, node);
         }
     } else {
-        lyd_insert_node_ordby_schema(parent, first_sibling, node);
+        lyd_insert_node_ordby_schema(parent, &first_sibling, node);
     }
 
     /* insert into parent HT */
@@ -768,7 +782,7 @@
     }
 
     if (first_sibling_p) {
-        *first_sibling_p = node->prev->next ? first_sibling : node;
+        *first_sibling_p = first_sibling;
     }
 }
 
@@ -799,25 +813,26 @@
  *
  * The nodes will remain sorted according to the schema.
  *
- * @param[in] first_sibling First sibling, destination.
+ * @param[in] first_dst First sibling, destination.
  * @param[in] node Starting node, all following nodes with the same schema will be moved.
  * @param[out] next_p Next node that has a different schema or NULL.
  * @return LY_ERR value.
  */
 static LY_ERR
-lyd_move_nodes_ordby_schema(struct lyd_node *first_sibling, struct lyd_node *node, struct lyd_node **next_p)
+lyd_move_nodes_ordby_schema(struct lyd_node **first_dst, struct lyd_node *node, struct lyd_node **next_p)
 {
-    struct lyd_node *second, *anchor, *iter, *next, *dst, *src;
+    struct lyd_node *second, *anchor, *iter, *next, *dst, *src, *first_src = NULL;
 
-    assert(first_sibling && !first_sibling->prev->next && node && next_p);
+    assert(first_dst && *first_dst && !(*first_dst)->prev->next && node && next_p);
 
-    if ((anchor = lyd_insert_node_find_anchor(first_sibling, node))) {
+    if ((anchor = lyd_insert_node_find_anchor(*first_dst, node))) {
         /* move the first node to the correct place according to the schema */
         LY_CHECK_RET(lyd_unlink_check(node));
         second = node->next;
-        lyd_unlink_ignore_lyds(node);
+        lyd_unlink_ignore_lyds(&first_src, node);
         lyd_insert_before_node(anchor, node);
         lyd_insert_hash(node);
+        *first_dst = *first_dst != anchor ? *first_dst : node;
         if (!second || (node->schema != second->schema)) {
             /* no more nodes to move */
             *next_p = second;
@@ -827,7 +842,7 @@
         src = second;
     } else {
         /* just move all instances to the end */
-        dst = first_sibling->prev;
+        dst = (*first_dst)->prev;
         src = node;
     }
 
@@ -837,8 +852,8 @@
         if (iter->schema != src->schema) {
             break;
         }
-        lyd_unlink_ignore_lyds(iter);
-        lyd_insert_after_node(dst, iter);
+        lyd_unlink_ignore_lyds(&first_src, iter);
+        lyd_insert_after_node(first_dst, dst, iter);
         lyd_insert_hash(iter);
         dst = iter;
     }
@@ -859,7 +874,7 @@
 static LY_ERR
 lyd_move_nodes_at_once(struct lyd_node *parent, struct lyd_node *first_src)
 {
-    struct lyd_node *start, *next, *iter;
+    struct lyd_node *start, *next, *iter, *first_dst;
 
     assert(!lyd_child(parent) && first_src && !first_src->prev->next && !first_src->parent);
 
@@ -867,19 +882,20 @@
 
     /* move the first node */
     start = first_src->next;
+    first_dst = first_src;
     if (parent) {
-        lyd_unlink_ignore_lyds(first_src);
-        lyd_insert_only_child(parent, first_src);
-        lyd_insert_hash(first_src);
+        lyd_unlink_ignore_lyds(&first_src, first_dst);
+        lyd_insert_only_child(parent, first_dst);
+        lyd_insert_hash(first_dst);
     } else {
-        lyd_unlink_ignore_lyds(first_src);
+        lyd_unlink_ignore_lyds(&first_src, first_dst);
     }
 
     /* move the rest of the nodes */
     LY_LIST_FOR_SAFE(start, next, iter) {
         LY_CHECK_RET(lyd_unlink_check(iter));
-        lyd_unlink_ignore_lyds(iter);
-        lyd_insert_after_node(first_src->prev, iter);
+        lyd_unlink_ignore_lyds(&first_src, iter);
+        lyd_insert_after_node(&first_dst, first_dst->prev, iter);
         lyd_insert_hash(iter);
     }
 
@@ -889,33 +905,33 @@
 /**
  * @brief Move the nodes in parts according to the schema.
  *
- * @param[in] first_sibling First sibling, destination.
+ * @param[in,out] first_dst First sibling, destination.
  * @param[in] first_src First sibling, all following nodes will be moved.
  * @return LY_ERR value.
  */
 static LY_ERR
-lyd_move_nodes_by_schema(struct lyd_node *first_sibling, struct lyd_node *first_src)
+lyd_move_nodes_by_schema(struct lyd_node **first_dst, struct lyd_node *first_src)
 {
     LY_ERR ret;
     struct lyd_node *next, *iter, *leader;
 
-    assert(first_sibling && !first_sibling->prev->next && first_src && !first_src->prev->next && !first_src->parent);
+    assert(first_dst && *first_dst && !(*first_dst)->prev->next && first_src &&
+            !first_src->prev->next && !first_src->parent);
 
     for (iter = first_src; iter; iter = next) {
         if (lyds_is_supported(iter) &&
-                (lyd_find_sibling_schema(first_sibling, iter->schema, &leader) == LY_SUCCESS)) {
-            ret = lyds_merge(&leader, iter, &next);
+                (lyd_find_sibling_schema(*first_dst, iter->schema, &leader) == LY_SUCCESS)) {
+            ret = lyds_merge(first_dst, &leader, &first_src, iter, &next);
             if (ret) {
                 /* The operation on the sorting tree unexpectedly failed due to some internal issue,
                  * but insert the node anyway although the nodes will not be sorted.
                  */
                 LOGWRN(LYD_CTX(first_src), "Data in \"%s\" are not sorted.", leader->schema->name);
-                LY_CHECK_RET(lyd_move_nodes_ordby_schema(first_sibling, next, &next));
+                LY_CHECK_RET(lyd_move_nodes_ordby_schema(first_dst, next, &next));
             }
         } else {
-            LY_CHECK_RET(lyd_move_nodes_ordby_schema(first_sibling, iter, &next));
+            LY_CHECK_RET(lyd_move_nodes_ordby_schema(first_dst, iter, &next));
         }
-        first_sibling = iter->prev->next ? first_sibling : iter;
     }
 
     return LY_SUCCESS;
@@ -925,22 +941,34 @@
  * @brief Move a nodes into parent/siblings.
  *
  * @param[in] parent Parent to insert into, NULL for top-level sibling.
- * @param[in] first_sibling First sibling, NULL if no top-level sibling exist yet. Can be also NULL if @p parent is set.
+ * @param[in,out] first_dst_p First sibling, NULL if no top-level sibling exist yet.
+ * Can be also NULL if @p parent is set.
  * @param[in] first_src First sibling, all following nodes will be moved.
  * @return LY_ERR value.
  */
 static LY_ERR
-lyd_move_nodes(struct lyd_node *parent, struct lyd_node *first_sibling, struct lyd_node *first_src)
+lyd_move_nodes(struct lyd_node *parent, struct lyd_node **first_dst_p, struct lyd_node *first_src)
 {
     LY_ERR ret;
+    struct lyd_node *first_dst;
 
-    assert(first_src && (!first_sibling || !first_sibling->prev->next) && !first_src->prev->next);
+    assert((parent || first_dst_p) && first_src && !first_src->prev->next);
 
-    first_sibling = first_sibling ? first_sibling : lyd_child(parent);
-    if (!first_sibling) {
-        ret = lyd_move_nodes_at_once(parent, first_src);
+    if (!first_dst_p || !*first_dst_p) {
+        first_dst = lyd_child(parent);
     } else {
-        ret = lyd_move_nodes_by_schema(first_sibling, first_src);
+        first_dst = *first_dst_p;
+    }
+
+    if (first_dst) {
+        ret = lyd_move_nodes_by_schema(&first_dst, first_src);
+    } else {
+        ret = lyd_move_nodes_at_once(parent, first_src);
+        first_dst = first_src;
+    }
+
+    if (first_dst_p) {
+        *first_dst_p = first_dst;
     }
 
     return ret;
@@ -1050,11 +1078,11 @@
         LY_CHECK_RET(lyd_unlink_tree(node));
         lyd_insert_node(NULL, &first_sibling, node, 0);
     } else {
-        LY_CHECK_RET(lyd_move_nodes(NULL, first_sibling, node));
+        LY_CHECK_RET(lyd_move_nodes(NULL, &first_sibling, node));
     }
 
     if (first) {
-        *first = node->prev->next ? first_sibling : node;
+        *first = first_sibling;
     }
 
     return LY_SUCCESS;
@@ -1102,16 +1130,16 @@
     }
 
     lyd_unlink(node);
-    lyd_insert_after_node(sibling, node);
+    lyd_insert_after_node(NULL, sibling, node);
     lyd_insert_hash(node);
 
     return LY_SUCCESS;
 }
 
 void
-lyd_unlink_ignore_lyds(struct lyd_node *node)
+lyd_unlink_ignore_lyds(struct lyd_node **first_sibling_p, struct lyd_node *node)
 {
-    struct lyd_node *iter;
+    struct lyd_node *first_sibling;
 
     /* update hashes while still linked into the tree */
     lyd_unlink_hash(node);
@@ -1122,23 +1150,27 @@
     }
 
     /* unlink from siblings */
-    if (node->prev->next) {
-        node->prev->next = node->next;
-    }
     if (node->next) {
         node->next->prev = node->prev;
+        if (node->prev->next) {
+            node->prev->next = node->next;
+        } else if (first_sibling_p) {
+            /* unlinking the first node */
+            *first_sibling_p = node->next;
+        }
     } else {
         /* unlinking the last node */
-        if (node->parent) {
-            iter = node->parent->child;
+        /* update the "last" pointer from the first node */
+        if (first_sibling_p && *first_sibling_p) {
+            (*first_sibling_p)->prev = node->prev;
         } else {
-            iter = node->prev;
-            while (iter->prev != node) {
-                iter = iter->prev;
+            first_sibling = lyd_first_sibling(node);
+            first_sibling->prev = node->prev;
+            if (first_sibling_p) {
+                *first_sibling_p = first_sibling;
             }
         }
-        /* update the "last" pointer from the first node */
-        iter->prev = node->prev;
+        node->prev->next = NULL;
     }
 
     /* unlink from parent */
@@ -1179,30 +1211,30 @@
     }
 
     /* unlink data tree */
-    lyd_unlink_ignore_lyds(node);
+    lyd_unlink_ignore_lyds(NULL, node);
 }
 
 LIBYANG_API_DEF LY_ERR
 lyd_unlink_siblings(struct lyd_node *node)
 {
-    struct lyd_node *next, *iter, *leader, *start;
+    struct lyd_node *next, *iter, *leader, *start, *first_sibling = NULL;
 
     if (lyds_is_supported(node) && node->prev->next && (node->prev->schema == node->schema)) {
         /* unlink starts at the non-first item in the (leaf-)list */
         lyd_find_sibling_val(node, node->schema, NULL, 0, &leader);
-        lyds_split(leader, node, &start);
+        lyds_split(&first_sibling, leader, node, &start);
     } else {
         /* unlink @p node */
         LY_CHECK_RET(lyd_unlink_check(node));
         start = node->next;
-        lyd_unlink_ignore_lyds(node);
+        lyd_unlink_ignore_lyds(&first_sibling, node);
     }
 
     /* continue unlinking the rest */
     LY_LIST_FOR_SAFE(start, next, iter) {
         LY_CHECK_RET(lyd_unlink_check(iter));
-        lyd_unlink_ignore_lyds(iter);
-        lyd_insert_after_node(node->prev, iter);
+        lyd_unlink_ignore_lyds(&first_sibling, iter);
+        lyd_insert_after_node(&node, node->prev, iter);
         lyd_insert_hash(iter);
     }
 
@@ -2516,7 +2548,7 @@
         /* node not found, merge it */
         if (options & LYD_MERGE_DESTRUCT) {
             dup_src = (struct lyd_node *)sibling_src;
-            lyd_unlink_ignore_lyds(dup_src);
+            lyd_unlink_ignore_lyds(NULL, dup_src);
             /* spend it */
             *sibling_src_p = NULL;
         } else {
diff --git a/src/tree_data_common.c b/src/tree_data_common.c
index e1973cc..eb14433 100644
--- a/src/tree_data_common.c
+++ b/src/tree_data_common.c
@@ -773,9 +773,13 @@
 
     /* get the first sibling */
     if (node->parent) {
-        start = node->parent->child;
-    } else {
-        for (start = (struct lyd_node *)node; start->prev->next; start = start->prev) {}
+        return node->parent->child;
+    } else if (!node->prev->next) {
+        return (struct lyd_node *)node;
+    }
+
+    for (start = (struct lyd_node *)node->prev; start->prev->next; start = start->prev) {
+        assert(start != node);
     }
 
     return start;
diff --git a/src/tree_data_free.c b/src/tree_data_free.c
index d76a4aa..32d32cc 100644
--- a/src/tree_data_free.c
+++ b/src/tree_data_free.c
@@ -267,7 +267,7 @@
 static void
 lyd_free_(struct lyd_node *node)
 {
-    struct lyd_node *iter, *next;
+    struct lyd_node *iter, *next, *first_sibling = NULL;
 
     if (!node) {
         return;
@@ -282,7 +282,7 @@
         /* in case of the top-level nodes (node->parent is NULL), no unlinking needed */
         if (iter->parent) {
             lyds_free_metadata(iter);
-            lyd_unlink_ignore_lyds(iter);
+            lyd_unlink_ignore_lyds(&first_sibling, iter);
         }
         lyd_free_subtree(iter);
     }
diff --git a/src/tree_data_internal.h b/src/tree_data_internal.h
index f7bf216..19fb859 100644
--- a/src/tree_data_internal.h
+++ b/src/tree_data_internal.h
@@ -372,10 +372,13 @@
  *
  * Handles inserting into NP containers and key-less lists.
  *
+ * @param[in,out] first_sibling Optional, useful for optimization. The function operates with the first sibling
+ * only if the node is inserted last. It is optimal when the first sibling is set. If it is set to NULL or
+ * if it points to a NULL pointer, then the function will find the first sibling itself.
  * @param[in] sibling Sibling to insert after.
  * @param[in] node Node to insert.
  */
-void lyd_insert_after_node(struct lyd_node *sibling, struct lyd_node *node);
+void lyd_insert_after_node(struct lyd_node **first_sibling, struct lyd_node *sibling, struct lyd_node *node);
 
 /**
  * @brief Insert node before a sibling.
@@ -402,10 +405,11 @@
  * @brief Insert a node into parent/siblings, either before the 'anchor' or as the last sibling.
  *
  * @param[in] parent Parent to insert into, NULL for top-level sibling.
- * @param[in] first_sibling First sibling, NULL if no top-level sibling exist yet. Can be also NULL if @p parent is set.
+ * @param[in,out] first_sibling First sibling, pointing to NULL pointer if no top-level sibling exist yet
+ * or if @p parent is set.
  * @param[in] node Individual node (without siblings) to insert.
  */
-void lyd_insert_node_ordby_schema(struct lyd_node *parent, struct lyd_node *first_sibling, struct lyd_node *node);
+void lyd_insert_node_ordby_schema(struct lyd_node *parent, struct lyd_node **first_sibling, struct lyd_node *node);
 
 /**
  * @brief Unlink the specified data subtree.
@@ -676,8 +680,9 @@
  *
  * The lyds_unlink() is NOT called in this function.
  *
+ * @param[in,out] first_sibling Optional, performs an update if @p node is first or @p sibling is last.
  * @param[in] node Data tree node to be unlinked (together with all the children).
  */
-void lyd_unlink_ignore_lyds(struct lyd_node *node);
+void lyd_unlink_ignore_lyds(struct lyd_node **first_sibling, struct lyd_node *node);
 
 #endif /* LY_TREE_DATA_INTERNAL_H_ */
diff --git a/src/tree_data_sorted.c b/src/tree_data_sorted.c
index f2ba654..cce9f9c 100644
--- a/src/tree_data_sorted.c
+++ b/src/tree_data_sorted.c
@@ -1008,32 +1008,37 @@
 /**
  * @brief Connect data node with siblings so that the nodes are sorted.
  *
+ * @param[in,out] first_sibling First sibling node.
  * @param[in,out] leader First instance of the (leaf-)list.
  * @param[in] node Data node to link.
  * @param[in] root_meta Metadata containing Red-black tree. Can be moved to a new leader.
  * @param[in] rbn red-black node of @p node.
  */
 static void
-lyds_link_data_node(struct lyd_node **leader, struct lyd_node *node, struct lyd_meta *root_meta, struct rb_node *rbn)
+lyds_link_data_node(struct lyd_node **first_sibling, struct lyd_node **leader, struct lyd_node *node,
+        struct lyd_meta *root_meta, struct rb_node *rbn)
 {
     struct rb_node *prev;
 
     /* insert @p node also into the data node (struct lyd_node) siblings */
     prev = rb_prev(rbn);
     if (prev) {
-        lyd_insert_after_node(RBN_DNODE(prev), RBN_DNODE(rbn));
+        lyd_insert_after_node(first_sibling, RBN_DNODE(prev), RBN_DNODE(rbn));
     } else {
         /* leader is no longer the first, the first is @p node */
         lyd_insert_before_node(*leader, RBN_DNODE(rbn));
         *leader = node;
         /* move metadata from the old leader to the new one */
         lyds_move_meta(node, root_meta);
+        /* update first_sibling */
+        *first_sibling = node->prev->next ? *first_sibling : node;
     }
 }
 
 /**
  * @brief Additionally create the Red-black nodes.
  *
+ * @param[in,out] first_sibling First sibling node.
  * @param[in,out] leader First instance of the (leaf-)list.
  * @param[in] root_meta From the @p leader, metadata in which is the root of the Red-black tree.
  * @param[in] rbt From the @p root_meta, root of the Red-black tree.
@@ -1041,8 +1046,8 @@
  * @return LY_ERR value.
  */
 static LY_ERR
-lyds_additionally_create_rb_nodes(struct lyd_node **leader, struct lyd_meta *root_meta,
-        struct rb_node **rbt, struct lyd_node *node)
+lyds_additionally_create_rb_nodes(struct lyd_node **first_sibling, struct lyd_node **leader,
+        struct lyd_meta *root_meta, struct rb_node **rbt, struct lyd_node *node)
 {
     LY_ERR ret;
     ly_bool max;
@@ -1055,8 +1060,8 @@
         rb_insert_node(rbt, rbn, &max);
         if (!max) {
             /* nodes were not sorted, they will be sorted now */
-            lyd_unlink_ignore_lyds(iter);
-            lyds_link_data_node(leader, iter, root_meta, rbn);
+            lyd_unlink_ignore_lyds(first_sibling, iter);
+            lyds_link_data_node(first_sibling, leader, iter, root_meta, rbn);
         }
     }
 
@@ -1066,13 +1071,15 @@
 /**
  * @brief Additionally create the Red-black tree for the sorted nodes.
  *
+ * @param[in,out] first_sibling First sibling node.
  * @param[in,out] leader First instance of the (leaf-)list.
  * @param[in] root_meta From the @p leader, metadata in which is the root of the Red-black tree.
  * @param[in] rbt From the @p root_meta, root of the Red-black tree.
  * @return LY_ERR value.
  */
 static LY_ERR
-lyds_additionally_create_rb_tree(struct lyd_node **leader, struct lyd_meta *root_meta, struct rb_node **rbt)
+lyds_additionally_create_rb_tree(struct lyd_node **first_sibling, struct lyd_node **leader,
+        struct lyd_meta *root_meta, struct rb_node **rbt)
 {
     LY_ERR ret;
     struct rb_node *rbn;
@@ -1083,7 +1090,7 @@
     *rbt = rbn;
 
     /* continue with the rest of the nodes */
-    ret = lyds_additionally_create_rb_nodes(leader, root_meta, rbt, (*leader)->next);
+    ret = lyds_additionally_create_rb_nodes(first_sibling, leader, root_meta, rbt, (*leader)->next);
 
     /* store pointer to the root */
     RBT_SET(root_meta, *rbt);
@@ -1094,6 +1101,7 @@
 /**
  * @brief Additionally reuse the Red-black tree for the sorted nodes.
  *
+ * @param[in,out] first_sibling First sibling node.
  * @param[in,out] leader First instance of the (leaf-)list.
  * @param[in] root_meta From the @p leader, metadata in which is the root of the Red-black tree.
  * @param[in] rbt From the @p root_meta, root of the Red-black tree.
@@ -1102,8 +1110,8 @@
  * so not all nodes have been processed. Or is set to NULL, so all were sorted successfully.
  */
 static void
-lyds_additionally_reuse_rb_tree(struct lyd_node **leader, struct lyd_meta *root_meta, struct rb_node **rbt,
-        struct lyds_pool *pool, struct lyd_node **next)
+lyds_additionally_reuse_rb_tree(struct lyd_node **first_sibling, struct lyd_node **leader, struct lyd_meta *root_meta,
+        struct rb_node **rbt, struct lyds_pool *pool, struct lyd_node **next)
 {
     ly_bool max;
     struct lyd_node *iter;
@@ -1123,8 +1131,8 @@
         rb_insert_node(rbt, pool->rbn, &max);
         if (!max) {
             /* nodes were not sorted, they will be sorted now */
-            lyd_unlink_ignore_lyds(iter);
-            lyds_link_data_node(leader, iter, root_meta, pool->rbn);
+            lyd_unlink_ignore_lyds(first_sibling, iter);
+            lyds_link_data_node(first_sibling, leader, iter, root_meta, pool->rbn);
         }
         pool->rbn = rb_iter_next(&pool->iter_state);
     }
@@ -1195,7 +1203,7 @@
 }
 
 LY_ERR
-lyds_insert(struct lyd_node **leader, struct lyd_node *node)
+lyds_insert(struct lyd_node **first_sibling, struct lyd_node **leader, struct lyd_node *node)
 {
     LY_ERR ret;
     struct rb_node *rbt, *rbn;
@@ -1222,14 +1230,14 @@
          * created additionally now. It may still not be worth creating a tree and it may be better
          * to insert the node by linear search instead, but that is a case for further optimization.
          */
-        ret = lyds_additionally_create_rb_tree(leader, root_meta, &rbt);
+        ret = lyds_additionally_create_rb_tree(first_sibling, leader, root_meta, &rbt);
         LY_CHECK_RET(ret);
     }
 
     /* Insert the node to the correct order. */
     ret = rb_insert(node, &rbt, &rbn);
     LY_CHECK_RET(ret);
-    lyds_link_data_node(leader, node, root_meta, rbn);
+    lyds_link_data_node(first_sibling, leader, node, root_meta, rbn);
 
     /* the root of the Red-black tree may changed due to insertion, so update the pointer to the root */
     RBT_SET(root_meta, rbt);
@@ -1254,7 +1262,7 @@
         lyd_find_sibling_schema(*first_sibling, node->schema, &ld);
         if (!ld) {
             /* the destination has no (leaf-)list instance yet. */
-            lyd_insert_node_ordby_schema(parent, *first_sibling, node);
+            lyd_insert_node_ordby_schema(parent, first_sibling, node);
             *leader = node;
             goto cleanup;
         } else {
@@ -1279,10 +1287,10 @@
     }
     if (!rbt) {
         /* Red-black tree needed */
-        lyds_additionally_reuse_rb_tree(leader, root_meta, &rbt, pool, &next);
+        lyds_additionally_reuse_rb_tree(first_sibling, leader, root_meta, &rbt, pool, &next);
         if (next) {
             /* there is no free structure for red-black node, a new ones must be allocated */
-            ret = lyds_additionally_create_rb_nodes(leader, root_meta, &rbt, next);
+            ret = lyds_additionally_create_rb_nodes(first_sibling, leader, root_meta, &rbt, next);
             LY_CHECK_RET(ret);
         }
     }
@@ -1301,7 +1309,7 @@
         LY_CHECK_RET(ret);
     }
     /* connect @p node with siblings so that the order is maintained */
-    lyds_link_data_node(leader, node, root_meta, rbn);
+    lyds_link_data_node(first_sibling, leader, node, root_meta, rbn);
 
 cleanup:
     if (rbt) {
@@ -1341,7 +1349,7 @@
 }
 
 void
-lyds_split(struct lyd_node *leader, struct lyd_node *node, struct lyd_node **next_p)
+lyds_split(struct lyd_node **first_sibling, struct lyd_node *leader, struct lyd_node *node, struct lyd_node **next_p)
 {
     struct rb_node *rbt, *rbn;
     struct lyd_node *iter, *next, *start, *dst;
@@ -1353,14 +1361,14 @@
     if (!rbt || (leader == node)) {
         /* Second list is just unlinked */
         start = node->next;
-        lyd_unlink_ignore_lyds(node);
+        lyd_unlink_ignore_lyds(first_sibling, node);
         dst = node;
         LY_LIST_FOR_SAFE(start, next, iter) {
             if (iter->schema != node->schema) {
                 break;
             }
-            lyd_unlink_ignore_lyds(iter);
-            lyd_insert_after_node(dst, iter);
+            lyd_unlink_ignore_lyds(first_sibling, iter);
+            lyd_insert_after_node(&node, dst, iter);
             dst = iter;
         }
         *next_p = iter;
@@ -1372,7 +1380,7 @@
         /* @p node is the last node, remove from Red-black tree and unlink */
         rb_remove_node(root_meta, &rbt, node, &rbn);
         rb_free_node(rbn);
-        lyd_unlink_ignore_lyds(node);
+        lyd_unlink_ignore_lyds(first_sibling, node);
         *next_p = start;
         goto cleanup;
     }
@@ -1380,7 +1388,7 @@
     /* remove @p node from Red-black tree and unlink */
     rb_remove_node(root_meta, &rbt, node, &rbn);
     rb_free_node(rbn);
-    lyd_unlink_ignore_lyds(node);
+    lyd_unlink_ignore_lyds(first_sibling, node);
 
     /* remove the rest of nodes from Red-black tree and unlink */
     dst = node;
@@ -1390,9 +1398,9 @@
         }
         rb_remove_node(root_meta, &rbt, iter, &rbn);
         rb_free_node(rbn);
-        lyd_unlink_ignore_lyds(iter);
+        lyd_unlink_ignore_lyds(first_sibling, iter);
         /* insert them to the second (leaf-)list */
-        lyd_insert_after_node(dst, iter);
+        lyd_insert_after_node(&node, dst, iter);
         dst = iter;
     }
     *next_p = iter;
@@ -1415,17 +1423,19 @@
 /**
  * @brief Merge nodes (src) without Red-black tree into (dst) nodes with Red-black tree.
  *
+ * @param[in,out] first_dst First sibling node, destination.
  * @param[in,out] leader_dst First instance of the destination (leaf-)list.
  * @param[in] root_meta_dst Metadata 'lyds_tree' from @p leader_dst.
  * @param[in] rbt_dst Root of the destination Red-black tree.
+ * @param[in,out] first_src First sibling node, source.
  * @param[in] leader_src First instance of the source (leaf-)list.
  * @param[out] next_p Data node located after source (leaf-)list.
  * On error, it points to the some node in source (leaf-)list that failed to merge.
  * @return LY_ERR value.
  */
 static LY_ERR
-lyds_merge_nodes1(struct lyd_node **leader_dst, struct lyd_meta *root_meta_dst, struct rb_node *rbt_dst,
-        struct lyd_node *leader_src, struct lyd_node **next_p)
+lyds_merge_nodes1(struct lyd_node **first_dst, struct lyd_node **leader_dst, struct lyd_meta *root_meta_dst,
+        struct rb_node *rbt_dst, struct lyd_node **first_src, struct lyd_node *leader_src, struct lyd_node **next_p)
 {
     LY_ERR ret;
     struct rb_node *rbn;
@@ -1440,8 +1450,8 @@
             break;
         }
         next = iter->next;
-        lyd_unlink_ignore_lyds(iter);
-        lyds_link_data_node(leader_dst, iter, root_meta_dst, rbn);
+        lyd_unlink_ignore_lyds(first_src, iter);
+        lyds_link_data_node(first_dst, leader_dst, iter, root_meta_dst, rbn);
         lyd_insert_hash(iter);
     }
     *next_p = iter;
@@ -1460,6 +1470,7 @@
  * At the end of the lyds_merge_nodes2(), rbt_src will move and become rbt_dst.
  *
  * @param[in,out] leader_dst First instance of the destination (leaf-)list.
+ * @param[in,out] first_src First sibling node, source.
  * @param[in] leader_src First instance of the source (leaf-)list.
  * @param[in,out] rbt_src Root of the source Red-black tree.
  * @param[out] dst_iter Last merged node into the @p rbt_src.
@@ -1467,7 +1478,7 @@
  * @return LY_ERR value.
  */
 static LY_ERR
-lyds_merge_nodes2_front(struct lyd_node **leader_dst, struct lyd_node *leader_src,
+lyds_merge_nodes2_front(struct lyd_node **leader_dst, struct lyd_node **first_src, struct lyd_node *leader_src,
         struct rb_node **rbt_src, struct rb_node **dst_iter, struct lyd_node **next_p)
 {
     LY_ERR ret;
@@ -1482,7 +1493,7 @@
     prev = rb_prev(*dst_iter);
     dst = *leader_dst;
     for (iter = prev; iter; iter = rb_prev(iter)) {
-        lyd_unlink_ignore_lyds(RBN_DNODE(iter));
+        lyd_unlink_ignore_lyds(first_src, RBN_DNODE(iter));
         lyd_insert_before_node(dst, RBN_DNODE(iter));
         lyd_insert_hash(RBN_DNODE(iter));
         dst = RBN_DNODE(iter);
@@ -1502,15 +1513,17 @@
  * Create red-black nodes from destination (leaf-)list and insert them into @p rbt_src.
  * At the end of the lyds_merge_nodes2(), rbt_src will move and become rbt_dst.
  *
+ * @param[in,out] first_dst First sibling node, destination.
  * @param[in,out] leader_dst First instance of the destination (leaf-)list.
+ * @param[in,out] first_src First sibling node, source.
  * @param[in,out] rbt_src Root of the source Red-black tree.
  * @param[out] dst_iter Last merged node into the @p rbt_src.
  * @param[out] next_p On error, points to data node which failed to merge.
  * @return LY_ERR value.
  */
 static LY_ERR
-lyds_merge_nodes2_among(struct lyd_node **leader_dst, struct rb_node **rbt_src, struct rb_node **dst_iter,
-        struct lyd_node **next_p)
+lyds_merge_nodes2_among(struct lyd_node **first_dst, struct lyd_node **leader_dst, struct lyd_node **first_src,
+        struct rb_node **rbt_src, struct rb_node **dst_iter, struct lyd_node **next_p)
 {
     LY_ERR ret;
     struct rb_node *rbn, *iter, *rbn_prev;
@@ -1528,9 +1541,9 @@
         dst = node;
         /* move source data nodes between next-to-last inserted node and last inserted node */
         for (iter = rb_next(rbn_prev); iter != rbn; iter = rb_next(iter)) {
-            lyd_unlink_ignore_lyds(RBN_DNODE(iter));
+            lyd_unlink_ignore_lyds(first_src, RBN_DNODE(iter));
             /* insert source data nodes into destination (leaf-)list */
-            lyd_insert_after_node(dst, RBN_DNODE(iter));
+            lyd_insert_after_node(first_dst, dst, RBN_DNODE(iter));
             lyd_insert_hash(RBN_DNODE(iter));
             dst = RBN_DNODE(iter);
         }
@@ -1548,11 +1561,14 @@
  * Create red-black nodes from destination (leaf-)list and insert them into @p rbt_src.
  * At the end of the lyds_merge_nodes2(), rbt_src will move and become rbt_dst.
  *
+ * @param[in,out] first_dst First sibling node, destination.
+ * @param[in,out] first_src First sibling node, source.
  * @param[in] dst_iter Last merged node into the @p rbt_src.
  * @param[out] next_p Data node located after source (leaf-)list.
  */
 static void
-lyds_merge_nodes2_back(struct rb_node *dst_iter, struct lyd_node **next_p)
+lyds_merge_nodes2_back(struct lyd_node **first_dst, struct lyd_node **first_src, struct rb_node *dst_iter,
+        struct lyd_node **next_p)
 {
     struct rb_node *iter, *begin;
     struct lyd_node *dst;
@@ -1562,8 +1578,8 @@
     dst = RBN_DNODE(dst_iter);
     for (iter = begin; iter; iter = rb_next(iter)) {
         *next_p = RBN_DNODE(iter)->next;
-        lyd_unlink_ignore_lyds(RBN_DNODE(iter));
-        lyd_insert_after_node(dst, RBN_DNODE(iter));
+        lyd_unlink_ignore_lyds(first_src, RBN_DNODE(iter));
+        lyd_insert_after_node(first_dst, dst, RBN_DNODE(iter));
         lyd_insert_hash(RBN_DNODE(iter));
         dst = RBN_DNODE(iter);
     }
@@ -1575,7 +1591,9 @@
  * Create red-black nodes from destination (leaf-)list and insert them into @p rbt_src.
  * At the end of the lyds_merge_nodes2(), rbt_src will move and become rbt_dst.
  *
+ * @param[in,out] first_dst First sibling node, destination.
  * @param[in,out] leader_dst First instance of the destination (leaf-)list.
+ * @param[in,out] first_src First sibling node, source.
  * @param[in] leader_src First instance of the source (leaf-)list.
  * @param[in] root_meta_src Metadata 'lyds_tree' from @p leader_src.
  * @param[in] rbt_src Root of the source Red-black tree.
@@ -1584,28 +1602,30 @@
  * @return LY_ERR value.
  */
 static LY_ERR
-lyds_merge_nodes2(struct lyd_node **leader_dst, struct lyd_node *leader_src, struct lyd_meta *root_meta_src,
+lyds_merge_nodes2(struct lyd_node **first_dst, struct lyd_node **leader_dst,
+        struct lyd_node **first_src, struct lyd_node *leader_src, struct lyd_meta *root_meta_src,
         struct rb_node *rbt_src, struct lyd_node **next_p)
 {
     LY_ERR ret;
     struct rb_node *dst_iter;
 
     /* merge first destination node, move source nodes which belongs before this node */
-    ret = lyds_merge_nodes2_front(leader_dst, leader_src, &rbt_src, &dst_iter, next_p);
+    ret = lyds_merge_nodes2_front(leader_dst, first_src, leader_src, &rbt_src, &dst_iter, next_p);
     LY_CHECK_GOTO(ret, cleanup);
+    *first_dst = !(*first_dst)->prev->next ? *first_dst : *leader_dst;
 
     /* RB tree si moved from source to destination (leaf-)list */
     lyds_move_meta(*leader_dst, root_meta_src);
 
     /* merge the rest of the destination nodes, move corresponding source nodes */
-    ret = lyds_merge_nodes2_among(leader_dst, &rbt_src, &dst_iter, next_p);
+    ret = lyds_merge_nodes2_among(first_dst, leader_dst, first_src, &rbt_src, &dst_iter, next_p);
     LY_CHECK_GOTO(ret, cleanup);
 
     /* move the rest of the source nodes */
-    lyds_merge_nodes2_back(dst_iter, next_p);
+    lyds_merge_nodes2_back(first_dst, first_src, dst_iter, next_p);
 
     if (*next_p && ((*next_p)->schema == (*leader_dst)->schema)) {
-        ret = lyds_merge_nodes1(leader_dst, root_meta_src, rbt_src, *next_p, next_p);
+        ret = lyds_merge_nodes1(first_dst, leader_dst, root_meta_src, rbt_src, first_src, *next_p, next_p);
     }
 
 cleanup:
@@ -1620,17 +1640,20 @@
  * Possible improvement: find out which rb_tree is bigger and then move nodes into it.
  * Current implementation blindly guesses that the source Red-black tree is smaller.
  *
+ * @param[in,out] first_dst First sibling node, destination.
  * @param[in,out] leader_dst First instance of the destination (leaf-)list.
  * @param[in] root_meta_dst Metadata 'lyds_tree' from @p leader_dst.
  * @param[in] rbt_dst Root of the destination Red-black tree.
+ * @param[in,out] first_src First sibling node, source.
  * @param[in] root_meta_src Metadata 'lyds_tree' from @p leader_src.
  * @param[in] rbt_src Root of the source Red-black tree.
  * @param[out] next_p Data node located after source (leaf-)list.
  * @return LY_ERR value.
  */
 static LY_ERR
-lyds_merge_nodes3(struct lyd_node **leader_dst, struct lyd_meta *root_meta_dst, struct rb_node *rbt_dst,
-        struct lyd_meta *root_meta_src, struct rb_node *rbt_src, struct lyd_node **next_p)
+lyds_merge_nodes3(struct lyd_node **first_dst, struct lyd_node **leader_dst, struct lyd_meta *root_meta_dst,
+        struct rb_node *rbt_dst, struct lyd_node **first_src, struct lyd_meta *root_meta_src,
+        struct rb_node *rbt_src, struct lyd_node **next_p)
 {
     struct rb_node *iter, *iter_state;
     struct lyd_node *node;
@@ -1645,22 +1668,23 @@
         *next_p = node->next;
         RBN_RESET(iter, node);
         rb_insert_node(&rbt_dst, iter, NULL);
-        lyd_unlink_ignore_lyds(node);
-        lyds_link_data_node(leader_dst, node, root_meta_dst, iter);
+        lyd_unlink_ignore_lyds(first_src, node);
+        lyds_link_data_node(first_dst, leader_dst, node, root_meta_dst, iter);
         lyd_insert_hash(node);
     }
 
     if (!*next_p || ((*next_p)->schema != (*leader_dst)->schema)) {
         RBT_SET(root_meta_dst, rbt_dst);
     } else {
-        LY_CHECK_RET(lyds_merge_nodes1(leader_dst, root_meta_dst, rbt_dst, *next_p, next_p));
+        LY_CHECK_RET(lyds_merge_nodes1(first_dst, leader_dst, root_meta_dst, rbt_dst, first_src, *next_p, next_p));
     }
 
     return LY_SUCCESS;
 }
 
 LY_ERR
-lyds_merge(struct lyd_node **leader_dst, struct lyd_node *leader_src, struct lyd_node **next_p)
+lyds_merge(struct lyd_node **first_dst, struct lyd_node **leader_dst, struct lyd_node **first_src,
+        struct lyd_node *leader_src, struct lyd_node **next_p)
 {
     LY_ERR ret = LY_SUCCESS;
     struct rb_node *rbt_dst, *rbt_src;
@@ -1679,19 +1703,18 @@
     if (!rbt_dst && !rbt_src) {
         /* create RB tree from destination nodes, merge and move source nodes */
         LY_CHECK_RET(lyds_create_metadata(*leader_dst, &root_meta_dst));
-        LY_CHECK_RET(lyds_additionally_create_rb_tree(leader_dst, root_meta_dst, &rbt_dst));
-        ret = lyds_merge_nodes1(leader_dst, root_meta_dst, rbt_dst, leader_src, next_p);
+        LY_CHECK_RET(lyds_additionally_create_rb_tree(first_dst, leader_dst, root_meta_dst, &rbt_dst));
+        ret = lyds_merge_nodes1(first_dst, leader_dst, root_meta_dst, rbt_dst, first_src, leader_src, next_p);
     } else if (rbt_dst && !rbt_src) {
         /* just merge and move source nodes */
-        ret = lyds_merge_nodes1(leader_dst, root_meta_dst, rbt_dst, leader_src, next_p);
+        ret = lyds_merge_nodes1(first_dst, leader_dst, root_meta_dst, rbt_dst, first_src, leader_src, next_p);
     } else if (!rbt_dst && rbt_src) {
         /* merge destination nodes with RB tree, move RB tree and source nodes */
-        ret = lyds_merge_nodes2(leader_dst, leader_src, root_meta_src, rbt_src, next_p);
+        ret = lyds_merge_nodes2(first_dst, leader_dst, first_src, leader_src, root_meta_src, rbt_src, next_p);
     } else {
         /* merge and move source nodes, release source RB tree */
         assert(rbt_dst && rbt_src);
-        lyds_merge_nodes3(leader_dst, root_meta_dst, rbt_dst,
-                root_meta_src, rbt_src, next_p);
+        lyds_merge_nodes3(first_dst, leader_dst, root_meta_dst, rbt_dst, first_src, root_meta_src, rbt_src, next_p);
     }
 
     return ret;
diff --git a/src/tree_data_sorted.h b/src/tree_data_sorted.h
index 848ac62..65dce57 100644
--- a/src/tree_data_sorted.h
+++ b/src/tree_data_sorted.h
@@ -90,12 +90,13 @@
  * The function automatically take care of lyds_create_metadata() and lyds_create_tree() calls.
  * Hash for data nodes is not added.
  *
+ * @param[in,out] first_sibling First sibling node, used for optimization.
  * @param[in,out] leader First instance of the (leaf-)list. After executing the function,
  * @p leader does not have to be be first if @p node was inserted before @p leader.
  * @param[in] node A single (without siblings) node or tree to be inserted. It must be unlinked.
  * @return LY_ERR value.
  */
-LY_ERR lyds_insert(struct lyd_node **leader, struct lyd_node *node);
+LY_ERR lyds_insert(struct lyd_node **first_sibling, struct lyd_node **leader, struct lyd_node *node);
 
 /**
  * @brief Insert the @p node into BST and into @p leader's siblings and use @p pool.
@@ -146,12 +147,14 @@
  * The second (leaf-)list is unlinked from the rest of the data nodes.
  * The hash for the data nodes is removed.
  *
+ * @param[in,out] first_sibling First sibling node, used for optimization.
  * @param[in] leader First instance of (leaf-)list.
  * @param[in] node Node in the (leaf-)list from which the second (leaf-)list will start.
  * @param[out] next Data node located after the second (leaf-)list.
  * The rest of the (leaf-)list nodes will belong under the second (leaf-)list.
  */
-void lyds_split(struct lyd_node *leader, struct lyd_node *node, struct lyd_node **next);
+void lyds_split(struct lyd_node **first_sibling, struct lyd_node *leader, struct lyd_node *node,
+        struct lyd_node **next);
 
 /**
  * @brief Merge source (leaf-)list nodes into destination (leaf-)list nodes.
@@ -159,14 +162,17 @@
  * Pointers in sibling data nodes (lyd_node) are modified.
  * The hash for the data nodes will be adjusted.
  *
+ * @param[in,out] first_dst First sibling node, destination.
  * @param[in,out] leader_dst Destination (leaf-)list, first instance. It may not contain
  * the lyds_tree metadata or BST. After merge @p leader_dst can be reset to new leader.
+ * @param[in,out] first_src First sibling node, source.
  * @param[in] leader_src Source (leaf-)list, first instance. It may not contain the lyds_tree metadata or BST.
  * @param[out] next Data node located after source (leaf-)list.
  * On error, points to data node which failed to merge.
  * @return LY_ERR value.
  */
-LY_ERR lyds_merge(struct lyd_node **leader_dst, struct lyd_node *leader_src, struct lyd_node **next);
+LY_ERR lyds_merge(struct lyd_node **first_dst, struct lyd_node **leader_dst, struct lyd_node **first_src,
+        struct lyd_node *leader_src, struct lyd_node **next);
 
 /**
  * @brief Compare (sort) 2 data nodes.
diff --git a/src/validation.c b/src/validation.c
index 74653a7..1faf3f1 100644
--- a/src/validation.c
+++ b/src/validation.c
@@ -2090,7 +2090,7 @@
     /* restore operation tree */
     lyd_unlink(op_subtree);
     if (op_sibling_before) {
-        lyd_insert_after_node(op_sibling_before, op_subtree);
+        lyd_insert_after_node(NULL, op_sibling_before, op_subtree);
         lyd_insert_hash(op_subtree);
     } else if (op_sibling_after) {
         lyd_insert_before_node(op_sibling_after, op_subtree);