radij.c

This a good VPN source

	register struct radij_node *x;
	register caddr_t cp, cplim;
	register int b, mlen, j;
	int maskduplicated;

	mlen = *(u_char *)netmask;
	if (search) {
		x = rj_search(netmask, rj_masktop);
		mlen = *(u_char *)netmask;
		if (Bcmp(netmask, x->rj_key, mlen) == 0)
			return (x);
	}
	R_Malloc(x, struct radij_node *, maj_keylen + 2 * sizeof (*x));
	if (x == 0)
		return (0);
	Bzero(x, maj_keylen + 2 * sizeof (*x));
	cp = (caddr_t)(x + 2);
	Bcopy(netmask, cp, mlen);
	netmask = cp;
	x = rj_insert(netmask, mask_rjhead, &maskduplicated, x);
	/*
	 * Calculate index of mask.
	 */
	cplim = netmask + mlen;
	for (cp = netmask + skip; cp < cplim; cp++)
		if (*(u_char *)cp != 0xff)
			break;
	b = (cp - netmask) << 3;
	if (cp != cplim) {
		if (*cp != 0) {
			gotOddMasks = 1;
			for (j = 0x80; j; b++, j >>= 1)  
				if ((j & *cp) == 0)
					break;
		}
	}
	x->rj_b = -1 - b;
	return (x);
}

#if 0
struct radij_node *
#endif
int
rj_addroute(v_arg, n_arg, head, treenodes)
	void *v_arg, *n_arg;
	struct radij_node_head *head;
	struct radij_node treenodes[2];
{
	caddr_t v = (caddr_t)v_arg, netmask = (caddr_t)n_arg;
	register struct radij_node *t, *x=NULL, *tt;
	struct radij_node *saved_tt, *top = head->rnh_treetop;
	short b = 0, b_leaf;
	int mlen, keyduplicated;
	caddr_t cplim;
	struct radij_mask *m, **mp;

	/*
	 * In dealing with non-contiguous masks, there may be
	 * many different routes which have the same mask.
	 * We will find it useful to have a unique pointer to
	 * the mask to speed avoiding duplicate references at
	 * nodes and possibly save time in calculating indices.
	 */
	if (netmask)  {
		x = rj_search(netmask, rj_masktop);
		mlen = *(u_char *)netmask;
		if (Bcmp(netmask, x->rj_key, mlen) != 0) {
			x = rj_addmask(netmask, 0, top->rj_off);
			if (x == 0)
				return -ENOMEM; /* (0) rgb */
		}
		netmask = x->rj_key;
		b = -1 - x->rj_b;
	}
	/*
	 * Deal with duplicated keys: attach node to previous instance
	 */
	saved_tt = tt = rj_insert(v, head, &keyduplicated, treenodes);
#ifdef RJ_DEBUG
	printk("addkey: duplicated: %d\n", keyduplicated);
#endif
	if (keyduplicated) {
		do {
			if (tt->rj_mask == netmask)
				return -EEXIST; /* -ENXIO; (0) rgb */
			t = tt;
			if (netmask == 0 ||
			    (tt->rj_mask && rj_refines(netmask, tt->rj_mask)))
				break;
		} while ((tt = tt->rj_dupedkey));
		/*
		 * If the mask is not duplicated, we wouldn't
		 * find it among possible duplicate key entries
		 * anyway, so the above test doesn't hurt.
		 *
		 * We sort the masks for a duplicated key the same way as
		 * in a masklist -- most specific to least specific.
		 * This may require the unfortunate nuisance of relocating
		 * the head of the list.
		 */
		if (tt && t == saved_tt) {
			struct	radij_node *xx = x;
			/* link in at head of list */
			(tt = treenodes)->rj_dupedkey = t;
			tt->rj_flags = t->rj_flags;
			tt->rj_p = x = t->rj_p;
			if (x->rj_l == t) x->rj_l = tt; else x->rj_r = tt;
			saved_tt = tt; x = xx;
		} else {
			(tt = treenodes)->rj_dupedkey = t->rj_dupedkey;
			t->rj_dupedkey = tt;
		}
#ifdef RJ_DEBUG
		t=tt+1; tt->rj_info = rj_nodenum++; t->rj_info = rj_nodenum++;
		tt->rj_twin = t; tt->rj_ybro = rj_clist; rj_clist = tt;
#endif /* RJ_DEBUG */
		t = saved_tt;
		tt->rj_key = (caddr_t) v;
		tt->rj_b = -1;
		tt->rj_flags = t->rj_flags & ~RJF_ROOT;
	}
	/*
	 * Put mask in tree.
	 */
	if (netmask) {
		tt->rj_mask = netmask;
		tt->rj_b = x->rj_b;
	}
	t = saved_tt->rj_p;
	b_leaf = -1 - t->rj_b;
	if (t->rj_r == saved_tt) x = t->rj_l; else x = t->rj_r;
	/* Promote general routes from below */
	if (x->rj_b < 0) { 
		if (x->rj_mask && (x->rj_b >= b_leaf) && x->rj_mklist == 0) {
			MKGet(m);
			if (m) {
				Bzero(m, sizeof *m);
				m->rm_b = x->rj_b;
				m->rm_mask = x->rj_mask;
				x->rj_mklist = t->rj_mklist = m;
			}
		}
	} else if (x->rj_mklist) {
		/*
		 * Skip over masks whose index is > that of new node
		 */
		for (mp = &x->rj_mklist; (m = *mp); mp = &m->rm_mklist)
			if (m->rm_b >= b_leaf)
				break;
		t->rj_mklist = m; *mp = 0;
	}
	/* Add new route to highest possible ancestor's list */
	if ((netmask == 0) || (b > t->rj_b )) {
#ifdef RJ_DEBUG
	        printk("klips:radij.c: netmask = %p or b(%d)>t->rjb(%d)\n", netmask, b, t->rj_b);
#endif
		return 0; /* tt rgb */ /* can't lift at all */
	}
	b_leaf = tt->rj_b;
	do {
		x = t;
		t = t->rj_p;
	} while (b <= t->rj_b && x != top);
	/*
	 * Search through routes associated with node to
	 * insert new route according to index.
	 * For nodes of equal index, place more specific
	 * masks first.
	 */
	cplim = netmask + mlen;
	for (mp = &x->rj_mklist; (m = *mp); mp = &m->rm_mklist) {
		if (m->rm_b < b_leaf)
			continue;
		if (m->rm_b > b_leaf)
			break;
		if (m->rm_mask == netmask) {
			m->rm_refs++;
			tt->rj_mklist = m;
#ifdef RJ_DEBUG
			printk("klips:radij.c: m->rm_mask %p == netmask\n", netmask);
#endif
			return 0; /* tt rgb */
		}
		if (rj_refines(netmask, m->rm_mask))
			break;
	}
	MKGet(m);
	if (m == 0) {
		printk("klips_debug:rj_addroute: "
		       "Mask for route not entered\n");
		return 0; /* (tt) rgb */
	}
	Bzero(m, sizeof *m);
	m->rm_b = b_leaf;
	m->rm_mask = netmask;
	m->rm_mklist = *mp;
	*mp = m;
	tt->rj_mklist = m;
#ifdef RJ_DEBUG
	printk("klips:radij.c: addroute done\n");
#endif
	return 0; /* tt rgb */
}

int
rj_delete(v_arg, netmask_arg, head, node)
	void *v_arg, *netmask_arg;
	struct radij_node_head *head;
	struct radij_node **node;
{
	register struct radij_node *t, *p, *x, *tt;
	struct radij_mask *m, *saved_m, **mp;
	struct radij_node *dupedkey, *saved_tt, *top;
	caddr_t v, netmask;
	int b, head_off, vlen;

	v = v_arg;
	netmask = netmask_arg;
	x = head->rnh_treetop;
	tt = rj_search(v, x);
	head_off = x->rj_off;
	vlen =  *(u_char *)v;
	saved_tt = tt;
	top = x;
	if (tt == 0 ||
	    Bcmp(v + head_off, tt->rj_key + head_off, vlen - head_off))
		return -EFAULT; /* (0) rgb */
	/*
	 * Delete our route from mask lists.
	 */
	if ((dupedkey = tt->rj_dupedkey)) {
		if (netmask) 
			netmask = rj_search(netmask, rj_masktop)->rj_key;
		while (tt->rj_mask != netmask)
			if ((tt = tt->rj_dupedkey) == 0)
				return -ENOENT; /* -ENXIO; (0) rgb */
	}
	if (tt->rj_mask == 0 || (saved_m = m = tt->rj_mklist) == 0)
		goto on1;
	if (m->rm_mask != tt->rj_mask) {
		printk("klips_debug:rj_delete: "
		       "inconsistent annotation\n");
		goto on1;
	}
	if (--m->rm_refs >= 0)
		goto on1;
	b = -1 - tt->rj_b;
	t = saved_tt->rj_p;
	if (b > t->rj_b)
		goto on1; /* Wasn't lifted at all */
	do {
		x = t;
		t = t->rj_p;
	} while (b <= t->rj_b && x != top);
	for (mp = &x->rj_mklist; (m = *mp); mp = &m->rm_mklist)
		if (m == saved_m) {
			*mp = m->rm_mklist;
			MKFree(m);
			break;
		}
	if (m == 0)
		printk("klips_debug:rj_delete: "
		       "couldn't find our annotation\n");
on1:
	/*
	 * Eliminate us from tree
	 */
	if (tt->rj_flags & RJF_ROOT)
		return -EFAULT; /* (0) rgb */
#ifdef RJ_DEBUG
	/* Get us out of the creation list */
	for (t = rj_clist; t && t->rj_ybro != tt; t = t->rj_ybro) {}
	if (t) t->rj_ybro = tt->rj_ybro;
#endif /* RJ_DEBUG */
	t = tt->rj_p;
	if (dupedkey) {
		if (tt == saved_tt) {
			x = dupedkey; x->rj_p = t;
			if (t->rj_l == tt) t->rj_l = x; else t->rj_r = x;
		} else {
			for (x = p = saved_tt; p && p->rj_dupedkey != tt;)
				p = p->rj_dupedkey;
			if (p) p->rj_dupedkey = tt->rj_dupedkey;
			else printk("klips_debug:rj_delete: "
				       "couldn't find node that we started with\n");
		}
		t = tt + 1;
		if  (t->rj_flags & RJF_ACTIVE) {
#ifndef RJ_DEBUG
			*++x = *t; p = t->rj_p;
#else
			b = t->rj_info; *++x = *t; t->rj_info = b; p = t->rj_p;
#endif /* RJ_DEBUG */
			if (p->rj_l == t) p->rj_l = x; else p->rj_r = x;
			x->rj_l->rj_p = x; x->rj_r->rj_p = x;
		}
		goto out;
	}
	if (t->rj_l == tt) x = t->rj_r; else x = t->rj_l;
	p = t->rj_p;
	if (p->rj_r == t) p->rj_r = x; else p->rj_l = x;
	x->rj_p = p;
	/*
	 * Demote routes attached to us.
	 */
	if (t->rj_mklist) {
		if (x->rj_b >= 0) {
			for (mp = &x->rj_mklist; (m = *mp);)
				mp = &m->rm_mklist;
			*mp = t->rj_mklist;
		} else {
			for (m = t->rj_mklist; m;) {
				struct radij_mask *mm = m->rm_mklist;
				if (m == x->rj_mklist && (--(m->rm_refs) < 0)) {
					x->rj_mklist = 0;
					MKFree(m);
				} else 
					printk("klips_debug:rj_delete: "
					    "Orphaned Mask 0p%p at 0p%p\n", m, x);
				m = mm;
			}
		}
	}
	/*
	 * We may be holding an active internal node in the tree.
	 */
	x = tt + 1;
	if (t != x) {
#ifndef RJ_DEBUG
		*t = *x;
#else
		b = t->rj_info; *t = *x; t->rj_info = b;
#endif /* RJ_DEBUG */
		t->rj_l->rj_p = t; t->rj_r->rj_p = t;
		p = x->rj_p;
		if (p->rj_l == x) p->rj_l = t; else p->rj_r = t;
	}
out:
	tt->rj_flags &= ~RJF_ACTIVE;
	tt[1].rj_flags &= ~RJF_ACTIVE;
	*node = tt;
	return 0; /* (tt) rgb */
}

int
rj_walktree(h, f, w)
	struct radij_node_head *h;
	register int (*f)(struct radij_node *,void *);
	void *w;
{
	int error;
	struct radij_node *base, *next;
	register struct radij_node *rn;

	if(!h || !f /* || !w */) {
		return -ENODATA;
	}

	rn = h->rnh_treetop;
	/*
	 * This gets complicated because we may delete the node
	 * while applying the function f to it, so we need to calculate
	 * the successor node in advance.
	 */
	/* First time through node, go left */
	while (rn->rj_b >= 0)
		rn = rn->rj_l;
	for (;;) {
#ifdef CONFIG_KLIPS_DEBUG
		if(debug_radij) {
			printk("klips_debug:rj_walktree: "
			       "for: rn=0p%p rj_b=%d rj_flags=%x",
			       rn,
			       rn->rj_b,
			       rn->rj_flags);
			rn->rj_b >= 0 ?
				printk(" node off=%x\n",
				       rn->rj_off) :
				printk(" leaf key = %08x->%08x\n",
				       (u_int)ntohl(((struct sockaddr_encap *)rn->rj_key)->sen_ip_src.s_addr),
				       (u_int)ntohl(((struct sockaddr_encap *)rn->rj_key)->sen_ip_dst.s_addr))
				;
		}
#endif /* CONFIG_KLIPS_DEBUG */
		base = rn;
		/* If at right child go back up, otherwise, go right */
		while (rn->rj_p->rj_r == rn && (rn->rj_flags & RJF_ROOT) == 0)
			rn = rn->rj_p;
		/* Find the next *leaf* since next node might vanish, too */
		for (rn = rn->rj_p->rj_r; rn->rj_b >= 0;)
			rn = rn->rj_l;
		next = rn;
#ifdef CONFIG_KLIPS_DEBUG
		if(debug_radij) {
			printk("klips_debug:rj_walktree: "
			       "processing leaves, rn=0p%p rj_b=%d rj_flags=%x",
			       rn,
			       rn->rj_b,
			       rn->rj_flags);
			rn->rj_b >= 0 ?
				printk(" node off=%x\n",
				       rn->rj_off) :
				printk(" leaf key = %08x->%08x\n",
				       (u_int)ntohl(((struct sockaddr_encap *)rn->rj_key)->sen_ip_src.s_addr),
				       (u_int)ntohl(((struct sockaddr_encap *)rn->rj_key)->sen_ip_dst.s_addr))
				;
		}
#endif /* CONFIG_KLIPS_DEBUG */
		/* Process leaves */