2 files changed, 57 insertions, 9 deletions

diff --git a/include/net/xfrm.h b/include/net/xfrm.h
index c435620dbb3..bed7d43932f 100644
--- a/include/net/xfrm.h
+++ b/include/net/xfrm.h
@@ -1045,6 +1045,23 @@ xfrm_address_t *xfrm_flowi_saddr(struct flowi *fl, unsigned short family)
 	return NULL;
 }
 
+static __inline__
+void xfrm_flowi_addr_get(struct flowi *fl,
+			 xfrm_address_t *saddr, xfrm_address_t *daddr,
+			 unsigned short family)
+{
+	switch(family) {
+	case AF_INET:
+		memcpy(&saddr->a4, &fl->fl4_src, sizeof(saddr->a4));
+		memcpy(&daddr->a4, &fl->fl4_dst, sizeof(daddr->a4));
+		break;
+	case AF_INET6:
+		ipv6_addr_copy((struct in6_addr *)&saddr->a6, &fl->fl6_src);
+		ipv6_addr_copy((struct in6_addr *)&daddr->a6, &fl->fl6_dst);
+		break;
+	}
+}
+
 static __inline__ int
 __xfrm4_state_addr_check(struct xfrm_state *x,
 			 xfrm_address_t *daddr, xfrm_address_t *saddr)
diff --git a/net/xfrm/xfrm_policy.c b/net/xfrm/xfrm_policy.c
index bae94a8031a..8e588f20c60 100644
--- a/net/xfrm/xfrm_policy.c
+++ b/net/xfrm/xfrm_policy.c
@@ -97,25 +97,52 @@ int xfrm_selector_match(struct xfrm_selector *sel, struct flowi *fl,
 	return 0;
 }
 
+static inline struct dst_entry *__xfrm_dst_lookup(int tos,
+						  xfrm_address_t *saddr,
+						  xfrm_address_t *daddr,
+						  int family)
+{
+	struct xfrm_policy_afinfo *afinfo;
+	struct dst_entry *dst;
+
+	afinfo = xfrm_policy_get_afinfo(family);
+	if (unlikely(afinfo == NULL))
+		return ERR_PTR(-EAFNOSUPPORT);
+
+	dst = afinfo->dst_lookup(tos, saddr, daddr);
+
+	xfrm_policy_put_afinfo(afinfo);
+
+	return dst;
+}
+
 static inline struct dst_entry *xfrm_dst_lookup(struct xfrm_state *x, int tos,
+						xfrm_address_t *prev_saddr,
+						xfrm_address_t *prev_daddr,
 						int family)
 {
 	xfrm_address_t *saddr = &x->props.saddr;
 	xfrm_address_t *daddr = &x->id.daddr;
-	struct xfrm_policy_afinfo *afinfo;
 	struct dst_entry *dst;
 
-	if (x->type->flags & XFRM_TYPE_LOCAL_COADDR)
+	if (x->type->flags & XFRM_TYPE_LOCAL_COADDR) {
 		saddr = x->coaddr;
-	if (x->type->flags & XFRM_TYPE_REMOTE_COADDR)
+		daddr = prev_daddr;
+	}
+	if (x->type->flags & XFRM_TYPE_REMOTE_COADDR) {
+		saddr = prev_saddr;
 		daddr = x->coaddr;
+	}
 
-	afinfo = xfrm_policy_get_afinfo(family);
-	if (unlikely(afinfo == NULL))
-		return ERR_PTR(-EAFNOSUPPORT);
+	dst = __xfrm_dst_lookup(tos, saddr, daddr, family);
+
+	if (!IS_ERR(dst)) {
+		if (prev_saddr != saddr)
+			memcpy(prev_saddr, saddr,  sizeof(*prev_saddr));
+		if (prev_daddr != daddr)
+			memcpy(prev_daddr, daddr,  sizeof(*prev_daddr));
+	}
 
-	dst = afinfo->dst_lookup(tos, saddr, daddr);
-	xfrm_policy_put_afinfo(afinfo);
 	return dst;
 }
 
@@ -1354,6 +1381,9 @@ static struct dst_entry *xfrm_bundle_create(struct xfrm_policy *policy,
 	int trailer_len = 0;
 	int tos;
 	int family = policy->selector.family;
+	xfrm_address_t saddr, daddr;
+
+	xfrm_flowi_addr_get(fl, &saddr, &daddr, family);
 
 	tos = xfrm_get_tos(fl, family);
 	err = tos;
@@ -1384,7 +1414,8 @@ static struct dst_entry *xfrm_bundle_create(struct xfrm_policy *policy,
 
 		if (xfrm[i]->props.mode != XFRM_MODE_TRANSPORT) {
 			family = xfrm[i]->props.family;
-			dst = xfrm_dst_lookup(xfrm[i], tos, family);
+			dst = xfrm_dst_lookup(xfrm[i], tos, &saddr, &daddr,
+					      family);
 			err = PTR_ERR(dst);
 			if (IS_ERR(dst))
 				goto put_states;