[1/3,RFC] Interval tree implementation

diff --git a/include/linux/intervaltree.h b/include/linux/intervaltree.h
new file mode 100644
index 0000000..cfaa174
--- /dev/null
+++ b/include/linux/intervaltree.h
@@ -0,0 +1,55 @@ 
+#ifndef _LINUX_INTERVALTREE_H
+#define _LINUX_INTERVALTREE_H
+
+#include <linux/types.h>
+#include <linux/rbtree.h>
+
+struct interval_tree_node {
+	struct rb_node rb;
+	u64 start;
+	u64 end;
+};
+
+struct interval_tree_root {
+	struct rb_root head;
+};
+
+static inline void interval_tree_init(struct interval_tree_root *root)
+{
+	root->head = RB_ROOT;
+}
+
+static inline void interval_tree_node_init(struct interval_tree_node *node)
+{
+	rb_init_node(&node->rb);
+	node->start = 0;
+	node->end = 0;
+}
+
+static inline int interval_tree_empty(struct interval_tree_root *root)
+{
+	return RB_EMPTY_ROOT(&root->head);
+}
+
+static inline
+struct interval_tree_node *interval_tree_root_node(
+						struct interval_tree_root *root)
+{
+	struct interval_tree_node *ret;
+	ret = container_of(root->head.rb_node, struct interval_tree_node, rb);
+	return ret;
+}
+
+extern struct interval_tree_node *interval_tree_in_interval(
+						struct interval_tree_root *root,
+						 u64 start, u64 end);
+extern struct interval_tree_node *interval_tree_next_in_interval(
+						struct interval_tree_node *node,
+						u64 start, u64 end);
+extern void interval_tree_add(struct interval_tree_root *root,
+					struct interval_tree_node *node);
+extern void interval_tree_remove(struct interval_tree_root *root,
+					struct interval_tree_node *node);
+#endif
+
+
diff --git a/lib/Makefile b/lib/Makefile
index 8c31a0c..2bbad25 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -12,7 +12,7 @@  lib-y := ctype.o string.o vsprintf.o cmdline.o \
 	 idr.o int_sqrt.o extable.o prio_tree.o \
 	 sha1.o md5.o irq_regs.o reciprocal_div.o argv_split.o \
 	 proportions.o prio_heap.o ratelimit.o show_mem.o \
-	 is_single_threaded.o plist.o decompress.o
+	 is_single_threaded.o plist.o decompress.o intervaltree.o
 
 lib-$(CONFIG_MMU) += ioremap.o
 lib-$(CONFIG_SMP) += cpumask.o
diff --git a/lib/intervaltree.c b/lib/intervaltree.c
new file mode 100644
index 0000000..47c52e0
--- /dev/null
+++ b/lib/intervaltree.c
@@ -0,0 +1,119 @@ 
+#include <linux/intervaltree.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+
+/* This code implements a naive interval tree, which stores a series of
+ * non-intersecting intervals.
+ * More complex interval trees can be read about here:
+ *		http://en.wikipedia.org/wiki/Interval_tree
+ */
+
+
+/**
+ * interval_tree_in_interval - Returns the first node that intersects with the
+ *                             given interval
+ * @root: interval_tree root
+ * @start: interval start
+ * @end: interval end
+ *
+ */
+struct interval_tree_node *interval_tree_in_interval(
+						struct interval_tree_root *root,
+						u64 start, u64 end)
+{
+	struct rb_node *p = root->head.rb_node;
+	struct interval_tree_node *candidate, *match = NULL;
+
+	while (p) {
+		candidate = rb_entry(p, struct interval_tree_node, rb);
+		if (end < candidate->start)
+			p = p->rb_left;
+		else if (start > candidate->end)
+			p = p->rb_right;
+		else {
+			/* We found one, but try to find an earlier match */
+			match = candidate;
+			p = p->rb_left;
+		}
+	}
+
+	return match;
+}
+
+
+/**
+ * interval_tree_next_in_interval - Return the next interval in a intervaltree
+ *                                  thatintersects with a specified interval.
+ * @root: interval_tree root
+ * @start: interval start
+ * @end: interval end
+ *
+ */
+struct interval_tree_node *interval_tree_next_in_interval(
+						struct interval_tree_node *node,
+						u64 start, u64 end)
+{
+	struct rb_node *next;
+	struct interval_tree_node *candidate;
+	if (!node)
+		return NULL;
+	next = rb_next(&node->rb);
+	if (!next)
+		return NULL;
+
+	candidate = container_of(next, struct interval_tree_node, rb);
+
+	if ((candidate->start > end) || (candidate->end < start))
+		return NULL;
+
+	return candidate;
+}
+
+/**
+ * interval_tree_add - Add a node to a interval tree
+ * @root: interval tree to be added to
+ * @node: interval_tree_node to be added
+ *
+ * Adds a node to the interval tree. Added interval should not intersect with
+ * existing intervals in the tree.
+ */
+void interval_tree_add(struct interval_tree_root *root,
+					struct interval_tree_node *node)
+{
+	struct rb_node **p = &root->head.rb_node;
+	struct rb_node *parent = NULL;
+	struct interval_tree_node *ptr;
+
+	WARN_ON_ONCE(!RB_EMPTY_NODE(&node->rb));
+
+	/* XXX might want to conditionalize this on debugging checks */
+	WARN_ON_ONCE(!!interval_tree_in_interval(root, node->start, node->end));
+
+	while (*p) {
+		parent = *p;
+		ptr = rb_entry(parent, struct interval_tree_node, rb);
+		if (node->start < ptr->start)
+			p = &(*p)->rb_left;
+		else
+			p = &(*p)->rb_right;
+	}
+	rb_link_node(&node->rb, parent, p);
+	rb_insert_color(&node->rb, &root->head);
+}
+
+
+/**
+ * interval_tree_remove: Removes a given node from the tree
+ * @root: root of tree
+ * @node: Node to be removed
+ *
+ * Removes a node and splays the tree
+ */
+void interval_tree_remove(struct interval_tree_root *root,
+					struct interval_tree_node *node)
+{
+	WARN_ON_ONCE(RB_EMPTY_NODE(&node->rb));
+
+	rb_erase(&node->rb, &root->head);
+	RB_CLEAR_NODE(&node->rb);
+}