3 files changed, 72 insertions, 2 deletions

diff --git a/include/net/xfrm.h b/include/net/xfrm.h
index 4655ca25f808..d341603e4ba8 100644
--- a/include/net/xfrm.h
+++ b/include/net/xfrm.h
@@ -254,6 +254,8 @@ struct xfrm_state_afinfo {
         struct xfrm_state       *(*find_acq)(u8 mode, u32 reqid, u8 proto, 
                                              xfrm_address_t *daddr, xfrm_address_t *saddr, 
                                              int create);
+        int                     (*tmpl_sort)(struct xfrm_tmpl **dst, struct xfrm_tmpl **src, int n);
+        int                     (*state_sort)(struct xfrm_state **dst, struct xfrm_state **src, int n);
 };
 
 extern int xfrm_state_register_afinfo(struct xfrm_state_afinfo *afinfo);
@@ -1002,6 +1004,24 @@ extern int xfrm_state_add(struct xfrm_state *x);
 extern int xfrm_state_update(struct xfrm_state *x);
 extern struct xfrm_state *xfrm_state_lookup(xfrm_address_t *daddr, u32 spi, u8 proto, unsigned short family);
 extern struct xfrm_state *xfrm_state_lookup_byaddr(xfrm_address_t *daddr, xfrm_address_t *saddr, u8 proto, unsigned short family);
+#ifdef CONFIG_XFRM_SUB_POLICY
+extern int xfrm_tmpl_sort(struct xfrm_tmpl **dst, struct xfrm_tmpl **src,
+                          int n, unsigned short family);
+extern int xfrm_state_sort(struct xfrm_state **dst, struct xfrm_state **src,
+                           int n, unsigned short family);
+#else
+static inline int xfrm_tmpl_sort(struct xfrm_tmpl **dst, struct xfrm_tmpl **src,
+                                 int n, unsigned short family)
+{
+        return -ENOSYS;
+}
+
+static inline int xfrm_state_sort(struct xfrm_state **dst, struct xfrm_state **src,
+                                  int n, unsigned short family)
+{
+        return -ENOSYS;
+}
+#endif
 extern struct xfrm_state *xfrm_find_acq_byseq(u32 seq);
 extern int xfrm_state_delete(struct xfrm_state *x);
 extern void xfrm_state_flush(u8 proto);
diff --git a/net/xfrm/xfrm_policy.c b/net/xfrm/xfrm_policy.c
index 96de6c76ed57..1732159ffd01 100644
--- a/net/xfrm/xfrm_policy.c
+++ b/net/xfrm/xfrm_policy.c
@@ -861,6 +861,8 @@ xfrm_tmpl_resolve(struct xfrm_policy **pols, int npols, struct flowi *fl,
                   struct xfrm_state **xfrm,
                   unsigned short family)
 {
+        struct xfrm_state *tp[XFRM_MAX_DEPTH];
+        struct xfrm_state **tpp = (npols > 1) ? tp : xfrm;
         int cnx = 0;
         int error;
         int ret;
@@ -871,7 +873,8 @@ xfrm_tmpl_resolve(struct xfrm_policy **pols, int npols, struct flowi *fl,
                         error = -ENOBUFS;
                         goto fail;
                 }
-                ret = xfrm_tmpl_resolve_one(pols[i], fl, &xfrm[cnx], family);
+
+                ret = xfrm_tmpl_resolve_one(pols[i], fl, &tpp[cnx], family);
                 if (ret < 0) {
                         error = ret;
                         goto fail;
@@ -879,11 +882,15 @@ xfrm_tmpl_resolve(struct xfrm_policy **pols, int npols, struct flowi *fl,
                         cnx += ret;
         }
 
+        /* found states are sorted for outbound processing */
+        if (npols > 1)
+                xfrm_state_sort(xfrm, tpp, cnx, family);
+
         return cnx;
 
  fail:
         for (cnx--; cnx>=0; cnx--)
-                xfrm_state_put(xfrm[cnx]);
+                xfrm_state_put(tpp[cnx]);
         return error;
 
 }
@@ -1280,6 +1287,7 @@ int __xfrm_policy_check(struct sock *sk, int dir, struct sk_buff *skb,
                 struct sec_path *sp;
                 static struct sec_path dummy;
                 struct xfrm_tmpl *tp[XFRM_MAX_DEPTH];
+                struct xfrm_tmpl *stp[XFRM_MAX_DEPTH];
                 struct xfrm_tmpl **tpp = tp;
                 int ti = 0;
                 int i, k;
@@ -1297,6 +1305,10 @@ int __xfrm_policy_check(struct sock *sk, int dir, struct sk_buff *skb,
                                 tpp[ti++] = &pols[pi]->xfrm_vec[i];
                 }
                 xfrm_nr = ti;
+                if (npols > 1) {
+                        xfrm_tmpl_sort(stp, tpp, xfrm_nr, family);
+                        tpp = stp;
+                }
 
                 /* For each tunnel xfrm, find the first matching tmpl.
                  * For each tmpl before that, find corresponding xfrm.
diff --git a/net/xfrm/xfrm_state.c b/net/xfrm/xfrm_state.c
index a26ef6952c30..622e92a08d0b 100644
--- a/net/xfrm/xfrm_state.c
+++ b/net/xfrm/xfrm_state.c
@@ -728,6 +728,44 @@ xfrm_find_acq(u8 mode, u32 reqid, u8 proto,
 }
 EXPORT_SYMBOL(xfrm_find_acq);
 
+#ifdef CONFIG_XFRM_SUB_POLICY
+int
+xfrm_tmpl_sort(struct xfrm_tmpl **dst, struct xfrm_tmpl **src, int n,
+               unsigned short family)
+{
+        int err = 0;
+        struct xfrm_state_afinfo *afinfo = xfrm_state_get_afinfo(family);
+        if (!afinfo)
+                return -EAFNOSUPPORT;
+
+        spin_lock_bh(&xfrm_state_lock);
+        if (afinfo->tmpl_sort)
+                err = afinfo->tmpl_sort(dst, src, n);
+        spin_unlock_bh(&xfrm_state_lock);
+        xfrm_state_put_afinfo(afinfo);
+        return err;
+}
+EXPORT_SYMBOL(xfrm_tmpl_sort);
+
+int
+xfrm_state_sort(struct xfrm_state **dst, struct xfrm_state **src, int n,
+                unsigned short family)
+{
+        int err = 0;
+        struct xfrm_state_afinfo *afinfo = xfrm_state_get_afinfo(family);
+        if (!afinfo)
+                return -EAFNOSUPPORT;
+
+        spin_lock_bh(&xfrm_state_lock);
+        if (afinfo->state_sort)
+                err = afinfo->state_sort(dst, src, n);
+        spin_unlock_bh(&xfrm_state_lock);
+        xfrm_state_put_afinfo(afinfo);
+        return err;
+}
+EXPORT_SYMBOL(xfrm_state_sort);
+#endif
+
 /* Silly enough, but I'm lazy to build resolution list */
 
 static struct xfrm_state *__xfrm_find_acq_byseq(u32 seq)