ipoe: use single socket for arp processing

5 files changed, 788 insertions, 90 deletions

diff --git a/accel-pppd/CMakeLists.txt b/accel-pppd/CMakeLists.txt
index d82f8d2..aa13e55 100644
--- a/accel-pppd/CMakeLists.txt
+++ b/accel-pppd/CMakeLists.txt
@@ -91,6 +91,7 @@ ADD_EXECUTABLE(accel-pppd
 	iprange.c
 
 	utils.c
+	rbtree.c
 
 	log.c
 	main.c
diff --git a/accel-pppd/ctrl/ipoe/arp.c b/accel-pppd/ctrl/ipoe/arp.c
index ebf05c7..6c647f1 100644
--- a/accel-pppd/ctrl/ipoe/arp.c
+++ b/accel-pppd/ctrl/ipoe/arp.c
@@ -6,6 +6,7 @@
 #include <string.h>
 #include <fcntl.h>
 #include <time.h>
+#include <pthread.h>
 #include <arpa/inet.h>
 #include <netinet/in.h>
 #include <net/ethernet.h>
@@ -17,7 +18,9 @@
 
 #include "list.h"
 #include "triton.h"
+#include "mempool.h"
 #include "log.h"
+#include "rbtree.h"
 
 #include "ipoe.h"
 
@@ -35,20 +38,34 @@ struct _arphdr {
 	__be32 ar_tpa;
 } __packed;
 
-static int arp_read(struct triton_md_handler_t *h)
+struct arp_node {
+	struct rb_node node;
+	struct ipoe_serv *ipoe;
+};
+
+struct arp_tree {
+	pthread_mutex_t lock;
+	struct rb_root root;
+};
+
+static mempool_t arp_pool;
+static mempool_t arp_hdr_pool;
+
+#define HASH_BITS 0xff
+static struct arp_tree *arp_tree;
+
+static struct triton_md_handler_t arp_hnd;
+
+static void arp_ctx_read(struct _arphdr *ah)
 {
-	struct arp_serv *s = container_of(h, typeof(*s), h);
-	char buf[128];
-	int n;
-	struct _arphdr *ah = (struct _arphdr *)buf;
 	struct _arphdr ah2;
-	struct sockaddr_ll src, dst;
-	socklen_t slen = sizeof(src);
-	struct ipoe_session *ses, *ses1, *ses2;
+	struct ipoe_session *ses, *ses1 = NULL, *ses2 = NULL;
+	struct ipoe_serv *ipoe = container_of(triton_context_self(), typeof(*ipoe), ctx);
+	struct sockaddr_ll dst;
 
 	memset(&dst, 0, sizeof(dst));
 	dst.sll_family = AF_PACKET;
-	dst.sll_ifindex = s->ipoe->ifindex;
+	dst.sll_ifindex = ipoe->ifindex;
 	dst.sll_protocol = htons(ETH_P_ARP);
 
 	ah2.ar_hrd = htons(ARPHRD_ETHER);
@@ -57,15 +74,78 @@ static int arp_read(struct triton_md_handler_t *h)
 	ah2.ar_pln = 4;
 	ah2.ar_op = htons(ARPOP_REPLY);
 
+	pthread_mutex_lock(&ipoe->lock);
+	list_for_each_entry(ses, &ipoe->sessions, entry) {
+		if (ses->yiaddr == ah->ar_spa) {
+			ses1 = ses;
+			if (ses->ses.state != AP_STATE_ACTIVE)
+				break;
+		}
+
+		if (ses->yiaddr == ah->ar_tpa) {
+			ses2 = ses;
+			if (ses->ses.state != AP_STATE_ACTIVE)
+				break;
+		}
+
+		if (ses1 && ses2)
+			break;
+	}
+
+	if (!ses1 || (ses1->ses.state != AP_STATE_ACTIVE) ||
+			(ses2 && ses2->ses.state != AP_STATE_ACTIVE)) {
+		pthread_mutex_unlock(&ipoe->lock);
+		goto out;
+	}
+
+	if (ses2) {
+		if (ipoe->opt_arp == 1 || ses1 == ses2) {
+			pthread_mutex_unlock(&ipoe->lock);
+			goto out;
+		}
+
+		if (ipoe->opt_arp == 2)
+			memcpy(ah2.ar_sha, ses2->hwaddr, ETH_ALEN);
+		else
+			memcpy(ah2.ar_sha, ipoe->hwaddr, ETH_ALEN);
+	} else
+		memcpy(ah2.ar_sha, ipoe->hwaddr, ETH_ALEN);
+
+	pthread_mutex_unlock(&ipoe->lock);
+
+	memcpy(dst.sll_addr, ah->ar_sha, ETH_ALEN);
+	memcpy(ah2.ar_tha, ah->ar_sha, ETH_ALEN);
+	ah2.ar_spa = ah->ar_tpa;
+	ah2.ar_tpa = ah->ar_spa;
+
+	sendto(arp_hnd.fd, &ah2, sizeof(ah2), MSG_DONTWAIT, (struct sockaddr *)&dst, sizeof(dst));
+
+out:
+	mempool_free(ah);
+}
+
+static int arp_read(struct triton_md_handler_t *h)
+{
+	int r, i;
+	struct _arphdr *ah = NULL;
+	struct sockaddr_ll src;
+	socklen_t slen = sizeof(src);
+	struct arp_tree *t;
+	struct arp_node *n;
+	struct rb_node **p, *parent;
+
 	while (1) {
-		n = recvfrom(h->fd, buf, sizeof(buf), MSG_DONTWAIT, (struct sockaddr *)&src, &slen);
-		if (n < 0) {
+		if (!ah)
+			ah = mempool_alloc(arp_hdr_pool);
+
+		r = recvfrom(h->fd, ah, sizeof(*ah), MSG_DONTWAIT, (struct sockaddr *)&src, &slen);
+		if (r < 0) {
 			if (errno == EAGAIN)
 				break;
 			continue;
 		}
 
-		if (n < sizeof(*ah))
+		if (r < sizeof(*ah))
 			continue;
 
 		if (ah->ar_op != htons(ARPOP_REQUEST))
@@ -83,60 +163,44 @@ static int arp_read(struct triton_md_handler_t *h)
 		if (memcmp(ah->ar_sha, src.sll_addr, ETH_ALEN))
 			continue;
 
-		ses1 = ses2 = NULL;
-		pthread_mutex_lock(&s->ipoe->lock);
-		list_for_each_entry(ses, &s->ipoe->sessions, entry) {
-			if (ses->yiaddr == ah->ar_spa) {
-				ses1 = ses;
-				if (ses->ses.state != AP_STATE_ACTIVE)
-					break;
-			}
-			if (ses->yiaddr == ah->ar_tpa) {
-				ses2 = ses;
-				if (ses->ses.state != AP_STATE_ACTIVE)
-					break;
-			}
-			if (ses1 && ses2)
-				break;
-		}
+		t = &arp_tree[src.sll_ifindex & HASH_BITS];
 
-		if (!ses1 || (ses1->ses.state != AP_STATE_ACTIVE) ||
-			  (ses2 && ses2->ses.state != AP_STATE_ACTIVE)) {
-			pthread_mutex_unlock(&s->ipoe->lock);
-			continue;
-		}
+		parent = NULL;
 
-		if (ses2) {
-			if (s->ipoe->opt_arp == 1 || ses1 == ses2) {
-				pthread_mutex_unlock(&s->ipoe->lock);
-				continue;
-			}
-			if (s->ipoe->opt_arp == 2)
-				memcpy(ah2.ar_sha, ses2->hwaddr, ETH_ALEN);
-			else
-				memcpy(ah2.ar_sha, s->ipoe->hwaddr, ETH_ALEN);
-		} else
-			memcpy(ah2.ar_sha, s->ipoe->hwaddr, ETH_ALEN);
+		pthread_mutex_lock(&t->lock);
+
+		p = &t->root.rb_node;
 
-		pthread_mutex_unlock(&s->ipoe->lock);
+		while (*p) {
+			parent = *p;
+			n = rb_entry(parent, typeof(*n), node);
+			i = n->ipoe->ifindex;
 
-		memcpy(dst.sll_addr, ah->ar_sha, ETH_ALEN);
-		memcpy(ah2.ar_tha, ah->ar_sha, ETH_ALEN);
-		ah2.ar_spa = ah->ar_tpa;
-		ah2.ar_tpa = ah->ar_spa;
+			if (src.sll_ifindex < i)
+				p = &(*p)->rb_left;
+			else if (src.sll_ifindex > i)
+				p = &(*p)->rb_right;
+			else {
+				triton_context_call(&n->ipoe->ctx, (triton_event_func)arp_ctx_read, ah);
+				ah = NULL;
+				break;
+			}
+		}
 
-		sendto(h->fd, &ah2, sizeof(ah2), MSG_DONTWAIT, (struct sockaddr *)&dst, sizeof(dst));
+		pthread_mutex_unlock(&t->lock);
 	}
 
+	mempool_free(ah);
+
 	return 0;
 }
 
-struct arp_serv *arpd_start(struct ipoe_serv *ipoe)
+void *arpd_start(struct ipoe_serv *ipoe)
 {
-	int sock;
-	struct sockaddr_ll addr;
-	struct arp_serv *s;
-	int f = 1, fd;
+	struct rb_node **p, *parent = NULL;
+	struct arp_node *n;
+	struct arp_tree *t;
+	int fd, ifindex = ipoe->ifindex, i;
 	char fname[1024];
 
 	sprintf(fname, "/proc/sys/net/ipv4/conf/%s/proxy_arp", ipoe->ifname);
@@ -147,46 +211,115 @@ struct arp_serv *arpd_start(struct ipoe_serv *ipoe)
 		close(fd);
 	}
 
+	t = &arp_tree[ifindex & HASH_BITS];
+
+	pthread_mutex_lock(&t->lock);
+
+	p = &t->root.rb_node;
+
+	while (*p) {
+		parent = *p;
+		n = rb_entry(parent, typeof(*n), node);
+		i = n->ipoe->ifindex;
+
+		if (ifindex < i)
+			p = &(*p)->rb_left;
+		else if (ifindex > i)
+			p = &(*p)->rb_right;
+		else {
+			pthread_mutex_unlock(&t->lock);
+			log_ppp_error("arp: attempt to add duplicate ifindex\n");
+			return NULL;
+		}
+	}
+
+	n = mempool_alloc(arp_pool);
+	if (!n) {
+		pthread_mutex_unlock(&t->lock);
+		log_emerg("out of memory\n");
+		return NULL;
+	}
+
+	n->ipoe = ipoe;
+
+	rb_link_node(&n->node, parent, p);
+	rb_insert_color(&n->node, &t->root);
+
+	pthread_mutex_unlock(&t->lock);
+
+	return n;
+}
+
+void arpd_stop(void *arg)
+{
+	struct arp_node *n = arg;
+	struct arp_tree *t = &arp_tree[n->ipoe->ifindex];
+
+	pthread_mutex_lock(&t->lock);
+	rb_erase(&n->node, &t->root);
+	pthread_mutex_unlock(&t->lock);
+
+	mempool_free(n);
+}
+
+static void arp_close(struct triton_context_t *ctx);
+
+static struct triton_context_t arp_ctx = {
+	.close = arp_close,
+};
+
+static struct triton_md_handler_t arp_hnd = {
+	.read = arp_read,
+};
+
+static void arp_close(struct triton_context_t *ctx)
+{
+	triton_md_unregister_handler(&arp_hnd, 1);
+	triton_context_unregister(ctx);
+}
+
+static void init()
+{
+	struct sockaddr_ll addr;
+	int i, f = 1;
+	int sock;
+
+	arp_pool = mempool_create(sizeof(struct arp_node));
+	arp_hdr_pool = mempool_create(sizeof(struct _arphdr));
+
+	arp_tree = malloc((HASH_BITS + 1) * sizeof(struct arp_tree));
+	for (i = 0; i <= HASH_BITS; i++) {
+		pthread_mutex_init(&arp_tree[i].lock, NULL);
+		arp_tree[i].root = RB_ROOT;
+	}
+
 	sock = socket(PF_PACKET, SOCK_DGRAM, 0);
 	if (sock < 0) {
-		log_error("ipoe: arp: socket: %s\n", strerror(errno));
-		return NULL;
+		log_error("arp: socket: %s\n", strerror(errno));
+		return;
 	}
 
 	memset(&addr, 0, sizeof(addr));
 	addr.sll_family = AF_PACKET;
 	addr.sll_protocol = htons(ETH_P_ARP);
-	addr.sll_ifindex = ipoe->ifindex;
 
-	if (setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &f, sizeof(f))) {
-		log_error("ipoe: setsockopt(SO_BROADCAST): %s\n", strerror(errno));
-		close(sock);
-		return NULL;
-	}
+	setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &f, sizeof(f));
 
 	if (bind(sock, (struct sockaddr *)&addr, sizeof(addr))) {
-		log_error("ipoe: arp: bind: %s\n", strerror(errno));
+		log_error("arp: bind: %s\n", strerror(errno));
 		close(sock);
-		return NULL;
+		return;
 	}
 
-	s = _malloc(sizeof(*s));
-	s->ipoe = ipoe;
-	s->h.fd = sock;
-	s->h.read = arp_read;
-
 	fcntl(sock, F_SETFL, O_NONBLOCK);
-	fcntl(sock, F_SETFD, fcntl(sock, F_GETFD) | FD_CLOEXEC);
+	fcntl(sock, F_SETFD, FD_CLOEXEC);
 
-	triton_md_register_handler(&ipoe->ctx, &s->h);
-	triton_md_enable_handler(&s->h, MD_MODE_READ);
+	arp_hnd.fd = sock;
 
-	return s;
-}
-
-void arpd_stop(struct arp_serv *arp)
-{
-	triton_md_unregister_handler(&arp->h, 1);
-	_free(arp);
+	triton_context_register(&arp_ctx, NULL);
+	triton_md_register_handler(&arp_ctx, &arp_hnd);
+	triton_md_enable_handler(&arp_hnd, MD_MODE_READ);
+	triton_context_wakeup(&arp_ctx);
 }
 
+DEFINE_INIT(1, init);
diff --git a/accel-pppd/ctrl/ipoe/ipoe.h b/accel-pppd/ctrl/ipoe/ipoe.h
index c27acdd..f14bb5d 100644
--- a/accel-pppd/ctrl/ipoe/ipoe.h
+++ b/accel-pppd/ctrl/ipoe/ipoe.h
@@ -17,8 +17,6 @@
 #define ETH_ALEN 6
 #endif
 
-struct arp_serv;
-
 struct ipoe_serv {
 	struct list_head entry;
 	struct triton_context_t ctx;
@@ -29,7 +27,7 @@ struct ipoe_serv {
 	struct list_head addr_list;
 	struct dhcpv4_serv *dhcpv4;
 	struct dhcpv4_relay *dhcpv4_relay;
-	struct arp_serv *arp;
+	void *arp;
 	struct list_head disc_list;
 	struct list_head req_list;
 	struct triton_timer_t disc_timer;
@@ -105,11 +103,6 @@ struct ipoe_session_info {
 	uint32_t peer_addr;
 };
 
-struct arp_serv {
-	struct triton_md_handler_t h;
-	struct ipoe_serv *ipoe;
-};
-
 #ifdef USE_LUA
 char *ipoe_lua_get_username(struct ipoe_session *, const char *func);
 #endif
@@ -138,8 +131,8 @@ int ipoe_nl_del_vlan_mon(int ifindex);
 int ipoe_nl_add_exclude(uint32_t addr, int mask);
 void ipoe_nl_del_exclude(uint32_t addr);
 
-struct arp_serv *arpd_start(struct ipoe_serv *ipoe);
-void arpd_stop(struct arp_serv *arp);
+void *arpd_start(struct ipoe_serv *ipoe);
+void arpd_stop(void *arp);
 
 #endif
 
diff --git a/accel-pppd/include/rbtree.h b/accel-pppd/include/rbtree.h
new file mode 100644
index 0000000..c9fe6b4
--- /dev/null
+++ b/accel-pppd/include/rbtree.h
@@ -0,0 +1,179 @@
+/*
+  Red Black Trees
+  (C) 1999  Andrea Arcangeli <andrea@suse.de>
+  
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+  linux/include/linux/rbtree.h
+
+  To use rbtrees you'll have to implement your own insert and search cores.
+  This will avoid us to use callbacks and to drop drammatically performances.
+  I know it's not the cleaner way,  but in C (not in C++) to get
+  performances and genericity...
+
+  Some example of insert and search follows here. The search is a plain
+  normal search over an ordered tree. The insert instead must be implemented
+  int two steps: as first thing the code must insert the element in
+  order as a red leaf in the tree, then the support library function
+  rb_insert_color() must be called. Such function will do the
+  not trivial work to rebalance the rbtree if necessary.
+
+-----------------------------------------------------------------------
+static inline struct page * rb_search_page_cache(struct inode * inode,
+						 unsigned long offset)
+{
+	struct rb_node * n = inode->i_rb_page_cache.rb_node;
+	struct page * page;
+
+	while (n)
+	{
+		page = rb_entry(n, struct page, rb_page_cache);
+
+		if (offset < page->offset)
+			n = n->rb_left;
+		else if (offset > page->offset)
+			n = n->rb_right;
+		else
+			return page;
+	}
+	return NULL;
+}
+
+static inline struct page * __rb_insert_page_cache(struct inode * inode,
+						   unsigned long offset,
+						   struct rb_node * node)
+{
+	struct rb_node ** p = &inode->i_rb_page_cache.rb_node;
+	struct rb_node * parent = NULL;
+	struct page * page;
+
+	while (*p)
+	{
+		parent = *p;
+		page = rb_entry(parent, struct page, rb_page_cache);
+
+		if (offset < page->offset)
+			p = &(*p)->rb_left;
+		else if (offset > page->offset)
+			p = &(*p)->rb_right;
+		else
+			return page;
+	}
+
+	rb_link_node(node, parent, p);
+
+	return NULL;
+}
+
+static inline struct page * rb_insert_page_cache(struct inode * inode,
+						 unsigned long offset,
+						 struct rb_node * node)
+{
+	struct page * ret;
+	if ((ret = __rb_insert_page_cache(inode, offset, node)))
+		goto out;
+	rb_insert_color(node, &inode->i_rb_page_cache);
+ out:
+	return ret;
+}
+-----------------------------------------------------------------------
+*/
+
+#ifndef	_LINUX_RBTREE_H
+#define	_LINUX_RBTREE_H
+
+#include <stdlib.h>
+
+#undef offsetof
+#ifdef __compiler_offsetof
+#define offsetof(TYPE,MEMBER) __compiler_offsetof(TYPE,MEMBER)
+#else
+#define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)
+#endif
+
+/**
+ * container_of - cast a member of a structure out to the containing structure
+ * @ptr:        the pointer to the member.
+ * @type:       the type of the container struct this is embedded in.
+ * @member:     the name of the member within the struct.
+ *
+ */
+#define container_of(ptr, type, member) ({                      \
+        const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
+        (type *)( (char *)__mptr - offsetof(type,member) );})
+
+
+struct rb_node
+{
+	unsigned long  rb_parent_color;
+#define	RB_RED		0
+#define	RB_BLACK	1
+	struct rb_node *rb_right;
+	struct rb_node *rb_left;
+} __attribute__((aligned(sizeof(long))));
+    /* The alignment might seem pointless, but allegedly CRIS needs it */
+
+struct rb_root
+{
+	struct rb_node *rb_node;
+};
+
+
+#define rb_parent(r)   ((struct rb_node *)((r)->rb_parent_color & ~3))
+#define rb_color(r)   ((r)->rb_parent_color & 1)
+#define rb_is_red(r)   (!rb_color(r))
+#define rb_is_black(r) rb_color(r)
+#define rb_set_red(r)  do { (r)->rb_parent_color &= ~1; } while (0)
+#define rb_set_black(r)  do { (r)->rb_parent_color |= 1; } while (0)
+
+static inline void rb_set_parent(struct rb_node *rb, struct rb_node *p)
+{
+	rb->rb_parent_color = (rb->rb_parent_color & 3) | (unsigned long)p;
+}
+static inline void rb_set_color(struct rb_node *rb, int color)
+{
+	rb->rb_parent_color = (rb->rb_parent_color & ~1) | color;
+}
+
+#define RB_ROOT	(struct rb_root) { NULL, }
+#define	rb_entry(ptr, type, member) container_of(ptr, type, member)
+
+#define RB_EMPTY_ROOT(root)	((root)->rb_node == NULL)
+#define RB_EMPTY_NODE(node)	(rb_parent(node) == node)
+#define RB_CLEAR_NODE(node)	(rb_set_parent(node, node))
+
+extern void rb_insert_color(struct rb_node *, struct rb_root *);
+extern void rb_erase(struct rb_node *, struct rb_root *);
+
+/* Find logical next and previous nodes in a tree */
+extern struct rb_node *rb_next(struct rb_node *);
+extern struct rb_node *rb_prev(struct rb_node *);
+extern struct rb_node *rb_first(struct rb_root *);
+extern struct rb_node *rb_last(struct rb_root *);
+
+/* Fast replacement of a single node without remove/rebalance/add/rebalance */
+extern void rb_replace_node(struct rb_node *victim, struct rb_node *new, 
+			    struct rb_root *root);
+
+static inline void rb_link_node(struct rb_node * node, struct rb_node * parent,
+				struct rb_node ** rb_link)
+{
+	node->rb_parent_color = (unsigned long )parent;
+	node->rb_left = node->rb_right = NULL;
+
+	*rb_link = node;
+}
+
+#endif	/* _LINUX_RBTREE_H */
diff --git a/accel-pppd/rbtree.c b/accel-pppd/rbtree.c
new file mode 100644
index 0000000..11487fa
--- /dev/null
+++ b/accel-pppd/rbtree.c
@@ -0,0 +1,392 @@
+/*
+  Red Black Trees
+  (C) 1999  Andrea Arcangeli <andrea@suse.de>
+  (C) 2002  David Woodhouse <dwmw2@infradead.org>
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+  linux/lib/rbtree.c
+*/
+
+#include "triton.h"
+#include "rbtree.h"
+
+static void __rb_rotate_left(struct rb_node *node, struct rb_root *root)
+{
+	struct rb_node *right = node->rb_right;
+	struct rb_node *parent = rb_parent(node);
+
+	if ((node->rb_right = right->rb_left))
+		rb_set_parent(right->rb_left, node);
+	right->rb_left = node;
+
+	rb_set_parent(right, parent);
+
+	if (parent)
+	{
+		if (node == parent->rb_left)
+			parent->rb_left = right;
+		else
+			parent->rb_right = right;
+	}
+	else
+		root->rb_node = right;
+	rb_set_parent(node, right);
+}
+
+static void __rb_rotate_right(struct rb_node *node, struct rb_root *root)
+{
+	struct rb_node *left = node->rb_left;
+	struct rb_node *parent = rb_parent(node);
+
+	if ((node->rb_left = left->rb_right))
+		rb_set_parent(left->rb_right, node);
+	left->rb_right = node;
+
+	rb_set_parent(left, parent);
+
+	if (parent)
+	{
+		if (node == parent->rb_right)
+			parent->rb_right = left;
+		else
+			parent->rb_left = left;
+	}
+	else
+		root->rb_node = left;
+	rb_set_parent(node, left);
+}
+
+void __export rb_insert_color(struct rb_node *node, struct rb_root *root)
+{
+	struct rb_node *parent, *gparent;
+
+	while ((parent = rb_parent(node)) && rb_is_red(parent))
+	{
+		gparent = rb_parent(parent);
+
+		if (parent == gparent->rb_left)
+		{
+			{
+				register struct rb_node *uncle = gparent->rb_right;
+				if (uncle && rb_is_red(uncle))
+				{
+					rb_set_black(uncle);
+					rb_set_black(parent);
+					rb_set_red(gparent);
+					node = gparent;
+					continue;
+				}
+			}
+
+			if (parent->rb_right == node)
+			{
+				register struct rb_node *tmp;
+				__rb_rotate_left(parent, root);
+				tmp = parent;
+				parent = node;
+				node = tmp;
+			}
+
+			rb_set_black(parent);
+			rb_set_red(gparent);
+			__rb_rotate_right(gparent, root);
+		} else {
+			{
+				register struct rb_node *uncle = gparent->rb_left;
+				if (uncle && rb_is_red(uncle))
+				{
+					rb_set_black(uncle);
+					rb_set_black(parent);
+					rb_set_red(gparent);
+					node = gparent;
+					continue;
+				}
+			}
+
+			if (parent->rb_left == node)
+			{
+				register struct rb_node *tmp;
+				__rb_rotate_right(parent, root);
+				tmp = parent;
+				parent = node;
+				node = tmp;
+			}
+
+			rb_set_black(parent);
+			rb_set_red(gparent);
+			__rb_rotate_left(gparent, root);
+		}
+	}
+
+	rb_set_black(root->rb_node);
+}
+
+static void __rb_erase_color(struct rb_node *node, struct rb_node *parent,
+			     struct rb_root *root)
+{
+	struct rb_node *other;
+
+	while ((!node || rb_is_black(node)) && node != root->rb_node)
+	{
+		if (parent->rb_left == node)
+		{
+			other = parent->rb_right;
+			if (rb_is_red(other))
+			{
+				rb_set_black(other);
+				rb_set_red(parent);
+				__rb_rotate_left(parent, root);
+				other = parent->rb_right;
+			}
+			if ((!other->rb_left || rb_is_black(other->rb_left)) &&
+			    (!other->rb_right || rb_is_black(other->rb_right)))
+			{
+				rb_set_red(other);
+				node = parent;
+				parent = rb_parent(node);
+			}
+			else
+			{
+				if (!other->rb_right || rb_is_black(other->rb_right))
+				{
+					struct rb_node *o_left;
+					if ((o_left = other->rb_left))
+						rb_set_black(o_left);
+					rb_set_red(other);
+					__rb_rotate_right(other, root);
+					other = parent->rb_right;
+				}
+				rb_set_color(other, rb_color(parent));
+				rb_set_black(parent);
+				if (other->rb_right)
+					rb_set_black(other->rb_right);
+				__rb_rotate_left(parent, root);
+				node = root->rb_node;
+				break;
+			}
+		}
+		else
+		{
+			other = parent->rb_left;
+			if (rb_is_red(other))
+			{
+				rb_set_black(other);
+				rb_set_red(parent);
+				__rb_rotate_right(parent, root);
+				other = parent->rb_left;
+			}
+			if ((!other->rb_left || rb_is_black(other->rb_left)) &&
+			    (!other->rb_right || rb_is_black(other->rb_right)))
+			{
+				rb_set_red(other);
+				node = parent;
+				parent = rb_parent(node);
+			}
+			else
+			{
+				if (!other->rb_left || rb_is_black(other->rb_left))
+				{
+					register struct rb_node *o_right;
+					if ((o_right = other->rb_right))
+						rb_set_black(o_right);
+					rb_set_red(other);
+					__rb_rotate_left(other, root);
+					other = parent->rb_left;
+				}
+				rb_set_color(other, rb_color(parent));
+				rb_set_black(parent);
+				if (other->rb_left)
+					rb_set_black(other->rb_left);
+				__rb_rotate_right(parent, root);
+				node = root->rb_node;
+				break;
+			}
+		}
+	}
+	if (node)
+		rb_set_black(node);
+}
+
+void __export rb_erase(struct rb_node *node, struct rb_root *root)
+{
+	struct rb_node *child, *parent;
+	int color;
+
+	if (!node->rb_left)
+		child = node->rb_right;
+	else if (!node->rb_right)
+		child = node->rb_left;
+	else
+	{
+		struct rb_node *old = node, *left;
+
+		node = node->rb_right;
+		while ((left = node->rb_left) != NULL)
+			node = left;
+		child = node->rb_right;
+		parent = rb_parent(node);
+		color = rb_color(node);
+
+		if (child)
+			rb_set_parent(child, parent);
+		if (parent == old) {
+			parent->rb_right = child;
+			parent = node;
+		} else
+			parent->rb_left = child;
+
+		node->rb_parent_color = old->rb_parent_color;
+		node->rb_right = old->rb_right;
+		node->rb_left = old->rb_left;
+
+		if (rb_parent(old))
+		{
+			if (rb_parent(old)->rb_left == old)
+				rb_parent(old)->rb_left = node;
+			else
+				rb_parent(old)->rb_right = node;
+		} else
+			root->rb_node = node;
+
+		rb_set_parent(old->rb_left, node);
+		if (old->rb_right)
+			rb_set_parent(old->rb_right, node);
+		goto color;
+	}
+
+	parent = rb_parent(node);
+	color = rb_color(node);
+
+	if (child)
+		rb_set_parent(child, parent);
+	if (parent)
+	{
+		if (parent->rb_left == node)
+			parent->rb_left = child;
+		else
+			parent->rb_right = child;
+	}
+	else
+		root->rb_node = child;
+
+ color:
+	if (color == RB_BLACK)
+		__rb_erase_color(child, parent, root);
+}
+
+/*
+ * This function returns the first node (in sort order) of the tree.
+ */
+struct rb_node __export *rb_first(struct rb_root *root)
+{
+	struct rb_node	*n;
+
+	n = root->rb_node;
+	if (!n)
+		return NULL;
+	while (n->rb_left)
+		n = n->rb_left;
+	return n;
+}
+
+struct rb_node __export *rb_last(struct rb_root *root)
+{
+	struct rb_node	*n;
+
+	n = root->rb_node;
+	if (!n)
+		return NULL;
+	while (n->rb_right)
+		n = n->rb_right;
+	return n;
+}
+
+struct rb_node __export *rb_next(struct rb_node *node)
+{
+	struct rb_node *parent;
+
+	if (rb_parent(node) == node)
+		return NULL;
+
+	/* If we have a right-hand child, go down and then left as far
+	   as we can. */
+	if (node->rb_right) {
+		node = node->rb_right;
+		while (node->rb_left)
+			node=node->rb_left;
+		return node;
+	}
+
+	/* No right-hand children.  Everything down and left is
+	   smaller than us, so any 'next' node must be in the general
+	   direction of our parent. Go up the tree; any time the
+	   ancestor is a right-hand child of its parent, keep going
+	   up. First time it's a left-hand child of its parent, said
+	   parent is our 'next' node. */
+	while ((parent = rb_parent(node)) && node == parent->rb_right)
+		node = parent;
+
+	return parent;
+}
+
+struct rb_node __export *rb_prev(struct rb_node *node)
+{
+	struct rb_node *parent;
+
+	if (rb_parent(node) == node)
+		return NULL;
+
+	/* If we have a left-hand child, go down and then right as far
+	   as we can. */
+	if (node->rb_left) {
+		node = node->rb_left;
+		while (node->rb_right)
+			node=node->rb_right;
+		return node;
+	}
+
+	/* No left-hand children. Go up till we find an ancestor which
+	   is a right-hand child of its parent */
+	while ((parent = rb_parent(node)) && node == parent->rb_left)
+		node = parent;
+
+	return parent;
+}
+
+
+void __export rb_replace_node(struct rb_node *victim, struct rb_node *new,
+		     struct rb_root *root)
+{
+	struct rb_node *parent = rb_parent(victim);
+
+	/* Set the surrounding nodes to point to the replacement */
+	if (parent) {
+		if (victim == parent->rb_left)
+			parent->rb_left = new;
+		else
+			parent->rb_right = new;
+	} else {
+		root->rb_node = new;
+	}
+	if (victim->rb_left)
+		rb_set_parent(victim->rb_left, new);
+	if (victim->rb_right)
+		rb_set_parent(victim->rb_right, new);
+
+	/* Copy the pointers/colour from the victim to the replacement */
+	*new = *victim;
+}
+