ipfilter/netinet/radix_ipf.c

/*
 * Copyright (C) 2012 by Darren Reed.
 *
 * See the IPFILTER.LICENCE file for details on licencing.
 */
#include <sys/types.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <sys/param.h>
#include <netinet/in.h>
#include <net/if.h>
#ifdef _KERNEL
#include <sys/systm.h>
#else
# include <stddef.h>
# include <stdlib.h>
# include <strings.h>
# include <string.h>
#endif /* !_KERNEL */
#include "netinet/ip_compat.h"
#include "netinet/ip_fil.h"
#ifdef RDX_DEBUG
# include <arpa/inet.h>
# include <stdlib.h>
# include <stdio.h>
#endif
#include "netinet/radix_ipf.h"

#define	ADF_OFF	offsetof(addrfamily_t, adf_addr)
#define	ADF_OFF_BITS	(ADF_OFF << 3)

static ipf_rdx_node_t *ipf_rx_insert(ipf_rdx_head_t *,
					  ipf_rdx_node_t nodes[2], int *);
static void ipf_rx_attach_mask(ipf_rdx_node_t *, ipf_rdx_mask_t *);
static int count_mask_bits(addrfamily_t *, u_32_t **);
static void buildnodes(addrfamily_t *, addrfamily_t *,
			    ipf_rdx_node_t n[2]);
static ipf_rdx_node_t *ipf_rx_find_addr(ipf_rdx_node_t *, u_32_t *);
static ipf_rdx_node_t *ipf_rx_lookup(ipf_rdx_head_t *, addrfamily_t *,
					  addrfamily_t *);
static ipf_rdx_node_t *ipf_rx_match(ipf_rdx_head_t *, addrfamily_t *);

/*
 * Foreword.
 * ---------
 * The code in this file has been written to target using the addrfamily_t
 * data structure to house the address information and no other. Thus there
 * are certain aspects of thise code (such as offsets to the address itself)
 * that are hard coded here whilst they might be more variable elsewhere.
 * Similarly, this code enforces no maximum key length as that's implied by
 * all keys needing to be stored in addrfamily_t.
 */

/* ------------------------------------------------------------------------ */
/* Function:    count_mask_bits                                             */
/* Returns:     number of consecutive bits starting at "mask".              */
/*                                                                          */
/* Count the number of bits set in the address section of addrfamily_t and  */
/* return both that number and a pointer to the last word with a bit set if */
/* lastp is not NULL. The bit count is performed using network byte order   */
/* as the guide for which bit is the most significant bit.                  */
/* ------------------------------------------------------------------------ */
static int
count_mask_bits(addrfamily_t *mask, u_32_t **lastp)
{
	u_32_t *mp = (u_32_t *)&mask->adf_addr;
	u_32_t m;
	int count = 0;
	int mlen;

	mlen = mask->adf_len - offsetof(addrfamily_t, adf_addr);
	for (; mlen > 0; mlen -= 4, mp++) {
		if ((m = ntohl(*mp)) == 0)
			break;
		if (lastp != NULL)
			*lastp = mp;
		for (; m & 0x80000000; m <<= 1)
			count++;
	}

	return (count);
}


/* ------------------------------------------------------------------------ */
/* Function:    buildnodes                                                  */
/* Returns:     Nil                                                         */
/* Parameters:  addr(I)  - network address for this radix node              */
/*              mask(I)  - netmask associated with the above address        */
/*              nodes(O) - pair of ipf_rdx_node_t's to initialise with data */
/*                         associated with addr and mask.                   */
/*                                                                          */
/* Initialise the fields in a pair of radix tree nodes according to the     */
/* data supplied in the paramters "addr" and "mask". It is expected that    */
/* "mask" will contain a consecutive string of bits set. Masks with gaps in */
/* the middle are not handled by this implementation.                       */
/* ------------------------------------------------------------------------ */
static void
buildnodes(addrfamily_t *addr, addrfamily_t *mask, ipf_rdx_node_t nodes[2])
{
	u_32_t maskbits;
	u_32_t lastmask;
	u_32_t *last;
	int masklen;

	last = NULL;
	maskbits = count_mask_bits(mask, &last);
	if (last == NULL) {
		masklen = 0;
		lastmask = 0;
	} else {
		masklen = last - (u_32_t *)mask;
		lastmask = *last;
	}

	bzero(&nodes[0], sizeof(ipf_rdx_node_t) * 2);
	nodes[0].maskbitcount = maskbits;
	nodes[0].index = -1 - (ADF_OFF_BITS + maskbits);
	nodes[0].addrkey = (u_32_t *)addr;
	nodes[0].maskkey = (u_32_t *)mask;
	nodes[0].addroff = nodes[0].addrkey + masklen;
	nodes[0].maskoff = nodes[0].maskkey + masklen;
	nodes[0].parent = &nodes[1];
	nodes[0].offset = masklen;
	nodes[0].lastmask = lastmask;
	nodes[1].offset = masklen;
	nodes[1].left = &nodes[0];
	nodes[1].maskbitcount = maskbits;
#ifdef RDX_DEBUG
	(void) strcpy(nodes[0].name, "_BUILD.0");
	(void) strcpy(nodes[1].name, "_BUILD.1");
#endif
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_find_addr                                            */
/* Returns:     ipf_rdx_node_t * - pointer to a node in the radix tree.     */
/* Parameters:  tree(I)  - pointer to first right node in tree to search    */
/*              addr(I)  - pointer to address to match                      */
/*                                                                          */
/* Walk the radix tree given by "tree", looking for a leaf node that is a   */
/* match for the address given by "addr".                                   */
/* ------------------------------------------------------------------------ */
static ipf_rdx_node_t *
ipf_rx_find_addr(ipf_rdx_node_t *tree, u_32_t *addr)
{
	ipf_rdx_node_t *cur;

	for (cur = tree; cur->index >= 0;) {
		if (cur->bitmask & addr[cur->offset]) {
			cur = cur->right;
		} else {
			cur = cur->left;
		}
	}

	return (cur);
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_match                                                */
/* Returns:     ipf_rdx_node_t * - NULL on error, else pointer to the node  */
/*                                 added to the tree.                       */
/* Paramters:   head(I)  - pointer to tree head to search                   */
/*              addr(I)  - pointer to address to find                       */
/*                                                                          */
/* Search the radix tree for the best match to the address pointed to by    */
/* "addr" and return a pointer to that node. This search will not match the */
/* address information stored in either of the root leaves as neither of    */
/* them are considered to be part of the tree of data being stored.         */
/* ------------------------------------------------------------------------ */
static ipf_rdx_node_t *
ipf_rx_match(ipf_rdx_head_t *head, addrfamily_t *addr)
{
	ipf_rdx_mask_t *masknode;
	ipf_rdx_node_t *prev;
	ipf_rdx_node_t *node;
	ipf_rdx_node_t *cur;
	u_32_t *data;
	u_32_t *mask;
	u_32_t *key;
	u_32_t *end;
	int len;
	int i;

	len = addr->adf_len;
	end = (u_32_t *)((u_char *)addr + len);
	node = ipf_rx_find_addr(head->root, (u_32_t *)addr);

	/*
	 * Search the dupkey list for a potential match.
	 */
	for (cur = node; (cur != NULL) && (cur->root == 0); cur = cur->dupkey) {
		i = cur[0].addroff - cur[0].addrkey;
		data = cur[0].addrkey + i;
		mask = cur[0].maskkey + i;
		key = (u_32_t *)addr + i;
		for (; key < end; data++, key++, mask++)
			if ((*key & *mask) != *data)
				break;
		if ((end == key) && (cur->root == 0))
			return (cur);	/* Equal keys */
	}
	prev = node->parent;
	key = (u_32_t *)addr;

	for (node = prev; node->root == 0; node = node->parent) {
		/*
		 * We know that the node hasn't matched so therefore only
		 * the entries in the mask list are searched, not the top
		 * node nor the dupkey list.
		 */
		masknode = node->masks;
		for (; masknode != NULL; masknode = masknode->next) {
			if (masknode->maskbitcount > node->maskbitcount)
				continue;
			cur = masknode->node;
			for (i = ADF_OFF >> 2; i <= node->offset; i++) {
				if ((key[i] & masknode->mask[i]) ==
				    cur->addrkey[i])
					return (cur);
			}
		}
	}

	return (NULL);
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_lookup                                               */
/* Returns:     ipf_rdx_node_t * - NULL on error, else pointer to the node  */
/*                                 added to the tree.                       */
/* Paramters:   head(I)  - pointer to tree head to search                   */
/*              addr(I)  - address part of the key to match                 */
/*              mask(I)  - netmask part of the key to match                 */
/*                                                                          */
/* ipf_rx_lookup searches for an exact match on (addr,mask). The intention  */
/* is to see if a given key is in the tree, not to see if a route exists.   */
/* ------------------------------------------------------------------------ */
ipf_rdx_node_t *
ipf_rx_lookup(ipf_rdx_head_t *head, addrfamily_t *addr, addrfamily_t *mask)
{
	ipf_rdx_node_t *found;
	ipf_rdx_node_t *node;
	u_32_t *akey;
	int count;

	found = ipf_rx_find_addr(head->root, (u_32_t *)addr);
	if (found->root == 1)
		return (NULL);

	/*
	 * It is possible to find a matching address in the tree but for the
	 * netmask to not match. If the netmask does not match and there is
	* no list of alternatives present at dupkey, return a failure.
	 */
	count = count_mask_bits(mask, NULL);
	if (count != found->maskbitcount && found->dupkey == NULL)
		return (NULL);

	akey = (u_32_t *)addr;
	if ((found->addrkey[found->offset] & found->maskkey[found->offset]) !=
	    akey[found->offset])
		return (NULL);

	if (found->dupkey != NULL) {
		node = found;
		while (node != NULL && node->maskbitcount != count)
			node = node->dupkey;
		if (node == NULL)
			return (NULL);
		found = node;
	}
	return (found);
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_attach_mask                                          */
/* Returns:     Nil                                                         */
/* Parameters:  node(I)  - pointer to a radix tree node                     */
/*              mask(I)  - pointer to mask structure to add                 */
/*                                                                          */
/* Add the netmask to the given node in an ordering where the most specific */
/* netmask is at the top of the list.                                       */
/* ------------------------------------------------------------------------ */
static void
ipf_rx_attach_mask(ipf_rdx_node_t *node, ipf_rdx_mask_t *mask)
{
	ipf_rdx_mask_t **pm;
	ipf_rdx_mask_t *m;

	for (pm = &node->masks; (m = *pm) != NULL; pm = &m->next)
		if (m->maskbitcount < mask->maskbitcount)
			break;
	mask->next = *pm;
	*pm = mask;
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_insert                                               */
/* Returns:     ipf_rdx_node_t * - NULL on error, else pointer to the node  */
/*                                 added to the tree.                       */
/* Paramters:   head(I)  - pointer to tree head to add nodes to             */
/*              nodes(I) - pointer to radix nodes to be added               */
/*              dup(O)   - set to 1 if node is a duplicate, else 0.         */
/*                                                                          */
/* Add the new radix tree entry that owns nodes[] to the tree given by head.*/
/* If there is already a matching key in the table, "dup" will be set to 1  */
/* and the existing node pointer returned if there is a complete key match. */
/* A complete key match is a matching of all key data that is presented by  */
/* by the netmask.                                                          */
/* ------------------------------------------------------------------------ */
static ipf_rdx_node_t *
ipf_rx_insert(ipf_rdx_head_t *head, ipf_rdx_node_t *nodes, int *dup)
{
	ipf_rdx_mask_t **pmask;
	ipf_rdx_node_t *node;
	ipf_rdx_node_t *prev;
	ipf_rdx_mask_t *mask;
	ipf_rdx_node_t *cur;
	u_32_t nodemask;
	u_32_t *addr;
	u_32_t *data;
	int nodebits;
	u_32_t *key;
	u_32_t *end;
	u_32_t bits;
	int nodekey;
	int nodeoff;
	int nlen;
	int len;

	addr = nodes[0].addrkey;

	node = ipf_rx_find_addr(head->root, addr);
	len = ((addrfamily_t *)addr)->adf_len;
	key = (u_32_t *)&((addrfamily_t *)addr)->adf_addr;
	data= (u_32_t *)&((addrfamily_t *)node->addrkey)->adf_addr;
	end = (u_32_t *)((u_char *)addr + len);
	for (nlen = 0; key < end; data++, key++, nlen += 32)
		if (*key != *data)
			break;
	if (end == data) {
		*dup = 1;
		return (node);	/* Equal keys */
	}
	*dup = 0;

	bits = (ntohl(*data) ^ ntohl(*key));
	for (; bits != 0; nlen++) {
		if ((bits & 0x80000000) != 0)
			break;
		bits <<= 1;
	}
	nlen += ADF_OFF_BITS;
	nodes[1].index = nlen;
	nodes[1].bitmask = htonl(0x80000000 >> (nlen & 0x1f));
	nodes[0].offset = nlen / 32;
	nodes[1].offset = nlen / 32;

	/*
	 * Walk through the tree and look for the correct place to attach
	 * this node. ipf_rx_fin_addr is not used here because the place
	 * to attach this node may be an internal node (same key, different
	 * netmask.) Additionally, the depth of the search is forcibly limited
	 * here to not exceed the netmask, so that a short netmask will be
	 * added higher up the tree even if there are lower branches.
	 */
	cur = head->root;
	key = nodes[0].addrkey;
	do {
		prev = cur;
		if (key[cur->offset] & cur->bitmask) {
			cur = cur->right;
		} else {
			cur = cur->left;
		}
	} while (nlen > (unsigned)cur->index);

	if ((key[prev->offset] & prev->bitmask) == 0) {
		prev->left = &nodes[1];
	} else {
		prev->right = &nodes[1];
	}
	cur->parent = &nodes[1];
	nodes[1].parent = prev;
	if ((key[nodes[1].offset] & nodes[1].bitmask) == 0) {
		nodes[1].right = cur;
	} else {
		nodes[1].right = &nodes[0];
		nodes[1].left = cur;
	}

	nodeoff = nodes[0].offset;
	nodekey = nodes[0].addrkey[nodeoff];
	nodemask = nodes[0].lastmask;
	nodebits = nodes[0].maskbitcount;
	prev = NULL;
	/*
	 * Find the node up the tree with the largest pattern that still
	 * matches the node being inserted to see if this mask can be
	 * moved there.
	 */
	for (cur = nodes[1].parent; cur->root == 0; cur = cur->parent) {
		if (cur->maskbitcount <= nodebits)
			break;
		if (((cur - 1)->addrkey[nodeoff] & nodemask) != nodekey)
			break;
		prev = cur;
	}

	KMALLOC(mask, ipf_rdx_mask_t *);
	if (mask == NULL)
		return (NULL);
	bzero(mask, sizeof(*mask));
	mask->next = NULL;
	mask->node = &nodes[0];
	mask->maskbitcount = nodebits;
	mask->mask = nodes[0].maskkey;
	nodes[0].mymask = mask;

	if (prev != NULL) {
		ipf_rdx_mask_t *m;

		for (pmask = &prev->masks; (m = *pmask) != NULL;
		     pmask = &m->next) {
			if (m->maskbitcount < nodebits)
				break;
		}
	} else {
		/*
		 * No higher up nodes qualify, so attach mask locally.
		 */
		pmask = &nodes[0].masks;
	}
	mask->next = *pmask;
	*pmask = mask;

	/*
	 * Search the mask list on each child to see if there are any masks
	 * there that can be moved up to this newly inserted node.
	 */
	cur = nodes[1].right;
	if (cur->root == 0) {
		for (pmask = &cur->masks; (mask = *pmask) != NULL; ) {
			if (mask->maskbitcount < nodebits) {
				*pmask = mask->next;
				ipf_rx_attach_mask(&nodes[0], mask);
			} else {
				pmask = &mask->next;
			}
		}
	}
	cur = nodes[1].left;
	if (cur->root == 0 && cur != &nodes[0]) {
		for (pmask = &cur->masks; (mask = *pmask) != NULL; ) {
			if (mask->maskbitcount < nodebits) {
				*pmask = mask->next;
				ipf_rx_attach_mask(&nodes[0], mask);
			} else {
				pmask = &mask->next;
			}
		}
	}
	return (&nodes[0]);
}

/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_addroute                                             */
/* Returns:     ipf_rdx_node_t * - NULL on error, else pointer to the node  */
/*                                 added to the tree.                       */
/* Paramters:   head(I)  - pointer to tree head to search                   */
/*              addr(I)  - address portion of "route" to add                */
/*              mask(I)  - netmask portion of "route" to add                */
/*              nodes(I) - radix tree data nodes inside allocate structure  */
/*                                                                          */
/* Attempt to add a node to the radix tree. The key for the node is the     */
/* (addr,mask). No memory allocation for the radix nodes themselves is      */
/* performed here, the data structure that this radix node is being used to */
/* find is expected to house the node data itself however the call to       */
/* ipf_rx_insert() will attempt to allocate memory in order for netmask to  */
/* be promoted further up the tree.                                         */
/* In this case, the ip_pool_node_t structure from ip_pool.h contains both  */
/* the key material (addr,mask) and the radix tree nodes[].                 */
/*                                                                          */
/* The mechanics of inserting the node into the tree is handled by the      */
/* function ipf_rx_insert() above. Here, the code deals with the case       */
/* where the data to be inserted is a duplicate.                            */
/* ------------------------------------------------------------------------ */
ipf_rdx_node_t *
ipf_rx_addroute(ipf_rdx_head_t *head, addrfamily_t *addr, addrfamily_t *mask,
	ipf_rdx_node_t *nodes)
{
	ipf_rdx_node_t *node;
	ipf_rdx_node_t *prev;
	ipf_rdx_node_t *x;
	int dup;

	buildnodes(addr, mask, nodes);
	x = ipf_rx_insert(head, nodes, &dup);
	if (x == NULL)
		return (NULL);

	if (dup == 1) {
		node = &nodes[0];
		prev = NULL;
		/*
		 * The duplicate list is kept sorted with the longest
		 * mask at the top, meaning that the most specific entry
		 * in the listis found first. This list thus allows for
		 * duplicates such as 128.128.0.0/32 and 128.128.0.0/16.
		 */
		while ((x != NULL) && (x->maskbitcount > node->maskbitcount)) {
			prev = x;
			x = x->dupkey;
		}

		/*
		* Is it a complete duplicate? If so, return NULL and
		 * fail the insert. Otherwise, insert it into the list
		 * of netmasks active for this key.
		 */
		if ((x != NULL) && (x->maskbitcount == node->maskbitcount))
			return (NULL);

		if (prev != NULL) {
			nodes[0].dupkey = x;
			prev->dupkey = &nodes[0];
			nodes[0].parent = prev;
			if (x != NULL)
				x->parent = &nodes[0];
		} else {
			nodes[0].dupkey = x->dupkey;
			prev = x->parent;
			nodes[0].parent = prev;
			x->parent = &nodes[0];
			if (prev->left == x)
				prev->left = &nodes[0];
			else
				prev->right = &nodes[0];
		}
	}

	return (&nodes[0]);
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_delete                                               */
/* Returns:     ipf_rdx_node_t * - NULL on error, else node removed from    */
/*                                 the tree.                                */
/* Paramters:   head(I)  - pointer to tree head to search                   */
/*              addr(I)  - pointer to the address part of the key           */
/*              mask(I)  - pointer to the netmask part of the key           */
/*                                                                          */
/* Search for an entry in the radix tree that is an exact match for (addr,  */
/* mask) and remove it if it exists. In the case where (addr,mask) is a not */
/* a unique key, the tree structure itself is not changed - only the list   */
/* of duplicate keys.                                                       */
/* ------------------------------------------------------------------------ */
ipf_rdx_node_t *
ipf_rx_delete(ipf_rdx_head_t *head, addrfamily_t *addr, addrfamily_t *mask)
{
	ipf_rdx_mask_t **pmask;
	ipf_rdx_node_t *parent;
	ipf_rdx_node_t *found;
	ipf_rdx_node_t *prev;
	ipf_rdx_node_t *node;
	ipf_rdx_node_t *cur;
	ipf_rdx_mask_t *m;
	int count;

	found = ipf_rx_find_addr(head->root, (u_32_t *)addr);
	if (found == NULL)
		return (NULL);
	if (found->root == 1)
		return (NULL);
	count = count_mask_bits(mask, NULL);
	parent = found->parent;
	if (found->dupkey != NULL) {
		node = found;
		while (node != NULL && node->maskbitcount != count)
			node = node->dupkey;
		if (node == NULL)
			return (NULL);
		if (node != found) {
			/*
			 * Remove from the dupkey list. Here, "parent" is
			 * the previous node on the list (rather than tree)
			 * and "dupkey" is the next node on the list.
			 */
			parent = node->parent;
			parent->dupkey = node->dupkey;
			node->dupkey->parent = parent;
		} else {
			/*
			 * When removing the top node of the dupkey list,
			 * the pointers at the top of the list that point
			 * to other tree nodes need to be preserved and
			 * any children must have their parent updated.
			 */
			node = node->dupkey;
			node->parent = found->parent;
			node->right = found->right;
			node->left = found->left;
			found->right->parent = node;
			found->left->parent = node;
			if (parent->left == found)
				parent->left = node;
			else
				parent->right= node;
		}
	} else {
		if (count != found->maskbitcount)
			return (NULL);
		/*
		 * Remove the node from the tree and reconnect the subtree
		 * below.
		 */
		/*
		 * If there is a tree to the left, look for something to
		 * attach in place of "found".
		 */
		prev = found + 1;
		cur = parent->parent;
		if (parent != found + 1) {
			if ((found + 1)->parent->right == found + 1)
				(found + 1)->parent->right = parent;
			else
				(found + 1)->parent->left = parent;
			if (cur->right == parent) {
				if (parent->left == found) {
					cur->right = parent->right;
				} else if (parent->left != parent - 1) {
					cur->right = parent->left;
				} else {
					cur->right = parent - 1;
				}
				cur->right->parent = cur;
			} else {
				if (parent->right == found) {
					cur->left = parent->left;
				} else if (parent->right != parent - 1) {
					cur->left = parent->right;
				} else {
					cur->left = parent - 1;
				}
				cur->left->parent = cur;
			}
			parent->left = (found + 1)->left;
			if ((found + 1)->right != parent)
				parent->right = (found + 1)->right;
			parent->left->parent = parent;
			parent->right->parent = parent;
			parent->parent = (found + 1)->parent;

			parent->bitmask = prev->bitmask;
			parent->offset = prev->offset;
			parent->index = prev->index;
		} else {
			/*
			 * We found an edge node.
			 */
			cur = parent->parent;
			if (cur->left == parent) {
				if (parent->left == found) {
					cur->left = parent->right;
					parent->right->parent = cur;
				} else {
					cur->left = parent->left;
					parent->left->parent = cur;
				}
			} else {
				if (parent->right != found) {
					cur->right = parent->right;
					parent->right->parent = cur;
				} else {
					cur->right = parent->left;
					prev->left->parent = cur;
				}
			}
		}
	}

	/*
	 * Remove mask associated with this node.
	 */
	for (cur = parent; cur->root == 0; cur = cur->parent) {
		ipf_rdx_mask_t **pm;

		if (cur->maskbitcount <= found->maskbitcount)
			break;
		if (((cur - 1)->addrkey[found->offset] & found->bitmask) !=
		    found->addrkey[found->offset])
			break;
		for (pm = &cur->masks; (m = *pm) != NULL; )
			if (m->node == cur) {
				*pm = m->next;
				break;
			} else {
				pm = &m->next;
			}
	}
	KFREE(found->mymask);

	/*
	 * Masks that have been brought up to this node from below need to
	 * be sent back down.
	 */
	for (pmask = &parent->masks; (m = *pmask) != NULL; ) {
		*pmask = m->next;
		cur = m->node;
		if (cur == found)
			continue;
		if (found->addrkey[cur->offset] & cur->lastmask) {
			ipf_rx_attach_mask(parent->right, m);
		} else if (parent->left != found) {
			ipf_rx_attach_mask(parent->left, m);
		}
	}

	return (found);
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_walktree                                             */
/* Returns:     Nil                                                         */
/* Paramters:   head(I)   - pointer to tree head to search                  */
/*              walker(I) - function to call for each node in the tree      */
/*              arg(I)    - parameter to pass to walker, in addition to the */
/*                          node pointer                                    */
/*                                                                          */
/* A standard tree walking function except that it is iterative, rather     */
/* than recursive and tracks the next node in case the "walker" function    */
/* should happen to delete and free the current node. It thus goes without  */
/* saying that the "walker" function is not permitted to cause any change   */
/* in the validity of the data found at either the left or right child.     */
/* ------------------------------------------------------------------------ */
void
ipf_rx_walktree(ipf_rdx_head_t *head, radix_walk_func_t walker, void *arg)
{
	ipf_rdx_node_t *next;
	ipf_rdx_node_t *node = head->root;
	ipf_rdx_node_t *base;

	while (node->index >= 0)
		node = node->left;

	for (;;) {
		base = node;
		while ((node->parent->right == node) && (node->root == 0))
			node = node->parent;

		for (node = node->parent->right; node->index >= 0; )
			node = node->left;
		next = node;

		for (node = base; node != NULL; node = base) {
			base = node->dupkey;
			if (node->root == 0)
				walker(node, arg);
		}
		node = next;
		if (node->root)
			return;
	}
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_inithead                                             */
/* Returns:     int       - 0 = success, else failure                       */
/* Paramters:   softr(I)  - pointer to radix context                        */
/*              headp(O)  - location for where to store allocated tree head */
/*                                                                          */
/* This function allocates and initialises a radix tree head structure.     */
/* As a traditional radix tree, node 0 is used as the "0" sentinel and node */
/* "2" is used as the all ones sentinel, leaving node "1" as the root from  */
/* which the tree is hung with node "0" on its left and node "2" to the     */
/* right. The context, "softr", is used here to provide a common source of  */
/* the zeroes and ones data rather than have one per head.                  */
/* ------------------------------------------------------------------------ */
int
ipf_rx_inithead(radix_softc_t *softr, ipf_rdx_head_t **headp)
{
	ipf_rdx_head_t *ptr;
	ipf_rdx_node_t *node;

	KMALLOC(ptr, ipf_rdx_head_t *);
	*headp = ptr;
	if (ptr == NULL)
		return (-1);
	bzero(ptr, sizeof(*ptr));
	node = ptr->nodes;
	ptr->root = node + 1;
	node[0].index = ADF_OFF_BITS;
	node[0].index = -1 - node[0].index;
	node[1].index = ADF_OFF_BITS;
	node[2].index = node[0].index;
	node[0].parent = node + 1;
	node[1].parent = node + 1;
	node[2].parent = node + 1;
	node[1].bitmask = htonl(0x80000000);
	node[0].root = 1;
	node[1].root = 1;
	node[2].root = 1;
	node[0].offset = ADF_OFF_BITS >> 5;
	node[1].offset = ADF_OFF_BITS >> 5;
	node[2].offset = ADF_OFF_BITS >> 5;
	node[1].left = &node[0];
	node[1].right = &node[2];
	node[0].addrkey = (u_32_t *)softr->zeros;
	node[2].addrkey = (u_32_t *)softr->ones;
#ifdef RDX_DEBUG
	(void) strcpy(node[0].name, "0_ROOT");
	(void) strcpy(node[1].name, "1_ROOT");
	(void) strcpy(node[2].name, "2_ROOT");
#endif

	ptr->addaddr = ipf_rx_addroute;
	ptr->deladdr = ipf_rx_delete;
	ptr->lookup = ipf_rx_lookup;
	ptr->matchaddr = ipf_rx_match;
	ptr->walktree = ipf_rx_walktree;
	return (0);
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_freehead                                             */
/* Returns:     Nil                                                         */
/* Paramters:   head(I)  - pointer to tree head to free                     */
/*                                                                          */
/* This function simply free's up the radix tree head. Prior to calling     */
/* this function, it is expected that the tree will have been emptied.      */
/* ------------------------------------------------------------------------ */
void
ipf_rx_freehead(ipf_rdx_head_t *head)
{
	KFREE(head);
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_create                                               */
/* Parameters:  Nil                                                         */
/*                                                                          */
/* ------------------------------------------------------------------------ */
void *
ipf_rx_create(void)
{
	radix_softc_t *softr;

	KMALLOC(softr, radix_softc_t *);
	if (softr == NULL)
		return (NULL);
	bzero((char *)softr, sizeof(*softr));

	KMALLOCS(softr->zeros, u_char *, 3 * sizeof(addrfamily_t));
	if (softr->zeros == NULL) {
		KFREE(softr);
		return (NULL);
	}
	softr->ones = softr->zeros + sizeof(addrfamily_t);

	return (softr);
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_init                                                 */
/* Returns:     int       - 0 = success (always)                            */
/*                                                                          */
/* ------------------------------------------------------------------------ */
int
ipf_rx_init(void *ctx)
{
	radix_softc_t *softr = ctx;

	memset(softr->zeros, 0, 3 * sizeof(addrfamily_t));
	memset(softr->ones, 0xff, sizeof(addrfamily_t));

	return (0);
}


/* ------------------------------------------------------------------------ */
/* Function:    ipf_rx_destroy                                              */
/* Returns:     Nil                                                         */
/*                                                                          */
/* ------------------------------------------------------------------------ */
void
ipf_rx_destroy(void *ctx)
{
	radix_softc_t *softr = ctx;

	if (softr->zeros != NULL)
		KFREES(softr->zeros, 3 * sizeof(addrfamily_t));
	KFREE(softr);
}

/* ====================================================================== */

#ifdef RDX_DEBUG
/*
 * To compile this file as a standalone test unit, use -DRDX_DEBUG=1
 */
#define	NAME(x)	((x)->index < 0 ? (x)->name : (x)->name)
#define	GNAME(y)	((y) == NULL ? "NULL" : NAME(y))

typedef struct myst {
	struct ipf_rdx_node nodes[2];
	addrfamily_t	dst;
	addrfamily_t	mask;
	struct myst	*next;
	int		printed;
} myst_t;

typedef struct tabe_s {
	char	*host;
	char	*mask;
	char	*what;
} tabe_t;

tabe_t builtin[] = {
#if 1
	{ "192:168:100::0",	"48",			"d" },
	{ "192:168:100::2",	"128",			"d" },
#else
	{ "127.192.0.0",	"255.255.255.0",	"d" },
	{ "127.128.0.0",	"255.255.255.0",	"d" },
	{ "127.96.0.0",		"255.255.255.0",	"d" },
	{ "127.80.0.0",		"255.255.255.0",	"d" },
	{ "127.72.0.0",		"255.255.255.0",	"d" },
	{ "127.64.0.0",		"255.255.255.0",	"d" },
	{ "127.56.0.0",		"255.255.255.0",	"d" },
	{ "127.48.0.0",		"255.255.255.0",	"d" },
	{ "127.40.0.0",		"255.255.255.0",	"d" },
	{ "127.32.0.0",		"255.255.255.0",	"d" },
	{ "127.24.0.0",		"255.255.255.0",	"d" },
	{ "127.16.0.0",		"255.255.255.0",	"d" },
	{ "127.8.0.0",		"255.255.255.0",	"d" },
	{ "124.0.0.0",		"255.0.0.0",		"d" },
	{ "125.0.0.0",		"255.0.0.0",		"d" },
	{ "126.0.0.0",		"255.0.0.0",		"d" },
	{ "127.0.0.0",		"255.0.0.0",		"d" },
	{ "10.0.0.0",		"255.0.0.0",		"d" },
	{ "128.250.0.0",	"255.255.0.0",		"d" },
	{ "192.168.0.0",	"255.255.0.0",		"d" },
	{ "192.168.1.0",	"255.255.255.0",	"d" },
#endif
	{ NULL, NULL, NULL }
};

char *mtable[][1] = {
#if 1
	{ "192:168:100::2" },
	{ "192:168:101::2" },
#else
	{ "9.0.0.0" },
	{ "9.0.0.1" },
	{ "11.0.0.0" },
	{ "11.0.0.1" },
	{ "127.0.0.1" },
	{ "127.0.1.0" },
	{ "255.255.255.0" },
	{ "126.0.0.1" },
	{ "128.251.0.0" },
	{ "128.251.0.1" },
	{ "128.251.255.255" },
	{ "129.250.0.0" },
	{ "129.250.0.1" },
	{ "192.168.255.255" },
#endif
	{ NULL }
};


int forder[22] = {
	14, 13, 12,  5, 10,  3, 19,  7,  4, 20,  8,
	 2, 17,  9, 16, 11, 15,  1,  6, 18,  0, 21
};

static int nodecount = 0;
myst_t *myst_top = NULL;
tabe_t *ttable = NULL;

void add_addr(ipf_rdx_head_t *, int , int);
void checktree(ipf_rdx_head_t *);
void delete_addr(ipf_rdx_head_t *rnh, int item);
void dumptree(ipf_rdx_head_t *rnh);
void nodeprinter(ipf_rdx_node_t *, void *);
void printroots(ipf_rdx_head_t *);
void random_add(ipf_rdx_head_t *);
void random_delete(ipf_rdx_head_t *);
void test_addr(ipf_rdx_head_t *rnh, int pref, addrfamily_t *, int);


static void
ipf_rx_freenode(ipf_rdx_node_t *node, void *arg)
{
	ipf_rdx_head_t *head = arg;
	ipf_rdx_node_t *rv;
	myst_t *stp;

	stp = (myst_t *)node;
	rv = ipf_rx_delete(head, &stp->dst, &stp->mask);
	if (rv != NULL) {
		free(rv);
	}
}


const char *
addrname(addrfamily_t *ap)
{
	static char name[80];
	const char *txt;

	bzero((char *)name, sizeof(name));
	txt =  inet_ntop(ap->adf_family, &ap->adf_addr, name,
			 sizeof(name));
	return (txt);
}


void
fill6bits(int bits, u_int *msk)
{
	if (bits == 0) {
		msk[0] = 0;
		msk[1] = 0;
		msk[2] = 0;
		msk[3] = 0;
		return;
	}

	msk[0] = 0xffffffff;
	msk[1] = 0xffffffff;
	msk[2] = 0xffffffff;
	msk[3] = 0xffffffff;

	if (bits == 128)
		return;
	if (bits > 96) {
		msk[3] = htonl(msk[3] << (128 - bits));
	} else if (bits > 64) {
		msk[3] = 0;
		msk[2] = htonl(msk[2] << (96 - bits));
	} else if (bits > 32) {
		msk[3] = 0;
		msk[2] = 0;
		msk[1] = htonl(msk[1] << (64 - bits));
	} else {
		msk[3] = 0;
		msk[2] = 0;
		msk[1] = 0;
		msk[0] = htonl(msk[0] << (32 - bits));
	}
}


void
setaddr(addrfamily_t *afp, char *str)
{

	bzero((char *)afp, sizeof(*afp));

	if (strchr(str, ':') == NULL) {
		afp->adf_family = AF_INET;
		afp->adf_len = offsetof(addrfamily_t, adf_addr) + 4;
	} else {
		afp->adf_family = AF_INET6;
		afp->adf_len = offsetof(addrfamily_t, adf_addr) + 16;
	}
	inet_pton(afp->adf_family, str, &afp->adf_addr);
}


void
setmask(addrfamily_t *afp, char *str)
{
	if (strchr(str, '.') != NULL) {
		afp->adf_addr.in4.s_addr = inet_addr(str);
		afp->adf_len = offsetof(addrfamily_t, adf_addr) + 4;
	} else if (afp->adf_family == AF_INET) {
		afp->adf_addr.i6[0] = htonl(0xffffffff << (32 - atoi(str)));
		afp->adf_len = offsetof(addrfamily_t, adf_addr) + 4;
	} else if (afp->adf_family == AF_INET6) {
		fill6bits(atoi(str), afp->adf_addr.i6);
		afp->adf_len = offsetof(addrfamily_t, adf_addr) + 16;
	}
}


void
nodeprinter(ipf_rdx_node_t *node, void *arg)
{
	myst_t *stp = (myst_t *)node;

	printf("Node %-9.9s L %-9.9s R %-9.9s P %9.9s/%-9.9s %s/%d\n",
		node[0].name,
		GNAME(node[1].left), GNAME(node[1].right),
		GNAME(node[0].parent), GNAME(node[1].parent),
		addrname(&stp->dst), node[0].maskbitcount);
	if (stp->printed == -1)
		printf("!!! %d\n", stp->printed);
	else
		stp->printed = 1;
}


void
printnode(myst_t *stp)
{
	ipf_rdx_node_t *node = &stp->nodes[0];

	if (stp->nodes[0].index > 0)
		stp = (myst_t *)&stp->nodes[-1];

	printf("Node %-9.9s ", node[0].name);
	printf("L %-9.9s ", GNAME(node[1].left));
	printf("R %-9.9s ", GNAME(node[1].right));
	printf("P %9.9s", GNAME(node[0].parent));
	printf("/%-9.9s ", GNAME(node[1].parent));
	printf("%s P%d\n", addrname(&stp->dst), stp->printed);
}


void
buildtab(void)
{
	char line[80], *s;
	tabe_t *tab;
	int lines;
	FILE *fp;

	lines = 0;
	fp = fopen("hosts", "r");

	while (fgets(line, sizeof(line), fp) != NULL) {
		s = strchr(line, '\n');
		if (s != NULL)
			*s = '\0';
		lines++;
		if (lines == 1)
			tab = malloc(sizeof(*tab) * 2);
		else
			tab = realloc(tab, (lines + 1) * sizeof(*tab));
		tab[lines - 1].host = strdup(line);
		s = strchr(tab[lines - 1].host, '/');
		*s++ = '\0';
		tab[lines - 1].mask = s;
		tab[lines - 1].what = "d";
	}
	fclose(fp);

	tab[lines].host = NULL;
	tab[lines].mask = NULL;
	tab[lines].what = NULL;
	ttable = tab;
}


void
printroots(ipf_rdx_head_t *rnh)
{
	printf("Root.0.%s b %3d p %-9.9s l %-9.9s r %-9.9s\n",
		GNAME(&rnh->nodes[0]),
		rnh->nodes[0].index, GNAME(rnh->nodes[0].parent),
		GNAME(rnh->nodes[0].left), GNAME(rnh->nodes[0].right));
	printf("Root.1.%s b %3d p %-9.9s l %-9.9s r %-9.9s\n",
		GNAME(&rnh->nodes[1]),
		rnh->nodes[1].index, GNAME(rnh->nodes[1].parent),
		GNAME(rnh->nodes[1].left), GNAME(rnh->nodes[1].right));
	printf("Root.2.%s b %3d p %-9.9s l %-9.9s r %-9.9s\n",
		GNAME(&rnh->nodes[2]),
		rnh->nodes[2].index, GNAME(rnh->nodes[2].parent),
		GNAME(rnh->nodes[2].left), GNAME(rnh->nodes[2].right));
}


int
main(int argc, char *argv[])
{
	addrfamily_t af;
	ipf_rdx_head_t *rnh;
	radix_softc_t *ctx;
	int j;
	int i;

	rnh = NULL;

	buildtab();
	ctx = ipf_rx_create();
	ipf_rx_init(ctx);
	ipf_rx_inithead(ctx, &rnh);

	printf("=== ADD-0 ===\n");
	for (i = 0; ttable[i].host != NULL; i++) {
		add_addr(rnh, i, i);
		checktree(rnh);
	}
	printroots(rnh);
	ipf_rx_walktree(rnh, nodeprinter, NULL);
	printf("=== DELETE-0 ===\n");
	for (i = 0; ttable[i].host != NULL; i++) {
		delete_addr(rnh, i);
		printroots(rnh);
		ipf_rx_walktree(rnh, nodeprinter, NULL);
	}
	printf("=== ADD-1 ===\n");
	for (i = 0; ttable[i].host != NULL; i++) {
		setaddr(&af, ttable[i].host);
		add_addr(rnh, i, i); /*forder[i]); */
		checktree(rnh);
	}
	dumptree(rnh);
	ipf_rx_walktree(rnh, nodeprinter, NULL);
	printf("=== TEST-1 ===\n");
	for (i = 0; ttable[i].host != NULL; i++) {
		setaddr(&af, ttable[i].host);
		test_addr(rnh, i, &af, -1);
	}

	printf("=== TEST-2 ===\n");
	for (i = 0; mtable[i][0] != NULL; i++) {
		setaddr(&af, mtable[i][0]);
		test_addr(rnh, i, &af, -1);
	}
	printf("=== DELETE-1 ===\n");
	for (i = 0; ttable[i].host != NULL; i++) {
		if (ttable[i].what[0] != 'd')
			continue;
		delete_addr(rnh, i);
		for (j = 0; ttable[j].host != NULL; j++) {
			setaddr(&af, ttable[j].host);
			test_addr(rnh, i, &af, 3);
		}
		printroots(rnh);
		ipf_rx_walktree(rnh, nodeprinter, NULL);
	}

	dumptree(rnh);

	printf("=== ADD-2 ===\n");
	random_add(rnh);
	checktree(rnh);
	dumptree(rnh);
	ipf_rx_walktree(rnh, nodeprinter, NULL);
	printf("=== DELETE-2 ===\n");
	random_delete(rnh);
	checktree(rnh);
	dumptree(rnh);

	ipf_rx_walktree(rnh, ipf_rx_freenode, rnh);

	return (0);
}


void
dumptree(ipf_rdx_head_t *rnh)
{
	myst_t *stp;

	printf("VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV\n");
	printroots(rnh);
	for (stp = myst_top; stp; stp = stp->next)
		printnode(stp);
	printf("^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n");
}


void
test_addr(ipf_rdx_head_t *rnh, int pref, addrfamily_t *addr, int limit)
{
	static int extras[14] = { 0, -1, 1, 3, 5, 8, 9,
				  15, 16, 19, 255, 256, 65535, 65536
	};
	ipf_rdx_node_t *rn;
	addrfamily_t af;
	char name[80];
	myst_t *stp;
	int i;

	memset(&af, 0, sizeof(af));
#if 0
	if (limit < 0 || limit > 14)
		limit = 14;

	for (i = 0; i < limit; i++) {
		if (ttable[i].host == NULL)
			break;
		setaddr(&af, ttable[i].host);
		printf("%d.%d.LOOKUP(%s)", pref, i, addrname(&af));
		rn = ipf_rx_match(rnh, &af);
		stp = (myst_t *)rn;
		printf(" = %s (%s/%d)\n", GNAME(rn),
			rn ? addrname(&stp->dst) : "NULL",
			rn ? rn->maskbitcount : 0);
	}
#else
	printf("%d.%d.LOOKUP(%s)", pref, -1, addrname(addr));
	rn = ipf_rx_match(rnh, addr);
	stp = (myst_t *)rn;
	printf(" = %s (%s/%d)\n", GNAME(rn),
		rn ? addrname(&stp->dst) : "NULL", rn ? rn->maskbitcount : 0);
#endif
}


void
delete_addr(ipf_rdx_head_t *rnh, int item)
{
	ipf_rdx_node_t *rn;
	addrfamily_t mask;
	addrfamily_t af;
	myst_t **pstp;
	myst_t *stp;

	memset(&af, 0, sizeof(af));
	memset(&mask, 0, sizeof(mask));
	setaddr(&af, ttable[item].host);
	mask.adf_family = af.adf_family;
	setmask(&mask, ttable[item].mask);

	printf("DELETE(%s)\n", addrname(&af));
	rn = ipf_rx_delete(rnh, &af, &mask);
	if (rn == NULL) {
		printf("FAIL LOOKUP DELETE\n");
		checktree(rnh);
		for (stp = myst_top; stp != NULL; stp = stp->next)
			if (stp->printed != -1)
				stp->printed = -2;
		ipf_rx_walktree(rnh, nodeprinter, NULL);
		dumptree(rnh);
		abort();
	}
	printf("%d.delete(%s) = %s\n", item, addrname(&af), GNAME(rn));

	for (pstp = &myst_top; (stp = *pstp) != NULL; pstp = &stp->next)
		if (stp == (myst_t *)rn)
			break;
	stp->printed = -1;
	stp->nodes[0].parent = &stp->nodes[0];
	stp->nodes[1].parent = &stp->nodes[1];
	*pstp = stp->next;
	free(stp);
	nodecount--;
	checktree(rnh);
}


void
add_addr(ipf_rdx_head_t *rnh, int n, int item)
{
	ipf_rdx_node_t *rn;
	myst_t *stp;

	stp = calloc(1, sizeof(*stp));
	rn = (ipf_rdx_node_t *)stp;
	setaddr(&stp->dst, ttable[item].host);
	stp->mask.adf_family = stp->dst.adf_family;
	setmask(&stp->mask, ttable[item].mask);
	stp->next = myst_top;
	myst_top = stp;
#ifdef RDX_DEBUG
	(void) snprintf(rn[0].name, sizeof(ipf_rdx_node.name), "_BORN.0");
	(void) snprintf(rn[1].name, sizeof(ipf_rdx_node.name), "_BORN.1");
	rn = ipf_rx_addroute(rnh, &stp->dst, &stp->mask, stp->nodes);
	(void) snprintf(rn[0].name, sizeof(ipf_rdx_node.name), "%d_NODE.0", item);
	(void) snprintf(rn[1].name, sizeof(ipf_rdx_node.name), "%d_NODE.1", item);
	printf("ADD %d/%d %s/%s\n", n, item, rn[0].name, rn[1].name);
#endif
	nodecount++;
	checktree(rnh);
}


void
checktree(ipf_rdx_head_t *head)
{
	myst_t *s1;
	ipf_rdx_node_t *rn;

	if (nodecount <= 1)
		return;

	for (s1 = myst_top; s1 != NULL; s1 = s1->next) {
		int fault = 0;
		if (s1->printed == -1)
			continue;
		rn = &s1->nodes[1];
		if (rn->right->parent != rn)
			fault |= 1;
		if (rn->left->parent != rn)
			fault |= 2;
		if (rn->parent->left != rn && rn->parent->right != rn)
			fault |= 4;
		if (fault != 0) {
			printf("FAULT %#x %s\n", fault, rn->name);
			dumptree(head);
			ipf_rx_walktree(head, nodeprinter, NULL);
			fflush(stdout);
			fflush(stderr);
			printf("--\n");
			abort();
		}
	}
}


int *
randomize(int *pnitems)
{
	int *order;
	int nitems;
	int choice;
	int j;
	int i;

	nitems = sizeof(ttable) / sizeof(ttable[0]);
	*pnitems = nitems;
	order = calloc(nitems, sizeof(*order));
	srandom(getpid() * time(NULL));
	memset(order, 0xff, nitems * sizeof(*order));
	order[21] = 21;
	for (i = 0; i < nitems - 1; i++) {
		do {
			choice = rand() % (nitems - 1);
			for (j = 0; j < nitems; j++)
				if (order[j] == choice)
					break;
		} while (j != nitems);
		order[i] = choice;
	}

	return (order);
}


void
random_add(ipf_rdx_head_t *rnh)
{
	int *order;
	int nitems;
	int i;

	order = randomize(&nitems);

	for (i = 0; i < nitems - 1; i++) {
		add_addr(rnh, i, order[i]);
		checktree(rnh);
	}

	free(order);
}


void
random_delete(ipf_rdx_head_t *rnh)
{
	int *order;
	int nitems;
	int i;

	order = randomize(&nitems);

	for (i = 0; i < nitems - 1; i++) {
		delete_addr(rnh, i);
		checktree(rnh);
	}

	free(order);
}
#endif /* RDX_DEBUG */