Add binary search tree

Add api, basic implementation and tests.
TODO: Add balancing code

Bug: 141329802
Change-Id: I819bd2ffc35d139f1f6f357eb71bba839c7b2bd9

6 files changed, 1125 insertions, 0 deletions

diff --git a/lib/binary_search_tree/binary_search_tree.c b/lib/binary_search_tree/binary_search_tree.c
new file mode 100644
index 00000000..144200b9
--- /dev/null
+++ b/lib/binary_search_tree/binary_search_tree.c
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2019 LK Trusty Authors. All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <lib/binary_search_tree.h>
+
+/**
+ * bst_is_right_child - Internal helper function
+ * @node:   Node to check.
+ *
+ * Return: %true if @node is the right child of @node->parent. %false if
+ *         @node->parent is %NULL or if @node is the left child of
+ *         @node->parent.
+ */
+static bool bst_is_right_child(struct bst_node *node) {
+    DEBUG_ASSERT(node);
+    DEBUG_ASSERT(!node->parent || node->parent->child[0] == node ||
+                 node->parent->child[1] == node);
+    return node->parent && node->parent->child[1] == node;
+}
+
+/**
+ * bst_parent_ptr - Internal helper function
+ * @root:   Tree.
+ * @node:   Node in @root.
+ *
+ * Return: Pointer where @node is linked in the tree. If @node is the root node
+ * this is &root->root, otherwise it is the address of child pointer in the
+ * parent node of @node that points to @node.
+ */
+static struct bst_node **bst_parent_ptr(struct bst_root *root,
+                                        struct bst_node *node) {
+    DEBUG_ASSERT(root);
+    DEBUG_ASSERT(node);
+    struct bst_node **parent_ptr = node->parent ?
+        &node->parent->child[bst_is_right_child(node)] : &root->root;
+    DEBUG_ASSERT(*parent_ptr == node);
+    return parent_ptr;
+}
+
+/**
+ * bst_link_node - Internal helper function
+ * @parent:         Target node.
+ * @is_right_child: Index of child to set.
+ * @child:          New child node.
+ *
+ * Set child node in @parent. If @child is not %NULL, update it to point to
+ * @parent.
+ */
+static void bst_link_node(struct bst_node *parent,
+                          bool is_right_child,
+                          struct bst_node *child) {
+    parent->child[is_right_child] = child;
+    if (child) {
+        child->parent = parent;
+    }
+}
+
+/**
+ * bst_move_node - Internal helper function
+ * @root:       Tree.
+ * @old_node:   Node to unlink.
+ * @new_node:   Node to link where @old_node was.
+ *
+ * Replace node in @root at @old_node with @new_node.
+ */
+static void bst_move_node(struct bst_root *root,
+                          struct bst_node *old_node,
+                          struct bst_node *new_node) {
+    DEBUG_ASSERT(root);
+    DEBUG_ASSERT(old_node);
+
+    *bst_parent_ptr(root, old_node) = new_node;
+    if (new_node) {
+        new_node->parent = old_node->parent;
+    }
+    old_node->parent = NULL;
+}
+
+/**
+ * bst_find_edge - Internal helper function
+ * @node:   Node to start search at.
+ * @edge:   Direction if search.
+ *
+ * Return: leftmost (if @edge is %false) or rightmost (if @edge is %true) node
+ * in subtree with @node as root.
+ */
+static struct bst_node *bst_find_edge(struct bst_node *node, bool edge) {
+    struct bst_node *saved_node;
+
+    DEBUG_ASSERT(node);
+
+    do {
+        saved_node = node;
+        node = node->child[edge];
+    } while (node);
+
+    return saved_node;
+}
+
+/**
+ * bst_delete_all_helper - Internal helper function
+ * @root:   Tree.
+ * @node:   Node to delete (most be the leftmost node in @root).
+ *
+ * Helper function to delete leftmost node in @root, assuming all other nodes
+ * will be deleted next.
+ */
+void bst_delete_all_helper(struct bst_root *root, struct bst_node *node) {
+    DEBUG_ASSERT(root);
+    DEBUG_ASSERT(node);
+    DEBUG_ASSERT(!node->child[0]);
+    bst_move_node(root, node, node->child[1]);
+}
+
+void bst_delete(struct bst_root *root, struct bst_node *node) {
+    DEBUG_ASSERT(root);
+    DEBUG_ASSERT(node);
+
+    struct bst_node *new_child;
+    bool node_is_right_child = bst_is_right_child(node);
+    if (!node->child[0]) {
+        /*
+         * If @node has no left child, link its right child in its place. (The
+         * right child could be %NULL in this case)
+         */
+        new_child = node->child[1];
+    } else if (!node->child[1]) {
+        /*
+         * If @node has no right child, link its left child in its place.
+         */
+        DEBUG_ASSERT(node->child[0]);
+        new_child = node->child[0];
+    } else {
+        /*
+         * If @node has both left and right children, delete (from the tree
+         * structure point of view) the left-most node in the right sub-tree or
+         * the right-most node in the left sub-tree instead. Either side would
+         * work.
+         */
+        struct bst_node *edge_node = bst_find_edge(
+                node->child[!node_is_right_child], node_is_right_child);
+        struct bst_node *edge_child = edge_node->child[!node_is_right_child];
+        bst_move_node(root, edge_node, edge_child);
+
+        new_child = edge_node;
+        DEBUG_ASSERT(new_child);
+        bst_link_node(new_child, 0, node->child[0]);
+        bst_link_node(new_child, 1, node->child[1]);
+    }
+    bst_move_node(root, node, new_child);
+    node->rank = 0;
+}
+
+/**
+ * bst_prev_next - Internal helper function
+ * @root:       Tree.
+ * @node:       Node to move from.
+ * @dir_next:   Directon to move.
+ *
+ * Return: If @node is %NULL and @dir_next is %false, right-most node in @root.
+ *         If @node is %NULL and @dir_next is %true, left-most node in @root.
+ *         If @node is not %NULL and @dir_next is %false, right-most node to the
+ *         left of @node.
+ *         If @node is not %NULL and @dir_next is %true, left-most node to the
+ *         right of @node.
+ *         %NULL if the node described above does not exist.
+ */
+static struct bst_node *bst_prev_next(const struct bst_root *root,
+                                      struct bst_node *node,
+                                      bool dir_next) {
+    DEBUG_ASSERT(root);
+
+    struct bst_node *next_child = node ? node->child[dir_next] : root->root;
+
+    if (!node && !next_child) {
+        return NULL; /* Empty tree */
+    }
+
+    /*
+     * Comments below assume @dir_next is %true. For the @dir_next is %false
+     * case, swap left and right.
+     */
+    if (next_child) {
+        /* There is a right child, return the left-most node in that subtree */
+        return bst_find_edge(next_child, !dir_next);
+    } else {
+        /* No right child, next node is the first right parent */
+        struct bst_node *next_parent = node;
+        while (bst_is_right_child(next_parent) == dir_next) {
+            next_parent = next_parent->parent;
+            if (!next_parent) {
+                return NULL;
+            }
+        }
+        return next_parent->parent;
+    }
+}
+
+struct bst_node *bst_prev(struct bst_root *root, struct bst_node *node) {
+    return bst_prev_next(root, node, false);
+}
+
+struct bst_node *bst_next(const struct bst_root *root, struct bst_node *node) {
+    return bst_prev_next(root, node, true);
+}
diff --git a/lib/binary_search_tree/hosttest/binary_search_tree_test.cpp b/lib/binary_search_tree/hosttest/binary_search_tree_test.cpp
new file mode 100644
index 00000000..c30a90ca
--- /dev/null
+++ b/lib/binary_search_tree/hosttest/binary_search_tree_test.cpp
@@ -0,0 +1,547 @@
+/*
+ * Copyright (c) 2019 LK Trusty Authors. All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <lib/binary_search_tree.h>
+#include <lk/compiler.h>
+#include <stdlib.h>
+
+#include "gtest/gtest.h"
+
+class BstTest : public testing::TestWithParam<bool> {
+};
+
+struct bst_test_entry {
+    struct bst_node node;
+};
+
+INSTANTIATE_TEST_SUITE_P(BstTestMirror, BstTest, testing::Bool());
+
+static int bst_test_compare(struct bst_node *a, struct bst_node *b) {
+    if (BstTest::GetParam()) {
+        return a > b ? 1 : a < b ? -1 : 0;
+    } else {
+        return a < b ? 1 : a > b ? -1 : 0;
+    }
+}
+
+static struct bst_node *bst_test_search(struct bst_root *root,
+                                        struct bst_node *node) {
+    return bst_search(root, node, bst_test_compare);
+}
+
+static struct bst_node *bst_node(struct bst_node nodes[], size_t index[],
+                                 size_t i, size_t i_size) {
+    if (BstTest::GetParam()) {
+        i = i_size - 1 - i;
+    }
+    if (index) {
+        i = index[i];
+    }
+    return &nodes[i];
+}
+
+std::ostream& operator<<(std::ostream& os, const struct bst_node* node) {
+  return os <<
+    "Node (" << (void*)node << "):\n" <<
+    "  Parent:" << (void*)node->parent << "\n" <<
+    "  Rank: " << node->rank << "\n" <<
+    "  Left child: " << (void*)node->child[0] << "\n" <<
+    "  Right child: " << (void*)node->child[1] << "\n";
+}
+
+std::ostream& operator<<(std::ostream& os, const struct bst_root* root) {
+  return os << "Root (" << (void*)root << ")\n";
+}
+
+/**
+ * bst_subtree_depth - Internal helper function
+ * @node:   Root of a subtree.
+ *
+ * Return: Depth of subtree at @node, or 0 if @node is %NULL.
+ */
+static size_t bst_subtree_depth(struct bst_node *node) {
+    if (!node) {
+        return 0;
+    } else {
+        size_t child_depth[2];
+        for (size_t i = 0; i < 2; i++) {
+            child_depth[i] = bst_subtree_depth(node->child[i]);
+        }
+        return 1 + MAX(child_depth[0], child_depth[1]);
+    }
+}
+
+/**
+ * bst_depth - Debug function - return depth of tree
+ * @root:   Tree.
+ *
+ * Return: Depth of @root.
+ */
+static size_t bst_depth(struct bst_root *root) {
+    return bst_subtree_depth(root->root);
+}
+
+static bool bst_is_right_child(struct bst_node *node) {
+    DEBUG_ASSERT(node);
+    DEBUG_ASSERT(!node->parent || node->parent->child[0] == node ||
+                 node->parent->child[1] == node);
+    return node->parent && node->parent->child[1] == node;
+}
+
+static void bst_test_check_node(struct bst_root *root, struct bst_node *node) {
+    if (node->parent) {
+        ASSERT_NE(root->root, node) << node << root;
+        ASSERT_EQ(node->parent->child[bst_is_right_child(node)], node) << node << root;
+    } else {
+        ASSERT_EQ(root->root, node) << node << root;;
+    }
+}
+
+static void print_rep(char ch, size_t count) {
+    for (size_t i = 0; i < count; i++) {
+        printf("%c", ch);
+    }
+}
+
+static size_t bst_test_node_num(struct bst_node *node,
+                                struct bst_node nodes[]) {
+    return nodes ? node - nodes : 0;
+}
+
+static void bst_test_print_node_at(struct bst_root *root,
+                                   struct bst_node nodes[], size_t row,
+                                   size_t col, size_t depth) {
+    size_t space = (1 << (depth - row)) - 2;
+    struct bst_node *node = root->root;
+    for (size_t mask = 1 << row >> 1; node && mask; mask >>= 1) {
+        node = node->child[!!(col & mask)];
+    }
+    if (col) {
+        printf("    ");
+    }
+    print_rep(' ', space);
+    print_rep(node && node->child[0] ? '_' : ' ', space);
+    if (node) {
+        printf("%02zur%01zu", bst_test_node_num(node, nodes), node->rank);
+    } else {
+        printf("    ");
+    }
+    print_rep(node && node->child[1] ? '_' : ' ', space);
+    print_rep(' ', space);
+    /*
+     *               ______________0004______________
+     *       ______0003______                ______0003______
+     *   __0002__        __0002__        __0002__        __0002__
+     * 0001    0001    0001    0001    0001    0001    0001    0001
+     */
+}
+
+static void bst_test_print_tree(struct bst_root *root, struct bst_node nodes[]) {
+    size_t depth = bst_depth(root);
+    printf("Tree depth %zu\n", depth);
+    for (size_t row = 0; row < depth; row++) {
+        for (size_t col = 0; col < 1 << row; col++) {
+            bst_test_print_node_at(root, nodes, row, col, depth);
+        }
+        printf("\n");
+    }
+}
+
+static void bst_test_check_tree_valid(struct bst_root *root) {
+    struct bst_node *node = 0;
+    while (true) {
+        node = bst_next(root, node);
+        if (!node) {
+            break;
+        }
+        bst_test_check_node(root, node);
+    }
+    if(::testing::Test::HasFailure()) {
+        bst_test_print_tree(root, NULL);
+    }
+}
+
+static void bst_test_check_array(struct bst_root *root,
+                                     struct bst_node nodes[],
+                                     size_t index[],
+                                     size_t count,
+                                     struct bst_node *left,
+                                     struct bst_node *right) {
+    for (size_t i = 0; i < count; i++) {
+        struct bst_node *node = bst_node(nodes, index, i, count);
+        EXPECT_EQ(bst_test_search(root, node), node) << "Node: " << (node - nodes) << "\n";
+    }
+
+    if (!count) {
+        EXPECT_EQ(bst_next(root, left), right);
+        EXPECT_EQ(bst_prev(root, right), left);
+        return;
+    }
+
+    EXPECT_EQ(bst_next(root, left), bst_node(nodes, index, 0, count));
+    EXPECT_EQ(bst_prev(root, bst_node(nodes, index, 0, count)), left);
+    for (size_t i = 0; i < count - 1; i++) {
+        struct bst_node *node = bst_node(nodes, index, i, count);
+        struct bst_node *next = bst_node(nodes, index, i + 1, count);
+        EXPECT_EQ(bst_next(root, node), next) << "node: " << (node - nodes) << ", next: " << (next - nodes);
+        EXPECT_EQ(bst_prev(root, next), node) << "next: " << (next - nodes) << ", node: " << (node - nodes);
+    }
+    EXPECT_EQ(bst_next(root, bst_node(nodes, index, count - 1, count)), right);
+    EXPECT_EQ(bst_prev(root, right), bst_node(nodes, index, count - 1, count));}
+
+#define bst_test_check(root, nodes, items...) do { \
+    SCOPED_TRACE("bst_test_check"); \
+    size_t index[] = {items}; \
+    bst_test_check_array(root, nodes, index, countof(index), NULL, NULL); \
+    if (HasFatalFailure()) return; \
+} while(0)
+
+static void bst_test_insert_func(struct bst_root *root, struct bst_node *node) {
+    ASSERT_EQ(node->rank, 0U);
+    bst_insert(root, node, bst_test_compare);
+    bst_test_check_tree_valid(root);
+    EXPECT_EQ(bst_test_search(root, node), node);
+}
+
+#define bst_test_insert(root, node) do { \
+    SCOPED_TRACE(testing::Message() << "bst_insert" << node); \
+    bst_test_insert_func(root, node); \
+    if (HasFatalFailure()) return; \
+} while(0)
+
+#define bst_test_insert_check(root, nodes, insert, items...) do { \
+    bst_test_insert(root, &nodes[insert]); \
+    bst_test_check(root, nodes, items); \
+} while(0)
+
+#define bst_test_delete_check(root, nodes, insert, items...) do { \
+    bst_test_delete(root, &nodes[insert]); \
+    bst_test_check(root, nodes, items); \
+} while(0)
+
+static void bst_test_delete_func(struct bst_root *root, struct bst_node *node) {
+    bst_delete(root, node);
+    bst_test_check_tree_valid(root);
+    EXPECT_EQ(bst_test_search(root, node), nullptr);
+}
+
+#define bst_test_delete(root, node) do { \
+    SCOPED_TRACE(testing::Message() << "bst_delete" << node); \
+    bst_test_delete_func(root, node); \
+    if (HasFatalFailure()) return; \
+} while(0)
+
+/*
+ * Test init api
+ */
+TEST(BstTest, InitRootValue) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    EXPECT_EQ(root.root, nullptr);
+}
+
+TEST(BstTest, InitRootFunction) {
+    struct bst_root root;
+    memset(&root, 0xff, sizeof(root));
+    bst_root_initialize(&root);
+    EXPECT_EQ(root.root, nullptr);
+}
+
+TEST(BstTest, InitNodeValue) {
+    struct bst_node node = BST_NODE_INITIAL_VALUE;
+    EXPECT_EQ(node.rank, 0U);
+}
+
+TEST(BstTest, InitNodeFnction) {
+    struct bst_node node;
+    memset(&node, 0xff, sizeof(node));
+    bst_node_initialize(&node);
+    EXPECT_EQ(node.rank, 0U);
+}
+
+/*
+ * Simple tests to check that api return expected results.
+ */
+TEST_P(BstTest, InsertAscending) {
+    /* Insert nodes in ascending order (or decending for mirrored test) */
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 14] = BST_NODE_INITIAL_VALUE};
+
+    bst_test_check(&root, nodes);
+    for (size_t i = 0; i < countof(nodes); i++) {
+        bst_test_insert(&root, &nodes[i]);
+        if (GetParam()) {
+            EXPECT_EQ(bst_prev(&root, &nodes[i]), nullptr);
+        } else {
+            EXPECT_EQ(bst_next(&root, &nodes[i]), nullptr);
+        }
+    }
+    bst_test_check_array(&root, nodes, NULL, countof(nodes), NULL, NULL);
+    EXPECT_GE(bst_depth(&root), 4U); /* Minimum depth for a binary tree */
+}
+
+TEST_P(BstTest, InsertBalanced) {
+    /*
+     *         ______7_____
+     *        /            \
+     *     __3__           _11_
+     *    /     \         /    \
+     *   1       5       9      13
+     *  / \     / \     / \    /  \
+     * 0   2   4   6   8  10  12  14
+     */
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 14] = BST_NODE_INITIAL_VALUE};
+    size_t index[] = { 7, 3, 11, 1, 5, 9, 13, 0, 2, 4, 6, 8, 10, 12, 14 };
+    for (size_t i = 0; i < countof(index); i++) { \
+        bst_test_insert(&root, &nodes[index[i]]);
+    }
+    bst_test_check_array(&root, nodes, NULL, countof(nodes), NULL, NULL);
+    EXPECT_EQ(bst_depth(&root), 4U);
+}
+
+TEST_P(BstTest, DeleteOnlyEntry) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 0] = BST_NODE_INITIAL_VALUE};
+    bst_test_insert_check(&root, nodes, 0, 0);
+    /*
+     * 0
+     */
+    bst_test_delete_check(&root, nodes, 0);
+}
+
+TEST_P(BstTest, DeleteRootOneChild) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 1] = BST_NODE_INITIAL_VALUE};
+    bst_test_insert_check(&root, nodes, 1, 1);
+    bst_test_insert_check(&root, nodes, 0, 0, 1);
+    /*
+     *   1
+     *  /
+     * 0
+     */
+    bst_test_delete_check(&root, nodes, 1, 0);
+}
+
+TEST_P(BstTest, DeleteRootTwoChildren) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 2] = BST_NODE_INITIAL_VALUE};
+
+    bst_test_insert_check(&root, nodes, 1, 1);
+    bst_test_insert_check(&root, nodes, 0, 0, 1);
+    bst_test_insert_check(&root, nodes, 2, 0, 1, 2);
+    /*
+     *   1
+     *  / \
+     * 0   2
+     */
+    bst_test_delete_check(&root, nodes, 1, 0, 2);
+}
+
+TEST_P(BstTest, DeleteRootManyChildrenOneSide) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 4] = BST_NODE_INITIAL_VALUE};
+
+    bst_test_insert_check(&root, nodes, 3, 3);
+    bst_test_insert_check(&root, nodes, 1, 1, 3);
+    bst_test_insert_check(&root, nodes, 4, 1, 3, 4);
+    bst_test_insert_check(&root, nodes, 0, 0, 1, 3, 4);
+    bst_test_insert_check(&root, nodes, 2, 0, 1, 2, 3, 4);
+    /*
+     *     __3__
+     *    /     \
+     *   1       4
+     *  / \
+     * 0   2
+     */
+    bst_test_delete_check(&root, nodes, 3, 0, 1, 2, 4);
+}
+
+TEST_P(BstTest, DeleteRootManyChildrenBothSides) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 6] = BST_NODE_INITIAL_VALUE};
+
+    bst_test_insert_check(&root, nodes, 3, 3);
+    bst_test_insert_check(&root, nodes, 1, 1, 3);
+    bst_test_insert_check(&root, nodes, 5, 1, 3, 5);
+    bst_test_insert_check(&root, nodes, 0, 0, 1, 3, 5);
+    bst_test_insert_check(&root, nodes, 2, 0, 1, 2, 3, 5);
+    bst_test_insert_check(&root, nodes, 4, 0, 1, 2, 3, 4, 5);
+    bst_test_insert_check(&root, nodes, 6, 0, 1, 2, 3, 4, 5, 6);
+    /*
+     *     __3__
+     *    /     \
+     *   1       5
+     *  / \     / \
+     * 0   2   4   6
+     */
+    bst_test_delete_check(&root, nodes, 3, 0, 1, 2, 4, 5, 6);
+}
+
+TEST_P(BstTest, DeleteEdge1) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 4] = BST_NODE_INITIAL_VALUE};
+
+    bst_test_insert_check(&root, nodes, 3, 3);
+    bst_test_insert_check(&root, nodes, 1, 1, 3);
+    bst_test_insert_check(&root, nodes, 4, 1, 3, 4);
+    bst_test_insert_check(&root, nodes, 0, 0, 1, 3, 4);
+    bst_test_insert_check(&root, nodes, 2, 0, 1, 2, 3, 4);
+    /*
+     *     __3__
+     *    /     \
+     *   1       4
+     *  / \
+     * 0   2
+     */
+
+    bst_test_delete_check(&root, nodes, 4, 0, 1, 2, 3);
+}
+
+TEST_P(BstTest, DeleteInternal) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 4] = BST_NODE_INITIAL_VALUE};
+
+    bst_test_insert_check(&root, nodes, 3, 3);
+    bst_test_insert_check(&root, nodes, 1, 1, 3);
+    bst_test_insert_check(&root, nodes, 4, 1, 3, 4);
+    bst_test_insert_check(&root, nodes, 0, 0, 1, 3, 4);
+    bst_test_insert_check(&root, nodes, 2, 0, 1, 2, 3, 4);
+    /*
+     *     __3__
+     *    /     \
+     *   1       4
+     *  / \
+     * 0   2
+     */
+    bst_test_delete_check(&root, nodes, 1, 0, 2, 3, 4);
+}
+
+TEST_P(BstTest, ForEveryEntry) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 254] = BST_NODE_INITIAL_VALUE};
+    struct bst_test_entry *entry;
+
+    for (size_t i = 0; i < countof(nodes); i++) {
+        bst_test_insert(&root, &nodes[i]);
+    }
+
+    size_t i = 0;
+    bst_for_every_entry(&root, entry, struct bst_test_entry, node) {
+        EXPECT_EQ(&entry->node, bst_node(nodes, NULL, i, countof(nodes)));
+        i++;
+    }
+    EXPECT_EQ(i, countof(nodes));
+}
+
+TEST_P(BstTest, ForEveryEntryExplicitDelete) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 254] = BST_NODE_INITIAL_VALUE};
+    struct bst_test_entry *entry;
+
+    for (size_t i = 0; i < countof(nodes); i++) {
+        bst_test_insert(&root, &nodes[i]);
+    }
+
+    size_t i = 0;
+    bst_for_every_entry(&root, entry, struct bst_test_entry, node) {
+        EXPECT_EQ(&entry->node, bst_node(nodes, NULL, i, countof(nodes)));
+        i++;
+        bst_delete(&root, &entry->node);
+    }
+    EXPECT_EQ(i, countof(nodes));
+
+    EXPECT_EQ(bst_next(&root, NULL), nullptr);
+}
+
+TEST_P(BstTest, ForEveryEntryDelete) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 254] = BST_NODE_INITIAL_VALUE};
+    struct bst_test_entry *entry;
+
+    for (size_t i = 0; i < countof(nodes); i++) {
+        bst_test_insert(&root, &nodes[i]);
+    }
+
+    size_t i = 0;
+    bst_for_every_entry_delete(&root, entry, struct bst_test_entry, node) {
+        EXPECT_EQ(&entry->node, bst_node(nodes, NULL, i, countof(nodes)));
+        i++;
+    }
+    EXPECT_EQ(i, countof(nodes));
+
+    EXPECT_EQ(bst_next(&root, NULL), nullptr);
+}
+
+TEST_P(BstTest, RandomInsert) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 999] = BST_NODE_INITIAL_VALUE};
+    for (size_t i = 0; i < countof(nodes);) {
+        struct bst_node *node = &nodes[lrand48() % countof(nodes)];
+        if (!node->rank) {
+            bst_test_insert(&root, node);
+            ASSERT_GE(node->rank, 1U);
+            EXPECT_EQ(bst_test_search(&root, node), node);
+            i++;
+        }
+    }
+}
+
+TEST_P(BstTest, RandomInsertRandomDelete) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 999] = BST_NODE_INITIAL_VALUE};
+    for (size_t i = 0; i < countof(nodes);) {
+        struct bst_node *node = &nodes[lrand48() % countof(nodes)];
+        if (!node->rank) {
+            bst_test_insert(&root, node);
+            ASSERT_GE(node->rank, 1U);
+            EXPECT_EQ(bst_test_search(&root, node), node);
+            i++;
+        }
+    }
+    for (size_t i = 0; i < countof(nodes);) {
+        struct bst_node *node = &nodes[lrand48() % countof(nodes)];
+        if (node->rank) {
+            bst_test_delete(&root, node);
+            EXPECT_EQ(node->rank, 0U);
+            EXPECT_EQ(bst_test_search(&root, node), nullptr) << node;
+            i++;
+        }
+    }
+}
+
+TEST_P(BstTest, RandomInsertDelete) {
+    struct bst_root root = BST_ROOT_INITIAL_VALUE;
+    struct bst_node nodes[] = {[0 ... 499] = BST_NODE_INITIAL_VALUE};
+    for (size_t i = 0; i < countof(nodes) * 100; i++) {
+        struct bst_node *node = &nodes[lrand48() % countof(nodes)];
+        if (node->rank) {
+            bst_test_delete(&root, node);
+            ASSERT_EQ(node->rank, 0U);
+            EXPECT_EQ(bst_test_search(&root, node), nullptr);
+        } else {
+            bst_test_insert(&root, node);
+            EXPECT_GE(node->rank, 1U);
+            EXPECT_EQ(bst_test_search(&root, node), node);
+        }
+    }
+}
diff --git a/lib/binary_search_tree/hosttest/rules.mk b/lib/binary_search_tree/hosttest/rules.mk
new file mode 100644
index 00000000..5d2b4fa8
--- /dev/null
+++ b/lib/binary_search_tree/hosttest/rules.mk
@@ -0,0 +1,45 @@
+#
+# Copyright (c) 2019 LK Trusty Authors. All Rights Reserved.
+#
+# Permission is hereby granted, free of charge, to any person obtaining
+# a copy of this software and associated documentation files
+# (the "Software"), to deal in the Software without restriction,
+# including without limitation the rights to use, copy, modify, merge,
+# publish, distribute, sublicense, and/or sell copies of the Software,
+# and to permit persons to whom the Software is furnished to do so,
+# subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be
+# included in all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+# IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+# CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+# TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+# SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+#
+
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+HOST_TEST := binary_search_tree_test
+
+GTEST_DIR := external/googletest/googletest
+
+HOST_SRCS := \
+        $(LOCAL_DIR)/binary_search_tree_test.cpp \
+        $(LOCAL_DIR)/../binary_search_tree.c \
+        $(GTEST_DIR)/src/gtest-all.cc \
+        $(GTEST_DIR)/src/gtest_main.cc \
+
+HOST_INCLUDE_DIRS := \
+        $(LOCAL_DIR)/../include \
+        $(GTEST_DIR)/include \
+        $(GTEST_DIR) \
+
+HOST_LIBS := \
+        stdc++ \
+        pthread \
+
+include make/host_test.mk
diff --git a/lib/binary_search_tree/include/lib/binary_search_tree.h b/lib/binary_search_tree/include/lib/binary_search_tree.h
new file mode 100644
index 00000000..e6bfd931
--- /dev/null
+++ b/lib/binary_search_tree/include/lib/binary_search_tree.h
@@ -0,0 +1,275 @@
+/*
+ * Copyright (c) 2019 LK Trusty Authors. All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <assert.h>
+#include <lk/compiler.h>
+#include <lk/macros.h>
+#include <stdbool.h>
+#include <stddef.h>
+
+__BEGIN_CDECLS
+
+/*
+ * lk defines DEBUG_ASSERT for debug build asserts. Enable those checks for
+ * host test as well.
+ */
+#ifndef DEBUG_ASSERT
+#define DEBUG_ASSERT(e) assert(e)
+#endif
+
+/**
+ * struct bst_node - Node in binary search tree.
+ * @rank:   Rank value used for rebalancing. 0 indicates node is not in a tree.
+ * @parent: Pointer to parent node or %NULL for root node.
+ * @child:  Array of pointers to child nodes. @child[0] points to the left child
+ *          and @child[1] points to the right child.
+ */
+struct bst_node {
+    size_t rank;
+    struct bst_node *parent;
+    struct bst_node *child[2];
+};
+
+struct bst_root {
+    struct bst_node *root;
+};
+
+#define BST_NODE_INITIAL_VALUE {0, NULL, {NULL, NULL}}
+#define BST_ROOT_INITIAL_VALUE {NULL}
+
+static inline void bst_node_initialize(struct bst_node *node) {
+    /* Set rank to an invalid value to detect double insertion. */
+    node->rank = 0;
+}
+
+static inline void bst_root_initialize(struct bst_root *root) {
+    root->root = NULL;
+}
+
+/**
+ * bst_compare_t - Compare function provided by caller
+ * @a: First node to compare
+ * @b: Second node to compare
+ *
+ * Return: a positive number if @b should be after @a, 0 if @b is a match for
+ * @a, a negative otherwise.
+ */
+typedef int (*bst_compare_t)(struct bst_node *a, struct bst_node *b);
+
+/**
+ * bst_search - Find a node in a binary search tree.
+ * @root:       Tree to search
+ * @node:       Dummy node containing key to search for.
+ * @compare:    Compare function.
+ *
+ * Find a node in a binary search tree. Use bst_search_type instead to get a
+ * pointer to the struct that contains @node.
+ *
+ * Note that if there are multiple matching nodes in the tree, the node returned
+ * may not be the leftmost matching node.
+ *
+ * Return: Node in @root matching @node, or %NULL if no matching node is found.
+ */
+static inline struct bst_node *bst_search(const struct bst_root *root,
+                                          struct bst_node *node,
+                                          bst_compare_t compare) {
+    DEBUG_ASSERT(root);
+    DEBUG_ASSERT(node);
+    DEBUG_ASSERT(compare);
+
+    struct bst_node *tree_node = root->root;
+    while (tree_node) {
+        int cmp = compare(tree_node, node);
+        if (!cmp) {
+            return tree_node;
+        }
+        tree_node = tree_node->child[cmp > 0];
+    }
+    return NULL;
+}
+
+/**
+ * bst_search - Find an item in a binary search tree.
+ * @root:       Tree to search
+ * @item:       Dummy item containing key to search for.
+ * @compare:    Compare function.
+ * @type:       Type of @item.
+ * @member:     Name of struct bst_node embedded in @type.
+ *
+ * Return: Item in @root matching @item, or %NULL if no matching node is found.
+ */
+#define bst_search_type(root, item, compare, type, member) \
+    containerof_null_safe(bst_search(root, &(item)->member, compare), type, \
+                          member)
+
+/**
+ * bst_insert - Insert node in tree.
+ * @root:       Tree.
+ * @node:       Node to insert.
+ * @compare:    Compare function.
+ *
+ * Insert @node in @root.
+ *
+ * Return: %true if @node was inserted. %false if a node matching @node is
+ * already in @root.
+ */
+static inline bool bst_insert(struct bst_root *root, struct bst_node *node,
+                              bst_compare_t compare) {
+    DEBUG_ASSERT(root);
+    DEBUG_ASSERT(node);
+    DEBUG_ASSERT(compare);
+    DEBUG_ASSERT(!node->rank);
+
+    struct bst_node *parent = NULL;
+    struct bst_node **parent_ptr = &root->root;
+    int diff;
+    bool is_right_child = false;
+    while (true) {
+        struct bst_node *tree_node = *parent_ptr;
+        if (!tree_node) {
+            node->rank = 1;
+            node->parent = parent;
+            node->child[0] = NULL;
+            node->child[1] = NULL;
+            *parent_ptr = node;
+            return true;
+        }
+        diff = compare(tree_node, node);
+        if (!diff) {
+            return false;
+        }
+        is_right_child = diff > 0;
+        parent_ptr = &tree_node->child[is_right_child];
+        parent = tree_node;
+    }
+}
+
+/**
+ * bst_delete - Remove node from tree.
+ * @root:   Tree.
+ * @node:   Node to delete
+ *
+ * Delete @node from @root.
+ */
+void bst_delete(struct bst_root *root, struct bst_node *node);
+
+/**
+ * bst_prev - Get previous node.
+ * @root:       Tree.
+ * @node:       Node to move from.
+ *
+ * Use bst_prev_type instead to use pointers to the struct that contains @node.
+ *
+ * Return: If @node is %NULL, right-most node in @root.
+ *         If @node is not %NULL, right-most node to the left of @node.
+ *         %NULL if the node described above does not exist.
+ */
+struct bst_node *bst_prev(struct bst_root *root, struct bst_node *node);
+
+/**
+ * bst_prev_type - Get previous item.
+ * @root:       Tree.
+ * @item:       Item to move from.
+ * @type:       Type of @item.
+ * @member:     Name of struct bst_node embedded in @type.
+ *
+ * Return: If @item is %NULL, right-most item in @root.
+ *         If @item is not %NULL, right-most item to the left of @item.
+ *         %NULL if the item described above does not exist.
+ */
+#define bst_prev_type(root, item, type, member) \
+    containerof_null_safe(bst_prev(root, item), type, member)
+
+/**
+ * bst_next - Get next node.
+ * @root:       Tree.
+ * @node:       Node to move from.
+ *
+ * Use bst_next_type instead to use pointers to the struct that contains @node.
+ *
+ * Return: If @node is %NULL, left-most node in @root.
+ *         If @node is not %NULL, left-most node to the right of @node.
+ *         %NULL if the node described above does not exist.
+ */
+struct bst_node *bst_next(const struct bst_root *root, struct bst_node *node);
+
+/**
+ * bst_next_type - Get previous item.
+ * @root:       Tree.
+ * @item:       Item to move from.
+ * @type:       Type of @item.
+ * @member:     Name of struct bst_node embedded in @type.
+ *
+ * Return: If @item is %NULL, left-most item in @root.
+ *         If @item is not %NULL, left-most item to the right of @item.
+ *         %NULL if the item described above does not exist.
+ */
+#define bst_next_type(root, item, type, member) \
+    containerof_null_safe(bst_next(root, item), type, member)
+
+/**
+ * bst_for_every_entry - Loop over every entry in a tree.
+ * @root:       Tree.
+ * @entry:      Entry variable used by loop body.
+ * @type:       Type of @entry.
+ * @member:     Name of struct bst_node embedded in @type.
+ *
+ * Loop over every node in @root, convert that node to @type and provide it as
+ * @entry to the loop body directly following this macro.
+ *
+ * It is safe to delete @entry from @root in the body if the loop, but it is not
+ * safe to delete any other nodes or insert any nodes.
+ */
+#define bst_for_every_entry(root, entry, type, member) \
+    for (struct bst_node *_bst_for_every_cursor = bst_next(root, NULL); \
+            (_bst_for_every_cursor != NULL) && \
+            ((entry) = containerof(_bst_for_every_cursor, type, member)) && \
+            ((_bst_for_every_cursor = bst_next(root, _bst_for_every_cursor)) \
+             || true);)
+
+/* Internal helper. Don't call directly */
+void bst_delete_all_helper(struct bst_root *root, struct bst_node *node);
+
+/**
+ * bst_for_every_entry_delete - Loop over tree and delete every entry.
+ * @root:       Tree.
+ * @entry:      Entry variable used by loop body.
+ * @type:       Type of @entry.
+ * @member:     Name of struct bst_node embedded in @type.
+ *
+ * Loop over every node in @root, convert that node to @type and provide it as
+ * @entry to the loop body directly following this macro.
+ *
+ * @entry will be removed from @root before entering the loop bode. It is not
+ * safe to delete any other nodes or insert any nodes.
+ */
+#define bst_for_every_entry_delete(root, entry, type, member) \
+    for (struct bst_node *_bst_for_every_cursor = bst_next(root, NULL); \
+            (_bst_for_every_cursor != NULL) && ({\
+            (entry) = containerof(_bst_for_every_cursor, type, member); \
+            _bst_for_every_cursor = bst_next(root, _bst_for_every_cursor); \
+            bst_delete_all_helper(root, &(entry)->member); true;});)
+
+__END_CDECLS
diff --git a/lib/binary_search_tree/rules.mk b/lib/binary_search_tree/rules.mk
new file mode 100644
index 00000000..09066ea8
--- /dev/null
+++ b/lib/binary_search_tree/rules.mk
@@ -0,0 +1,8 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+	$(LOCAL_DIR)/binary_search_tree.c
+
+include make/module.mk
diff --git a/lib/kerneltests-inc.mk b/lib/kerneltests-inc.mk
new file mode 100644
index 00000000..e679e797
--- /dev/null
+++ b/lib/kerneltests-inc.mk
@@ -0,0 +1,25 @@
+# Copyright (c) 2019 LK Trusty Authors. All Rights Reserved.
+#
+# Permission is hereby granted, free of charge, to any person obtaining
+# a copy of this software and associated documentation files
+# (the "Software"), to deal in the Software without restriction,
+# including without limitation the rights to use, copy, modify, merge,
+# publish, distribute, sublicense, and/or sell copies of the Software,
+# and to permit persons to whom the Software is furnished to do so,
+# subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be
+# included in all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+# IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+# CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+# TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+# SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+#
+
+MODULES += \
+	$(GET_LOCAL_DIR)/binary_search_tree/hosttest \
+