diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/include/linux/ipv6.h linux-2.5/include/linux/ipv6.h
--- linux-2.5.OLD/include/linux/ipv6.h	Mon Jun 30 20:36:00 2003
+++ linux-2.5/include/linux/ipv6.h	Fri Jul  4 23:32:14 2003
@@ -150,7 +150,9 @@
 	struct in6_addr 	rcv_saddr;
 	struct in6_addr		daddr;
 	struct in6_addr		*daddr_cache;
-
+#ifdef CONFIG_IPV6_SUBTREES
+	struct in6_addr		*saddr_cache;
+#endif
 	__u32			flow_label;
 	__u32			frag_size;
 	int			hop_limit;
diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/include/net/ip6_route.h linux-2.5/include/net/ip6_route.h
--- linux-2.5.OLD/include/net/ip6_route.h	Mon Jun 30 20:36:00 2003
+++ linux-2.5/include/net/ip6_route.h	Fri Jul  4 23:32:14 2003
@@ -102,7 +102,8 @@
  */
 
 static inline void ip6_dst_store(struct sock *sk, struct dst_entry *dst,
-				     struct in6_addr *daddr)
+				 struct in6_addr *daddr,
+				 struct in6_addr *saddr)
 {
 	struct ipv6_pinfo *np = inet6_sk(sk);
 	struct rt6_info *rt = (struct rt6_info *) dst;
@@ -110,6 +111,9 @@
 	write_lock(&sk->sk_dst_lock);
 	__sk_dst_set(sk, dst);
 	np->daddr_cache = daddr;
+#ifdef CONFIG_IPV6_SUBTREES
+	np->saddr_cache = saddr;
+#endif
 	np->dst_cookie = rt->rt6i_node ? rt->rt6i_node->fn_sernum : 0;
 	write_unlock(&sk->sk_dst_lock);
 }
diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/net/ipv6/Kconfig linux-2.5/net/ipv6/Kconfig
--- linux-2.5.OLD/net/ipv6/Kconfig	Mon Jun 30 20:36:03 2003
+++ linux-2.5/net/ipv6/Kconfig	Fri Jul  4 23:32:14 2003
@@ -63,4 +63,12 @@
 
 	  If unsure, say N.
 
+config IPV6_SUBTREES
+	bool "IPv6: Source address routing"
+	depends on IPV6
+	---help---
+	  Support for advanced routing by both source and destination address.
+
+	  If unsure, say N.
+
 source "net/ipv6/netfilter/Kconfig"
diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/net/ipv6/ip6_fib.c linux-2.5/net/ipv6/ip6_fib.c
--- linux-2.5.OLD/net/ipv6/ip6_fib.c	Mon Jun 30 20:36:03 2003
+++ linux-2.5/net/ipv6/ip6_fib.c	Fri Jul  4 23:32:14 2003
@@ -18,6 +18,7 @@
  * 	Yuji SEKIYA @USAGI:	Support default route on router node;
  * 				remove ip6_null_entry from the top of
  * 				routing table.
+ * 	Ville Nuorvala:		Fixed routing subtrees.
  */
 #include <linux/config.h>
 #include <linux/errno.h>
@@ -496,6 +497,8 @@
 		mod_timer(&ip6_fib_timer, jiffies + ip6_rt_gc_interval);
 }
 
+static struct rt6_info * fib6_find_prefix(struct fib6_node *fn);
+
 /*
  *	Add routing information to the routing tree.
  *	<destination addr>/<source addr>
@@ -506,6 +509,9 @@
 {
 	struct fib6_node *fn;
 	int err = -ENOMEM;
+#ifdef CONFIG_IPV6_SUBTREES
+	struct fib6_node *pn = NULL;
+#endif
 
 	fn = fib6_add_1(root, &rt->rt6i_dst.addr, sizeof(struct in6_addr),
 			rt->rt6i_dst.plen, (u8*) &rt->rt6i_dst - (u8*) rt);
@@ -558,10 +564,6 @@
 			/* Now link new subtree to main tree */
 			sfn->parent = fn;
 			fn->subtree = sfn;
-			if (fn->leaf == NULL) {
-				fn->leaf = rt;
-				atomic_inc(&rt->rt6i_ref);
-			}
 		} else {
 			sn = fib6_add_1(fn->subtree, &rt->rt6i_src.addr,
 					sizeof(struct in6_addr), rt->rt6i_src.plen,
@@ -571,6 +573,13 @@
 				goto st_failure;
 		}
 
+		/* fib6_add_1 might have cleared the old leaf pointer */
+		if (fn->leaf == NULL) {
+			fn->leaf = rt;
+		  atomic_inc(&rt->rt6i_ref);
+		}
+
+		pn = fn;
 		fn = sn;
 	}
 #endif
@@ -584,8 +593,24 @@
 	}
 
 out:
-	if (err)
+	if (err) {
+#ifdef CONFIG_IPV6_SUBTREES
+		/* If fib6_add_1 has cleared the old leaf pointer in the 
+		   super-tree leaf node we have to find a new one for it. */
+		
+		if (pn && !(pn->fn_flags & RTN_RTINFO)) {
+			pn->leaf = fib6_find_prefix(pn);
+#if RT6_DEBUG >= 2
+			if (!pn->leaf) {
+				BUG_TRAP(pn->leaf);
+				pn->leaf = &ip6_null_entry;
+			}
+#endif
+			atomic_inc(&pn->leaf->rt6i_ref);
+		}
+#endif
 		dst_free(&rt->u.dst);
+	}
 	return err;
 
 #ifdef CONFIG_IPV6_SUBTREES
@@ -637,32 +662,31 @@
 		break;
 	}
 
-	while ((fn->fn_flags & RTN_ROOT) == 0) {
+	for (;;) {
+		if (SUBTREE(fn) || fn->fn_flags & RTN_RTINFO) {
+			struct rt6key *key;
+			key = (struct rt6key *) ((u8 *) fn->leaf + args->offset);
+			
+			if (addr_match(&key->addr, args->addr, key->plen)) {
 #ifdef CONFIG_IPV6_SUBTREES
-		if (fn->subtree) {
-			struct fib6_node *st;
-			struct lookup_args *narg;
-
-			narg = args + 1;
-
-			if (narg->addr) {
-				st = fib6_lookup_1(fn->subtree, narg);
-
-				if (st && !(st->fn_flags & RTN_ROOT))
-					return st;
-			}
-		}
+				if (fn->subtree) {
+					struct lookup_args *narg = args + 1;
+					
+					if (narg->addr) {
+						struct fib6_node *st;
+						st = fib6_lookup_1(fn->subtree, narg);
+						
+						if (st)
+							return st;
+					}
+				}
 #endif
-
-		if (fn->fn_flags & RTN_RTINFO) {
-			struct rt6key *key;
-
-			key = (struct rt6key *) ((u8 *) fn->leaf +
-						 args->offset);
-
-			if (addr_match(&key->addr, args->addr, key->plen))
-				return fn;
+				if (fn->fn_flags & RTN_RTINFO)
+					return fn;
+			}
 		}
+		if (fn->fn_flags & RTN_ROOT)
+			break;
 
 		fn = fn->parent;
 	}
@@ -741,12 +765,12 @@
 
 #ifdef CONFIG_IPV6_SUBTREES
 	if (src_len) {
-		BUG_TRAP(saddr!=NULL);
-		if (fn == NULL)
-			fn = fn->subtree;
-		if (fn)
-			fn = fib6_locate_1(fn, saddr, src_len,
+		BUG_TRAP(saddr != NULL);
+		if (fn && fn->subtree)
+			fn = fib6_locate_1(fn->subtree, saddr, src_len,
 					   (u8*) &rt->rt6i_src - (u8*) rt);
+		else
+			return NULL;
 	}
 #endif
 
diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/net/ipv6/ip6_output.c linux-2.5/net/ipv6/ip6_output.c
--- linux-2.5.OLD/net/ipv6/ip6_output.c	Wed Jul  2 15:42:03 2003
+++ linux-2.5/net/ipv6/ip6_output.c	Fri Jul  4 23:32:14 2003
@@ -531,6 +531,7 @@
 	struct ipv6_pinfo *np = inet6_sk(sk);
 	struct in6_addr final_dst_buf, *final_dst = NULL;
 	struct dst_entry *dst;
+	struct rt6_info *rt;
 	int err = 0;
 	unsigned int pktlength, jumbolen, mtu;
 
@@ -546,11 +547,11 @@
 
 	dst = __sk_dst_check(sk, np->dst_cookie);
 	if (dst) {
-		struct rt6_info *rt = (struct rt6_info*)dst;
+		rt = (struct rt6_info*)dst;
 
 			/* Yes, checking route validity in not connected
 			   case is not very simple. Take into account,
-			   that we do not support routing by source, TOS,
+			   that we do not support routing by TOS,
 			   and MSG_DONTROUTE 		--ANK (980726)
 
 			   1. If route was host route, check that
@@ -570,6 +571,13 @@
 		      ipv6_addr_cmp(&fl->fl6_dst, &rt->rt6i_dst.addr))
 		     && (np->daddr_cache == NULL ||
 			 ipv6_addr_cmp(&fl->fl6_dst, np->daddr_cache)))
+#ifdef CONFIG_IPV6_SUBTREES
+		    || (!ipv6_addr_any(&fl->fl6_src)
+			&& (rt->rt6i_src.plen != 128 ||
+			    ipv6_addr_cmp(&fl->fl6_src, &rt->rt6i_src.addr))
+			&& (np->saddr_cache == NULL ||
+			    ipv6_addr_cmp(&fl->fl6_src, np->saddr_cache)))
+#endif
 		    || (fl->oif && fl->oif != dst->dev->ifindex)) {
 			dst = NULL;
 		} else
@@ -596,6 +604,20 @@
 			goto out;
 		}
 	}
+#ifdef CONFIG_IPV6_SUBTREES
+	rt = (struct rt6_info*)dst;
+	if (ipv6_addr_cmp(&fl->fl6_src, &np->saddr) && 
+	    (rt->rt6i_src.plen != 128 || 
+	     ipv6_addr_cmp(&fl->fl6_src, &rt->rt6i_src.addr))) {
+		dst_release(dst);
+		dst = ip6_route_output(sk, fl);
+		if (dst->error) {
+			IP6_INC_STATS(Ip6OutNoRoutes);
+			dst_release(dst);
+			return -ENETUNREACH;
+		}
+	}
+#endif
 	pktlength = length;
 
         if (dst) {
@@ -719,7 +741,9 @@
 out:
 	ip6_dst_store(sk, dst,
 		      !ipv6_addr_cmp(&fl->fl6_dst, &np->daddr) ?
-		      &np->daddr : NULL);
+		      &np->daddr : NULL,
+		      !ipv6_addr_cmp(&fl->fl6_src, &np->saddr) ?
+		      &np->saddr : NULL);
 	if (err > 0)
 		err = np->recverr ? net_xmit_errno(err) : 0;
 	return err;
@@ -1144,15 +1168,16 @@
 int ip6_dst_lookup(struct sock *sk, struct dst_entry **dst, struct flowi *fl)
 {
 	struct ipv6_pinfo *np = inet6_sk(sk);
+	struct rt6_info *rt;
 	int err = 0;
 
 	*dst = __sk_dst_check(sk, np->dst_cookie);
 	if (*dst) {
-		struct rt6_info *rt = (struct rt6_info*)*dst;
+		rt = (struct rt6_info*)*dst;
 
 			/* Yes, checking route validity in not connected
 			   case is not very simple. Take into account,
-			   that we do not support routing by source, TOS,
+			   that we do not support routing by TOS,
 			   and MSG_DONTROUTE 		--ANK (980726)
 
 			   1. If route was host route, check that
@@ -1172,6 +1197,13 @@
 		      ipv6_addr_cmp(&fl->fl6_dst, &rt->rt6i_dst.addr))
 		     && (np->daddr_cache == NULL ||
 			 ipv6_addr_cmp(&fl->fl6_dst, np->daddr_cache)))
+#ifdef CONFIG_IPV6_SUBTREES
+		    || (!ipv6_addr_any(&fl->fl6_src)
+			&& (rt->rt6i_src.plen != 128 ||
+			    ipv6_addr_cmp(&fl->fl6_src, &rt->rt6i_src.addr))
+			&& (np->saddr_cache == NULL ||
+			    ipv6_addr_cmp(&fl->fl6_src, np->saddr_cache)))
+#endif
 		    || (fl->oif && fl->oif != (*dst)->dev->ifindex)) {
 			*dst = NULL;
 		} else
@@ -1198,7 +1230,20 @@
 			return err;
 		}
 	}
-
+#ifdef CONFIG_IPV6_SUBTREES
+	rt = (struct rt6_info*)*dst;
+	if (ipv6_addr_cmp(&fl->fl6_src, &np->saddr) && 
+	    (rt->rt6i_src.plen != 128 || 
+	     ipv6_addr_cmp(&fl->fl6_src, &rt->rt6i_src.addr))) {
+		dst_release(*dst);
+		*dst = ip6_route_output(sk, fl);
+		if ((*dst)->error) {
+			IP6_INC_STATS(Ip6OutNoRoutes);
+			dst_release(*dst);
+			return -ENETUNREACH;
+		}
+	}
+#endif
         if (*dst) {
 		if ((err = xfrm_lookup(dst, fl, sk, 0)) < 0) {
 			dst_release(*dst);	
diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/net/ipv6/ip6_tunnel.c linux-2.5/net/ipv6/ip6_tunnel.c
--- linux-2.5.OLD/net/ipv6/ip6_tunnel.c	Fri Jul  4 14:48:21 2003
+++ linux-2.5/net/ipv6/ip6_tunnel.c	Fri Jul  4 23:32:34 2003
@@ -757,6 +757,10 @@
 	if (dst) {
 		if (np->daddr_cache == NULL ||
 		    ipv6_addr_cmp(&fl.fl6_dst, np->daddr_cache) ||
+#ifdef CONFIG_IPV6_SUBTREES
+		    np->saddr_cache == NULL ||
+		    ipv6_addr_cmp(&fl.fl6_src, np->saddr_cache) ||
+#endif
 		    (fl.oif && fl.oif != dst->dev->ifindex)) {
 			dst = NULL;
 		}
@@ -816,7 +820,7 @@
 		sock_kfree_s(sk, opt, opt->tot_len);
 
 	fl6_sock_release(fl_lbl);
-	ip6_dst_store(sk, dst, &np->daddr);
+	ip6_dst_store(sk, dst, &np->daddr, &np->saddr);
 	ip6_xmit_unlock();
 	kfree_skb(skb);
 	t->recursion--;
diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/net/ipv6/raw.c linux-2.5/net/ipv6/raw.c
--- linux-2.5.OLD/net/ipv6/raw.c	Wed Jul  2 15:42:03 2003
+++ linux-2.5/net/ipv6/raw.c	Fri Jul  4 23:32:34 2003
@@ -694,7 +694,9 @@
 done:
 	ip6_dst_store(sk, dst,
 		      !ipv6_addr_cmp(&fl.fl6_dst, &np->daddr) ?
-		      &np->daddr : NULL);
+		      &np->daddr : NULL,
+		      !ipv6_addr_cmp(&fl.fl6_src, &np->saddr) ?
+		      &np->saddr : NULL);
 	if (err > 0)
 		err = np->recverr ? net_xmit_errno(err) : 0;
 
diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/net/ipv6/route.c linux-2.5/net/ipv6/route.c
--- linux-2.5.OLD/net/ipv6/route.c	Mon Jun 30 20:36:03 2003
+++ linux-2.5/net/ipv6/route.c	Sat Jul  5 00:23:44 2003
@@ -363,12 +363,8 @@
 		rt->u.dst.flags |= DST_HOST;
 
 #ifdef CONFIG_IPV6_SUBTREES
-		if (rt->rt6i_src.plen && saddr) {
-			ipv6_addr_copy(&rt->rt6i_src.addr, saddr);
-			rt->rt6i_src.plen = 128;
-		}
+		rt->rt6i_src.plen = ort->rt6i_src.plen;
 #endif
-
 		rt->rt6i_nexthop = ndisc_get_neigh(rt->rt6i_dev, &rt->rt6i_gateway);
 
 		dst_hold(&rt->u.dst);
@@ -734,7 +730,7 @@
 			if (!(gwa_type&IPV6_ADDR_UNICAST))
 				goto out;
 
-			grt = rt6_lookup(gw_addr, NULL, rtmsg->rtmsg_ifindex, 1);
+			grt = rt6_lookup(gw_addr, &rtmsg->rtmsg_src, rtmsg->rtmsg_ifindex, 1);
 
 			err = -EHOSTUNREACH;
 			if (grt == NULL)
@@ -879,7 +875,7 @@
 	struct rt6_info *rt, *nrt;
 
 	/* Locate old route to this destination. */
-	rt = rt6_lookup(dest, NULL, neigh->dev->ifindex, 1);
+	rt = rt6_lookup(dest, saddr, neigh->dev->ifindex, 1);
 
 	if (rt == NULL)
 		return;
@@ -1044,6 +1040,9 @@
 		nrt = ip6_rt_copy(rt);
 		if (nrt == NULL)
 			goto out;
+#ifdef CONFIG_IPV6_SUBTREES
+		nrt->rt6i_src.plen = rt->rt6i_src.plen;
+#endif
 		ipv6_addr_copy(&nrt->rt6i_dst.addr, daddr);
 		nrt->rt6i_dst.plen = 128;
 		nrt->u.dst.flags |= DST_HOST;
diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/net/ipv6/tcp_ipv6.c linux-2.5/net/ipv6/tcp_ipv6.c
--- linux-2.5.OLD/net/ipv6/tcp_ipv6.c	Fri Jul  4 23:31:13 2003
+++ linux-2.5/net/ipv6/tcp_ipv6.c	Fri Jul  4 23:32:34 2003
@@ -676,6 +676,16 @@
 			dst_release(dst);
 			goto failure;
 		}
+#ifdef CONFIG_IPV6_SUBTREES
+		dst_release(dst);
+
+		dst = ip6_route_output(sk, &fl);
+
+		if ((err = dst->error) != 0) {
+			dst_release(dst);
+			goto failure;
+		}
+#endif
 		saddr = &fl.fl6_src;
 		ipv6_addr_copy(&np->rcv_saddr, saddr);
 	}
@@ -684,7 +694,7 @@
 	ipv6_addr_copy(&np->saddr, saddr);
 	inet->rcv_saddr = LOOPBACK4_IPV6;
 
-	ip6_dst_store(sk, dst, NULL);
+	ip6_dst_store(sk, dst, NULL, NULL);
 	sk->sk_route_caps = dst->dev->features &
 			    ~(NETIF_F_IP_CSUM | NETIF_F_TSO);
 
@@ -1345,7 +1355,7 @@
 	atomic_inc(&inet6_sock_nr);
 #endif
 
-	ip6_dst_store(newsk, dst, NULL);
+	ip6_dst_store(newsk, dst, NULL, NULL);
 	sk->sk_route_caps = dst->dev->features &
 			    ~(NETIF_F_IP_CSUM | NETIF_F_TSO);
 
@@ -1737,7 +1747,7 @@
 			return err;
 		}
 
-		ip6_dst_store(sk, dst, NULL);
+		ip6_dst_store(sk, dst, NULL, NULL);
 		sk->sk_route_caps = dst->dev->features &
 				    ~(NETIF_F_IP_CSUM | NETIF_F_TSO);
 	}
@@ -1779,7 +1789,7 @@
 			return -sk->sk_err_soft;
 		}
 
-		ip6_dst_store(sk, dst, NULL);
+		ip6_dst_store(sk, dst, NULL, NULL);
 	}
 
 	skb->dst = dst_clone(dst);
diff -Nur --exclude=RCS --exclude=CVS --exclude=SCCS --exclude=BitKeeper --exclude=ChangeSet linux-2.5.OLD/net/ipv6/udp.c linux-2.5/net/ipv6/udp.c
--- linux-2.5.OLD/net/ipv6/udp.c	Wed Jul  2 15:42:03 2003
+++ linux-2.5/net/ipv6/udp.c	Fri Jul  4 23:32:34 2003
@@ -345,7 +345,16 @@
 		dst_release(dst);
 		goto out;
 	}
+#ifdef CONFIG_IPV6_SUBTREES
+	dst_release(dst);
 
+	dst = ip6_route_output(sk, &fl);
+
+	if ((err = dst->error) != 0) {
+		dst_release(dst);
+		goto out;
+	}
+#endif
 	if (ipv6_addr_any(&np->saddr))
 		ipv6_addr_copy(&np->saddr, &fl.fl6_src);
 
@@ -356,7 +365,9 @@
 
 	ip6_dst_store(sk, dst,
 		      !ipv6_addr_cmp(&fl.fl6_dst, &np->daddr) ?
-		      &np->daddr : NULL);
+		      &np->daddr : NULL,
+		      !ipv6_addr_cmp(&fl.fl6_src, &np->saddr) ?
+		      &np->saddr : NULL);
 
 	sk->sk_state = TCP_ESTABLISHED;
 out:
@@ -970,7 +981,9 @@
 
 	ip6_dst_store(sk, dst,
 		      !ipv6_addr_cmp(&fl.fl6_dst, &np->daddr) ?
-		      &np->daddr : NULL);
+		      &np->daddr : NULL,
+		      !ipv6_addr_cmp(&fl.fl6_src, &np->saddr) ?
+		      &np->saddr : NULL);
 	if (err > 0)
 		err = np->recverr ? net_xmit_errno(err) : 0;
 	release_sock(sk);