4 files changed, 72 insertions, 44 deletions

diff --git a/net/ipv6/xfrm6_mode_beet.c b/net/ipv6/xfrm6_mode_beet.c
index 2bfb4f05c14c..4988ed9c76c6 100644
--- a/net/ipv6/xfrm6_mode_beet.c
+++ b/net/ipv6/xfrm6_mode_beet.c
@@ -25,25 +25,24 @@
  */
 static int xfrm6_beet_output(struct xfrm_state *x, struct sk_buff *skb)
 {
-        struct ipv6hdr *iph, *top_iph;
-        u8 *prevhdr;
-        int hdr_len;
+        struct ipv6hdr *top_iph;
 
-        iph = ipv6_hdr(skb);
-
-        hdr_len = ip6_find_1stfragopt(skb, &prevhdr);
-
-        skb_set_mac_header(skb, (prevhdr - x->props.header_len) - skb->data);
         skb_set_network_header(skb, -x->props.header_len);
-        skb->transport_header = skb->network_header + hdr_len;
-        __skb_pull(skb, hdr_len);
-
+        skb->mac_header = skb->network_header +
+                          offsetof(struct ipv6hdr, nexthdr);
+        skb->transport_header = skb->network_header + sizeof(*top_iph);
         top_iph = ipv6_hdr(skb);
-        memmove(top_iph, iph, hdr_len);
 
+        top_iph->version = 6;
+
+        memcpy(top_iph->flow_lbl, XFRM_MODE_SKB_CB(skb)->flow_lbl,
+               sizeof(top_iph->flow_lbl));
+        top_iph->nexthdr = XFRM_MODE_SKB_CB(skb)->protocol;
+
+        ipv6_change_dsfield(top_iph, 0, XFRM_MODE_SKB_CB(skb)->tos);
+        top_iph->hop_limit = XFRM_MODE_SKB_CB(skb)->ttl;
         ipv6_addr_copy(&top_iph->saddr, (struct in6_addr *)&x->props.saddr);
         ipv6_addr_copy(&top_iph->daddr, (struct in6_addr *)&x->id.daddr);
-
         return 0;
 }
 
@@ -76,7 +75,8 @@ out:
 
 static struct xfrm_mode xfrm6_beet_mode = {
         .input = xfrm6_beet_input,
-        .output = xfrm6_beet_output,
+        .output2 = xfrm6_beet_output,
+        .output = xfrm6_prepare_output,
         .owner = THIS_MODULE,
         .encap = XFRM_MODE_BEET,
         .flags = XFRM_MODE_FLAG_TUNNEL,
diff --git a/net/ipv6/xfrm6_mode_tunnel.c b/net/ipv6/xfrm6_mode_tunnel.c
index 9a43ea722481..d45ce5d44197 100644
--- a/net/ipv6/xfrm6_mode_tunnel.c
+++ b/net/ipv6/xfrm6_mode_tunnel.c
@@ -38,33 +38,22 @@ static inline void ip6ip_ecn_decapsulate(struct sk_buff *skb)
 static int xfrm6_tunnel_output(struct xfrm_state *x, struct sk_buff *skb)
 {
         struct dst_entry *dst = skb->dst;
-        struct xfrm_dst *xdst = (struct xfrm_dst*)dst;
-        struct ipv6hdr *iph, *top_iph;
+        struct ipv6hdr *top_iph;
         int dsfield;
 
-        iph = ipv6_hdr(skb);
-
         skb_set_network_header(skb, -x->props.header_len);
         skb->mac_header = skb->network_header +
                           offsetof(struct ipv6hdr, nexthdr);
-        skb->transport_header = skb->network_header + sizeof(*iph);
+        skb->transport_header = skb->network_header + sizeof(*top_iph);
         top_iph = ipv6_hdr(skb);
 
         top_iph->version = 6;
-        if (xdst->route->ops->family == AF_INET6) {
-                top_iph->priority = iph->priority;
-                top_iph->flow_lbl[0] = iph->flow_lbl[0];
-                top_iph->flow_lbl[1] = iph->flow_lbl[1];
-                top_iph->flow_lbl[2] = iph->flow_lbl[2];
-                top_iph->nexthdr = IPPROTO_IPV6;
-        } else {
-                top_iph->priority = 0;
-                top_iph->flow_lbl[0] = 0;
-                top_iph->flow_lbl[1] = 0;
-                top_iph->flow_lbl[2] = 0;
-                top_iph->nexthdr = IPPROTO_IPIP;
-        }
-        dsfield = ipv6_get_dsfield(top_iph);
+
+        memcpy(top_iph->flow_lbl, XFRM_MODE_SKB_CB(skb)->flow_lbl,
+               sizeof(top_iph->flow_lbl));
+        top_iph->nexthdr = x->inner_mode->afinfo->proto;
+
+        dsfield = XFRM_MODE_SKB_CB(skb)->tos;
         dsfield = INET_ECN_encapsulate(dsfield, dsfield);
         if (x->props.flags & XFRM_STATE_NOECN)
                 dsfield &= ~INET_ECN_MASK;
@@ -72,7 +61,6 @@ static int xfrm6_tunnel_output(struct xfrm_state *x, struct sk_buff *skb)
         top_iph->hop_limit = dst_metric(dst->child, RTAX_HOPLIMIT);
         ipv6_addr_copy(&top_iph->saddr, (struct in6_addr *)&x->props.saddr);
         ipv6_addr_copy(&top_iph->daddr, (struct in6_addr *)&x->id.daddr);
-        skb->protocol = htons(ETH_P_IPV6);
         return 0;
 }
 
@@ -116,7 +104,8 @@ out:
 
 static struct xfrm_mode xfrm6_tunnel_mode = {
         .input = xfrm6_tunnel_input,
-        .output = xfrm6_tunnel_output,
+        .output2 = xfrm6_tunnel_output,
+        .output = xfrm6_prepare_output,
         .owner = THIS_MODULE,
         .encap = XFRM_MODE_TUNNEL,
         .flags = XFRM_MODE_FLAG_TUNNEL,
diff --git a/net/ipv6/xfrm6_output.c b/net/ipv6/xfrm6_output.c
index 656976760ad4..bc2e80e3b0b1 100644
--- a/net/ipv6/xfrm6_output.c
+++ b/net/ipv6/xfrm6_output.c
@@ -10,10 +10,12 @@
  */
 
 #include <linux/if_ether.h>
-#include <linux/compiler.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/skbuff.h>
 #include <linux/icmpv6.h>
 #include <linux/netfilter_ipv6.h>
+#include <net/dst.h>
 #include <net/ipv6.h>
 #include <net/xfrm.h>
 
@@ -43,19 +45,38 @@ static int xfrm6_tunnel_check_size(struct sk_buff *skb)
         return ret;
 }
 
+int xfrm6_extract_output(struct xfrm_state *x, struct sk_buff *skb)
+{
+        int err;
+
+        err = xfrm6_tunnel_check_size(skb);
+        if (err)
+                return err;
+
+        return xfrm6_extract_header(skb);
+}
+
+int xfrm6_prepare_output(struct xfrm_state *x, struct sk_buff *skb)
+{
+        int err;
+
+        err = x->inner_mode->afinfo->extract_output(x, skb);
+        if (err)
+                return err;
+
+        memset(IP6CB(skb), 0, sizeof(*IP6CB(skb)));
+
+        skb->protocol = htons(ETH_P_IPV6);
+
+        return x->outer_mode->output2(x, skb);
+}
+EXPORT_SYMBOL(xfrm6_prepare_output);
+
 static inline int xfrm6_output_one(struct sk_buff *skb)
 {
-        struct dst_entry *dst = skb->dst;
-        struct xfrm_state *x = dst->xfrm;
         struct ipv6hdr *iph;
         int err;
 
-        if (x->outer_mode->flags & XFRM_MODE_FLAG_TUNNEL) {
-                err = xfrm6_tunnel_check_size(skb);
-                if (err)
-                        goto error_nolock;
-        }
-
         err = xfrm_output(skb);
         if (err)
                 goto error_nolock;
diff --git a/net/ipv6/xfrm6_state.c b/net/ipv6/xfrm6_state.c
index b392bee396f1..98b05f472322 100644
--- a/net/ipv6/xfrm6_state.c
+++ b/net/ipv6/xfrm6_state.c
@@ -14,6 +14,7 @@
 #include <net/xfrm.h>
 #include <linux/pfkeyv2.h>
 #include <linux/ipsec.h>
+#include <net/dsfield.h>
 #include <net/ipv6.h>
 #include <net/addrconf.h>
 
@@ -168,13 +169,30 @@ __xfrm6_tmpl_sort(struct xfrm_tmpl **dst, struct xfrm_tmpl **src, int n)
         return 0;
 }
 
+int xfrm6_extract_header(struct sk_buff *skb)
+{
+        struct ipv6hdr *iph = ipv6_hdr(skb);
+
+        XFRM_MODE_SKB_CB(skb)->id = 0;
+        XFRM_MODE_SKB_CB(skb)->frag_off = htons(IP_DF);
+        XFRM_MODE_SKB_CB(skb)->tos = ipv6_get_dsfield(iph);
+        XFRM_MODE_SKB_CB(skb)->ttl = iph->hop_limit;
+        XFRM_MODE_SKB_CB(skb)->protocol = iph->nexthdr;
+        memcpy(XFRM_MODE_SKB_CB(skb)->flow_lbl, iph->flow_lbl,
+               sizeof(XFRM_MODE_SKB_CB(skb)->flow_lbl));
+
+        return 0;
+}
+
 static struct xfrm_state_afinfo xfrm6_state_afinfo = {
         .family                 = AF_INET6,
+        .proto                  = IPPROTO_IPV6,
         .owner                  = THIS_MODULE,
         .init_tempsel           = __xfrm6_init_tempsel,
         .tmpl_sort              = __xfrm6_tmpl_sort,
         .state_sort             = __xfrm6_state_sort,
         .output                 = xfrm6_output,
+        .extract_output         = xfrm6_extract_output,
 };
 
 void __init xfrm6_state_init(void)