3 files changed, 71 insertions, 0 deletions

diff --git a/include/net/flow.h b/include/net/flow.h
index ce4b10d8b412..f3cc1f812619 100644
--- a/include/net/flow.h
+++ b/include/net/flow.h
@@ -97,4 +97,10 @@ extern void *flow_cache_lookup(struct flowi *key, u16 family, u8 dir,
 extern void flow_cache_flush(void);
 extern atomic_t flow_cache_genid;
 
+static inline int flow_cache_uli_match(struct flowi *fl1, struct flowi *fl2)
+{
+        return (fl1->proto == fl2->proto &&
+                !memcmp(&fl1->uli_u, &fl2->uli_u, sizeof(fl1->uli_u)));
+}
+
 #endif
diff --git a/include/net/xfrm.h b/include/net/xfrm.h
index 9561bf817b02..66c2d3eec03c 100644
--- a/include/net/xfrm.h
+++ b/include/net/xfrm.h
@@ -603,6 +603,10 @@ struct xfrm_dst
                 struct rt6_info         rt6;
         } u;
         struct dst_entry *route;
+#ifdef CONFIG_XFRM_SUB_POLICY
+        struct flowi *origin;
+        struct xfrm_selector *partner;
+#endif
         u32 genid;
         u32 route_mtu_cached;
         u32 child_mtu_cached;
@@ -615,6 +619,12 @@ static inline void xfrm_dst_destroy(struct xfrm_dst *xdst)
         dst_release(xdst->route);
         if (likely(xdst->u.dst.xfrm))
                 xfrm_state_put(xdst->u.dst.xfrm);
+#ifdef CONFIG_XFRM_SUB_POLICY
+        kfree(xdst->origin);
+        xdst->origin = NULL;
+        kfree(xdst->partner);
+        xdst->partner = NULL;
+#endif
 }
 
 extern void xfrm_dst_ifdown(struct dst_entry *dst, struct net_device *dev);
diff --git a/net/xfrm/xfrm_policy.c b/net/xfrm/xfrm_policy.c
index dbf9d96a2f0b..263e34e45265 100644
--- a/net/xfrm/xfrm_policy.c
+++ b/net/xfrm/xfrm_policy.c
@@ -1344,6 +1344,40 @@ xfrm_bundle_create(struct xfrm_policy *policy, struct xfrm_state **xfrm, int nx,
         return err;
 }
 
+static int inline
+xfrm_dst_alloc_copy(void **target, void *src, int size)
+{
+        if (!*target) {
+                *target = kmalloc(size, GFP_ATOMIC);
+                if (!*target)
+                        return -ENOMEM;
+        }
+        memcpy(*target, src, size);
+        return 0;
+}
+
+static int inline
+xfrm_dst_update_parent(struct dst_entry *dst, struct xfrm_selector *sel)
+{
+#ifdef CONFIG_XFRM_SUB_POLICY
+        struct xfrm_dst *xdst = (struct xfrm_dst *)dst;
+        return xfrm_dst_alloc_copy((void **)&(xdst->partner),
+                                   sel, sizeof(*sel));
+#else
+        return 0;
+#endif
+}
+
+static int inline
+xfrm_dst_update_origin(struct dst_entry *dst, struct flowi *fl)
+{
+#ifdef CONFIG_XFRM_SUB_POLICY
+        struct xfrm_dst *xdst = (struct xfrm_dst *)dst;
+        return xfrm_dst_alloc_copy((void **)&(xdst->origin), fl, sizeof(*fl));
+#else
+        return 0;
+#endif
+}
 
 static int stale_bundle(struct dst_entry *dst);
 
@@ -1532,6 +1566,18 @@ restart:
                         err = -EHOSTUNREACH;
                         goto error;
                 }
+
+                if (npols > 1)
+                        err = xfrm_dst_update_parent(dst, &pols[1]->selector);
+                else
+                        err = xfrm_dst_update_origin(dst, fl);
+                if (unlikely(err)) {
+                        write_unlock_bh(&policy->lock);
+                        if (dst)
+                                dst_free(dst);
+                        goto error;
+                }
+
                 dst->next = policy->bundles;
                 policy->bundles = dst;
                 dst_hold(dst);
@@ -1947,6 +1993,15 @@ int xfrm_bundle_ok(struct xfrm_policy *pol, struct xfrm_dst *first,
         if (!dst_check(dst->path, ((struct xfrm_dst *)dst)->path_cookie) ||
             (dst->dev && !netif_running(dst->dev)))
                 return 0;
+#ifdef CONFIG_XFRM_SUB_POLICY
+        if (fl) {
+                if (first->origin && !flow_cache_uli_match(first->origin, fl))
+                        return 0;
+                if (first->partner &&
+                    !xfrm_selector_match(first->partner, fl, family))
+                        return 0;
+        }
+#endif
 
         last = NULL;