2 files changed, 147 insertions, 1 deletions

diff --git a/net/xfrm/xfrm_policy.c b/net/xfrm/xfrm_policy.c
index 762926009c04..263e34e45265 100644
--- a/net/xfrm/xfrm_policy.c
+++ b/net/xfrm/xfrm_policy.c
@@ -579,8 +579,22 @@ static inline int xfrm_byidx_should_resize(int total)
         return 0;
 }
 
-static DEFINE_MUTEX(hash_resize_mutex);
+void xfrm_spd_getinfo(struct xfrm_spdinfo *si)
+{
+        read_lock_bh(&xfrm_policy_lock);
+        si->incnt = xfrm_policy_count[XFRM_POLICY_IN];
+        si->outcnt = xfrm_policy_count[XFRM_POLICY_OUT];
+        si->fwdcnt = xfrm_policy_count[XFRM_POLICY_FWD];
+        si->inscnt = xfrm_policy_count[XFRM_POLICY_IN+XFRM_POLICY_MAX];
+        si->outscnt = xfrm_policy_count[XFRM_POLICY_OUT+XFRM_POLICY_MAX];
+        si->fwdscnt = xfrm_policy_count[XFRM_POLICY_FWD+XFRM_POLICY_MAX];
+        si->spdhcnt = xfrm_idx_hmask;
+        si->spdhmcnt = xfrm_policy_hashmax;
+        read_unlock_bh(&xfrm_policy_lock);
+}
+EXPORT_SYMBOL(xfrm_spd_getinfo);
 
+static DEFINE_MUTEX(hash_resize_mutex);
 static void xfrm_hash_resize(struct work_struct *__unused)
 {
         int dir, total;
@@ -1330,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);
 
@@ -1518,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);
@@ -1933,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;
 
diff --git a/net/xfrm/xfrm_user.c b/net/xfrm/xfrm_user.c
index 69110fed64b6..4210d91624cd 100644
--- a/net/xfrm/xfrm_user.c
+++ b/net/xfrm/xfrm_user.c
@@ -672,6 +672,81 @@ static struct sk_buff *xfrm_state_netlink(struct sk_buff *in_skb,
         return skb;
 }
 
+static int build_spdinfo(struct sk_buff *skb, u32 pid, u32 seq, u32 flags)
+{
+        struct xfrm_spdinfo si;
+        struct nlmsghdr *nlh;
+        u32 *f;
+
+        nlh = nlmsg_put(skb, pid, seq, XFRM_MSG_NEWSPDINFO, sizeof(u32), 0);
+        if (nlh == NULL) /* shouldnt really happen ... */
+                return -EMSGSIZE;
+
+        f = nlmsg_data(nlh);
+        *f = flags;
+        xfrm_spd_getinfo(&si);
+
+        if (flags & XFRM_SPD_HMASK)
+                NLA_PUT_U32(skb, XFRMA_SPDHMASK, si.spdhcnt);
+        if (flags & XFRM_SPD_HMAX)
+                NLA_PUT_U32(skb, XFRMA_SPDHMAX, si.spdhmcnt);
+        if (flags & XFRM_SPD_ICNT)
+                NLA_PUT_U32(skb, XFRMA_SPDICNT, si.incnt);
+        if (flags & XFRM_SPD_OCNT)
+                NLA_PUT_U32(skb, XFRMA_SPDOCNT, si.outcnt);
+        if (flags & XFRM_SPD_FCNT)
+                NLA_PUT_U32(skb, XFRMA_SPDFCNT, si.fwdcnt);
+        if (flags & XFRM_SPD_ISCNT)
+                NLA_PUT_U32(skb, XFRMA_SPDISCNT, si.inscnt);
+        if (flags & XFRM_SPD_OSCNT)
+                NLA_PUT_U32(skb, XFRMA_SPDOSCNT, si.inscnt);
+        if (flags & XFRM_SPD_FSCNT)
+                NLA_PUT_U32(skb, XFRMA_SPDFSCNT, si.inscnt);
+
+        return nlmsg_end(skb, nlh);
+
+nla_put_failure:
+        nlmsg_cancel(skb, nlh);
+        return -EMSGSIZE;
+}
+
+static int xfrm_get_spdinfo(struct sk_buff *skb, struct nlmsghdr *nlh,
+                struct rtattr **xfrma)
+{
+        struct sk_buff *r_skb;
+        u32 *flags = NLMSG_DATA(nlh);
+        u32 spid = NETLINK_CB(skb).pid;
+        u32 seq = nlh->nlmsg_seq;
+        int len = NLMSG_LENGTH(sizeof(u32));
+
+
+        if (*flags & XFRM_SPD_HMASK)
+                len += RTA_SPACE(sizeof(u32));
+        if (*flags & XFRM_SPD_HMAX)
+                len += RTA_SPACE(sizeof(u32));
+        if (*flags & XFRM_SPD_ICNT)
+                len += RTA_SPACE(sizeof(u32));
+        if (*flags & XFRM_SPD_OCNT)
+                len += RTA_SPACE(sizeof(u32));
+        if (*flags & XFRM_SPD_FCNT)
+                len += RTA_SPACE(sizeof(u32));
+        if (*flags & XFRM_SPD_ISCNT)
+                len += RTA_SPACE(sizeof(u32));
+        if (*flags & XFRM_SPD_OSCNT)
+                len += RTA_SPACE(sizeof(u32));
+        if (*flags & XFRM_SPD_FSCNT)
+                len += RTA_SPACE(sizeof(u32));
+
+        r_skb = alloc_skb(len, GFP_ATOMIC);
+        if (r_skb == NULL)
+                return -ENOMEM;
+
+        if (build_spdinfo(r_skb, spid, seq, *flags) < 0)
+                BUG();
+
+        return nlmsg_unicast(xfrm_nl, r_skb, spid);
+}
+
 static int build_sadinfo(struct sk_buff *skb, u32 pid, u32 seq, u32 flags)
 {
         struct xfrm_sadinfo si;
@@ -1879,6 +1954,7 @@ static const int xfrm_msg_min[XFRM_NR_MSGTYPES] = {
         [XFRM_MSG_REPORT      - XFRM_MSG_BASE] = XMSGSIZE(xfrm_user_report),
         [XFRM_MSG_MIGRATE     - XFRM_MSG_BASE] = XMSGSIZE(xfrm_userpolicy_id),
         [XFRM_MSG_GETSADINFO  - XFRM_MSG_BASE] = NLMSG_LENGTH(sizeof(u32)),
+        [XFRM_MSG_GETSPDINFO  - XFRM_MSG_BASE] = NLMSG_LENGTH(sizeof(u32)),
 };
 
 #undef XMSGSIZE
@@ -1907,6 +1983,7 @@ static struct xfrm_link {
         [XFRM_MSG_GETAE       - XFRM_MSG_BASE] = { .doit = xfrm_get_ae  },
         [XFRM_MSG_MIGRATE     - XFRM_MSG_BASE] = { .doit = xfrm_do_migrate    },
         [XFRM_MSG_GETSADINFO  - XFRM_MSG_BASE] = { .doit = xfrm_get_sadinfo   },
+        [XFRM_MSG_GETSPDINFO  - XFRM_MSG_BASE] = { .doit = xfrm_get_spdinfo   },
 };
 
 static int xfrm_user_rcv_msg(struct sk_buff *skb, struct nlmsghdr *nlh)