2 files changed, 57 insertions, 9 deletions

diff --git a/include/net/xfrm.h b/include/net/xfrm.h
index c435620dbb37..bed7d43932f6 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 bae94a8031a2..8e588f20c60c 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;