3 files changed, 54 insertions, 18 deletions

diff --git a/include/net/dst.h b/include/net/dst.h
index bed833d9796a..8197eadca819 100644
--- a/include/net/dst.h
+++ b/include/net/dst.h
@@ -60,6 +60,7 @@ struct dst_entry {
 #define DST_NOCOUNT             0x0020
 #define DST_NOPEER              0x0040
 #define DST_FAKE_RTABLE         0x0080
+#define DST_XFRM_TUNNEL         0x0100
 
         short                   error;
         short                   obsolete;
diff --git a/net/ipv6/ip6_output.c b/net/ipv6/ip6_output.c
index d99fdc699625..17b8c67998bb 100644
--- a/net/ipv6/ip6_output.c
+++ b/net/ipv6/ip6_output.c
@@ -1187,6 +1187,29 @@ static inline struct ipv6_rt_hdr *ip6_rthdr_dup(struct ipv6_rt_hdr *src,
         return src ? kmemdup(src, (src->hdrlen + 1) * 8, gfp) : NULL;
 }
 
+static void ip6_append_data_mtu(int *mtu,
+                                int *maxfraglen,
+                                unsigned int fragheaderlen,
+                                struct sk_buff *skb,
+                                struct rt6_info *rt)
+{
+        if (!(rt->dst.flags & DST_XFRM_TUNNEL)) {
+                if (skb == NULL) {
+                        /* first fragment, reserve header_len */
+                        *mtu = *mtu - rt->dst.header_len;
+
+                } else {
+                        /*
+                         * this fragment is not first, the headers
+                         * space is regarded as data space.
+                         */
+                        *mtu = dst_mtu(rt->dst.path);
+                }
+                *maxfraglen = ((*mtu - fragheaderlen) & ~7)
+                              + fragheaderlen - sizeof(struct frag_hdr);
+        }
+}
+
 int ip6_append_data(struct sock *sk, int getfrag(void *from, char *to,
         int offset, int len, int odd, struct sk_buff *skb),
         void *from, int length, int transhdrlen,
@@ -1196,7 +1219,7 @@ int ip6_append_data(struct sock *sk, int getfrag(void *from, char *to,
         struct inet_sock *inet = inet_sk(sk);
         struct ipv6_pinfo *np = inet6_sk(sk);
         struct inet_cork *cork;
-        struct sk_buff *skb;
+        struct sk_buff *skb, *skb_prev = NULL;
         unsigned int maxfraglen, fragheaderlen;
         int exthdrlen;
         int dst_exthdrlen;
@@ -1253,8 +1276,12 @@ int ip6_append_data(struct sock *sk, int getfrag(void *from, char *to,
                 inet->cork.fl.u.ip6 = *fl6;
                 np->cork.hop_limit = hlimit;
                 np->cork.tclass = tclass;
-                mtu = np->pmtudisc == IPV6_PMTUDISC_PROBE ?
-                      rt->dst.dev->mtu : dst_mtu(&rt->dst);
+                if (rt->dst.flags & DST_XFRM_TUNNEL)
+                        mtu = np->pmtudisc == IPV6_PMTUDISC_PROBE ?
+                              rt->dst.dev->mtu : dst_mtu(&rt->dst);
+                else
+                        mtu = np->pmtudisc == IPV6_PMTUDISC_PROBE ?
+                              rt->dst.dev->mtu : dst_mtu(rt->dst.path);
                 if (np->frag_size < mtu) {
                         if (np->frag_size)
                                 mtu = np->frag_size;
@@ -1350,25 +1377,27 @@ int ip6_append_data(struct sock *sk, int getfrag(void *from, char *to,
                         unsigned int fraglen;
                         unsigned int fraggap;
                         unsigned int alloclen;
-                        struct sk_buff *skb_prev;
 alloc_new_skb:
-                        skb_prev = skb;
-
                         /* There's no room in the current skb */
-                        if (skb_prev)
-                                fraggap = skb_prev->len - maxfraglen;
+                        if (skb)
+                                fraggap = skb->len - maxfraglen;
                         else
                                 fraggap = 0;
+                        /* update mtu and maxfraglen if necessary */
+                        if (skb == NULL || skb_prev == NULL)
+                                ip6_append_data_mtu(&mtu, &maxfraglen,
+                                                    fragheaderlen, skb, rt);
+
+                        skb_prev = skb;
 
                         /*
                          * If remaining data exceeds the mtu,
                          * we know we need more fragment(s).
                          */
                         datalen = length + fraggap;
-                        if (datalen > (cork->length <= mtu && !(cork->flags & IPCORK_ALLFRAG) ? mtu : maxfraglen) - fragheaderlen)
-                                datalen = maxfraglen - fragheaderlen;
 
-                        fraglen = datalen + fragheaderlen;
+                        if (datalen > (cork->length <= mtu && !(cork->flags & IPCORK_ALLFRAG) ? mtu : maxfraglen) - fragheaderlen)
+                                datalen = maxfraglen - fragheaderlen - rt->dst.trailer_len;
                         if ((flags & MSG_MORE) &&
                             !(rt->dst.dev->features&NETIF_F_SG))
                                 alloclen = mtu;
@@ -1377,13 +1406,16 @@ alloc_new_skb:
 
                         alloclen += dst_exthdrlen;
 
-                        /*
-                         * The last fragment gets additional space at tail.
-                         * Note: we overallocate on fragments with MSG_MODE
-                         * because we have no idea if we're the last one.
-                         */
-                        if (datalen == length + fraggap)
-                                alloclen += rt->dst.trailer_len;
+                        if (datalen != length + fraggap) {
+                                /*
+                                 * this is not the last fragment, the trailer
+                                 * space is regarded as data space.
+                                 */
+                                datalen += rt->dst.trailer_len;
+                        }
+
+                        alloclen += rt->dst.trailer_len;
+                        fraglen = datalen + fragheaderlen;
 
                         /*
                          * We just reserve space for fragment header.
diff --git a/net/xfrm/xfrm_policy.c b/net/xfrm/xfrm_policy.c
index c53e8f42aa75..ccfbd328a69d 100644
--- a/net/xfrm/xfrm_policy.c
+++ b/net/xfrm/xfrm_policy.c
@@ -1921,6 +1921,9 @@ no_transform:
         }
 ok:
         xfrm_pols_put(pols, drop_pols);
+        if (dst && dst->xfrm &&
+            dst->xfrm->props.mode == XFRM_MODE_TUNNEL)
+                dst->flags |= DST_XFRM_TUNNEL;
         return dst;
 
 nopol: