net/ipv6/exthdrs.c

   1 /*
   2  *      Extension Header handling for IPv6
   3  *      Linux INET6 implementation
   4  *
   5  *      Authors:
   6  *      Pedro Roque             <roque@di.fc.ul.pt>
   7  *      Andi Kleen              <ak@muc.de>
   8  *      Alexey Kuznetsov        <kuznet@ms2.inr.ac.ru>
   9  *
  10  *      $Id: exthdrs.c,v 1.13 2001/06/19 15:58:56 davem Exp $
  11  *
  12  *      This program is free software; you can redistribute it and/or
  13  *      modify it under the terms of the GNU General Public License
  14  *      as published by the Free Software Foundation; either version
  15  *      2 of the License, or (at your option) any later version.
  16  */
  17
  18 /* Changes:
  19  *      yoshfuji                : ensure not to overrun while parsing
  20  *                                tlv options.
  21  *      Mitsuru KANDA @USAGI and: Remove ipv6_parse_exthdrs().
  22  *      YOSHIFUJI Hideaki @USAGI  Register inbound extension header
  23  *                                handlers as inet6_protocol{}.
  24  */
  25
  26 #include <linux/errno.h>
  27 #include <linux/types.h>
  28 #include <linux/socket.h>
  29 #include <linux/sockios.h>
  30 #include <linux/sched.h>
  31 #include <linux/net.h>
  32 #include <linux/netdevice.h>
  33 #include <linux/in6.h>
  34 #include <linux/icmpv6.h>
  35
  36 #include <net/sock.h>
  37 #include <net/snmp.h>
  38
  39 #include <net/ipv6.h>
  40 #include <net/protocol.h>
  41 #include <net/transp_v6.h>
  42 #include <net/rawv6.h>
  43 #include <net/ndisc.h>
  44 #include <net/ip6_route.h>
  45 #include <net/addrconf.h>
  46 #ifdef CONFIG_IPV6_MIP6
  47 #include <net/xfrm.h>
  48 #endif
  49
  50 #include <asm/uaccess.h>
  51
  52 int ipv6_find_tlv(struct sk_buff *skb, int offset, int type)
  53 {
  54         int packet_len = skb->tail - skb->nh.raw;
  55         struct ipv6_opt_hdr *hdr;
  56         int len;
  57
  58         if (offset + 2 > packet_len)
  59                 goto bad;
  60         hdr = (struct ipv6_opt_hdr*)(skb->nh.raw + offset);
  61         len = ((hdr->hdrlen + 1) << 3);
  62
  63         if (offset + len > packet_len)
  64                 goto bad;
  65
  66         offset += 2;
  67         len -= 2;
  68
  69         while (len > 0) {
  70                 int opttype = skb->nh.raw[offset];
  71                 int optlen;
  72
  73                 if (opttype == type)
  74                         return offset;
  75
  76                 switch (opttype) {
  77                 case IPV6_TLV_PAD0:
  78                         optlen = 1;
  79                         break;
  80                 default:
  81                         optlen = skb->nh.raw[offset + 1] + 2;
  82                         if (optlen > len)
  83                                 goto bad;
  84                         break;
  85                 }
  86                 offset += optlen;
  87                 len -= optlen;
  88         }
  89         /* not_found */
  90         return -1;
  91  bad:
  92         return -1;
  93 }
  94
  95 /*
  96  *      Parsing tlv encoded headers.
  97  *
  98  *      Parsing function "func" returns 1, if parsing succeed
  99  *      and 0, if it failed.
 100  *      It MUST NOT touch skb->h.
 101  */
 102
 103 struct tlvtype_proc {
 104         int     type;
 105         int     (*func)(struct sk_buff **skbp, int offset);
 106 };
 107
 108 /*********************
 109   Generic functions
 110  *********************/
 111
 112 /* An unknown option is detected, decide what to do */
 113
 114 static int ip6_tlvopt_unknown(struct sk_buff **skbp, int optoff)
 115 {
 116         struct sk_buff *skb = *skbp;
 117
 118         switch ((skb->nh.raw[optoff] & 0xC0) >> 6) {
 119         case 0: /* ignore */
 120                 return 1;
 121
 122         case 1: /* drop packet */
 123                 break;
 124
 125         case 3: /* Send ICMP if not a multicast address and drop packet */
 126                 /* Actually, it is redundant check. icmp_send
 127                    will recheck in any case.
 128                  */
 129                 if (ipv6_addr_is_multicast(&skb->nh.ipv6h->daddr))
 130                         break;
 131         case 2: /* send ICMP PARM PROB regardless and drop packet */
 132                 icmpv6_param_prob(skb, ICMPV6_UNK_OPTION, optoff);
 133                 return 0;
 134         };
 135
 136         kfree_skb(skb);
 137         return 0;
 138 }
 139
 140 /* Parse tlv encoded option header (hop-by-hop or destination) */
 141
 142 static int ip6_parse_tlv(struct tlvtype_proc *procs, struct sk_buff **skbp)
 143 {
 144         struct sk_buff *skb = *skbp;
 145         struct tlvtype_proc *curr;
 146         int off = skb->h.raw - skb->nh.raw;
 147         int len = ((skb->h.raw[1]+1)<<3);
 148
 149         if ((skb->h.raw + len) - skb->data > skb_headlen(skb))
 150                 goto bad;
 151
 152         off += 2;
 153         len -= 2;
 154
 155         while (len > 0) {
 156                 int optlen = skb->nh.raw[off+1]+2;
 157
 158                 switch (skb->nh.raw[off]) {
 159                 case IPV6_TLV_PAD0:
 160                         optlen = 1;
 161                         break;
 162
 163                 case IPV6_TLV_PADN:
 164                         break;
 165
 166                 default: /* Other TLV code so scan list */
 167                         if (optlen > len)
 168                                 goto bad;
 169                         for (curr=procs; curr->type >= 0; curr++) {
 170                                 if (curr->type == skb->nh.raw[off]) {
 171                                         /* type specific length/alignment
 172                                            checks will be performed in the
 173                                            func(). */
 174                                         if (curr->func(skbp, off) == 0)
 175                                                 return 0;
 176                                         break;
 177                                 }
 178                         }
 179                         if (curr->type < 0) {
 180                                 if (ip6_tlvopt_unknown(skbp, off) == 0)
 181                                         return 0;
 182                         }
 183                         break;
 184                 }
 185                 off += optlen;
 186                 len -= optlen;
 187         }
 188         if (len == 0)
 189                 return 1;
 190 bad:
 191         kfree_skb(skb);
 192         return 0;
 193 }
 194
 195 /*****************************
 196   Destination options header.
 197  *****************************/
 198
 199 static struct tlvtype_proc tlvprocdestopt_lst[] = {
 200         /* No destination options are defined now */
 201         {-1,                    NULL}
 202 };
 203
 204 static int ipv6_destopt_rcv(struct sk_buff **skbp)
 205 {
 206         struct sk_buff *skb = *skbp;
 207         struct inet6_skb_parm *opt = IP6CB(skb);
 208
 209         if (!pskb_may_pull(skb, (skb->h.raw-skb->data)+8) ||
 210             !pskb_may_pull(skb, (skb->h.raw-skb->data)+((skb->h.raw[1]+1)<<3))) {
 211                 IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 212                 kfree_skb(skb);
 213                 return -1;
 214         }
 215
 216         opt->lastopt = skb->h.raw - skb->nh.raw;
 217         opt->dst1 = skb->h.raw - skb->nh.raw;
 218
 219         if (ip6_parse_tlv(tlvprocdestopt_lst, skbp)) {
 220                 skb = *skbp;
 221                 skb->h.raw += ((skb->h.raw[1]+1)<<3);
 222                 opt->nhoff = opt->dst1;
 223                 return 1;
 224         }
 225
 226         IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 227         return -1;
 228 }
 229
 230 static struct inet6_protocol destopt_protocol = {
 231         .handler        =       ipv6_destopt_rcv,
 232         .flags          =       INET6_PROTO_NOPOLICY | INET6_PROTO_GSO_EXTHDR,
 233 };
 234
 235 void __init ipv6_destopt_init(void)
 236 {
 237         if (inet6_add_protocol(&destopt_protocol, IPPROTO_DSTOPTS) < 0)
 238                 printk(KERN_ERR "ipv6_destopt_init: Could not register protocol\n");
 239 }
 240
 241 /********************************
 242   NONE header. No data in packet.
 243  ********************************/
 244
 245 static int ipv6_nodata_rcv(struct sk_buff **skbp)
 246 {
 247         struct sk_buff *skb = *skbp;
 248
 249         kfree_skb(skb);
 250         return 0;
 251 }
 252
 253 static struct inet6_protocol nodata_protocol = {
 254         .handler        =       ipv6_nodata_rcv,
 255         .flags          =       INET6_PROTO_NOPOLICY,
 256 };
 257
 258 void __init ipv6_nodata_init(void)
 259 {
 260         if (inet6_add_protocol(&nodata_protocol, IPPROTO_NONE) < 0)
 261                 printk(KERN_ERR "ipv6_nodata_init: Could not register protocol\n");
 262 }
 263
 264 /********************************
 265   Routing header.
 266  ********************************/
 267
 268 static int ipv6_rthdr_rcv(struct sk_buff **skbp)
 269 {
 270         struct sk_buff *skb = *skbp;
 271         struct inet6_skb_parm *opt = IP6CB(skb);
 272         struct in6_addr *addr = NULL;
 273         struct in6_addr daddr;
 274         int n, i;
 275
 276         struct ipv6_rt_hdr *hdr;
 277         struct rt0_hdr *rthdr;
 278
 279         if (!pskb_may_pull(skb, (skb->h.raw-skb->data)+8) ||
 280             !pskb_may_pull(skb, (skb->h.raw-skb->data)+((skb->h.raw[1]+1)<<3))) {
 281                 IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 282                 kfree_skb(skb);
 283                 return -1;
 284         }
 285
 286         hdr = (struct ipv6_rt_hdr *) skb->h.raw;
 287
 288         if (ipv6_addr_is_multicast(&skb->nh.ipv6h->daddr) ||
 289             skb->pkt_type != PACKET_HOST) {
 290                 IP6_INC_STATS_BH(IPSTATS_MIB_INADDRERRORS);
 291                 kfree_skb(skb);
 292                 return -1;
 293         }
 294
 295 looped_back:
 296         if (hdr->segments_left == 0) {
 297                 switch (hdr->type) {
 298 #ifdef CONFIG_IPV6_MIP6
 299                 case IPV6_SRCRT_TYPE_2:
 300                         /* Silently discard type 2 header unless it was
 301                          * processed by own
 302                          */
 303                         if (!addr) {
 304                                 IP6_INC_STATS_BH(IPSTATS_MIB_INADDRERRORS);
 305                                 kfree_skb(skb);
 306                                 return -1;
 307                         }
 308                         break;
 309 #endif
 310                 default:
 311                         break;
 312                 }
 313
 314                 opt->lastopt = skb->h.raw - skb->nh.raw;
 315                 opt->srcrt = skb->h.raw - skb->nh.raw;
 316                 skb->h.raw += (hdr->hdrlen + 1) << 3;
 317                 opt->dst0 = opt->dst1;
 318                 opt->dst1 = 0;
 319                 opt->nhoff = (&hdr->nexthdr) - skb->nh.raw;
 320                 return 1;
 321         }
 322
 323         switch (hdr->type) {
 324         case IPV6_SRCRT_TYPE_0:
 325                 if (hdr->hdrlen & 0x01) {
 326                         IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 327                         icmpv6_param_prob(skb, ICMPV6_HDR_FIELD, (&hdr->hdrlen) - skb->nh.raw);
 328                         return -1;
 329                 }
 330                 break;
 331 #ifdef CONFIG_IPV6_MIP6
 332         case IPV6_SRCRT_TYPE_2:
 333                 /* Silently discard invalid RTH type 2 */
 334                 if (hdr->hdrlen != 2 || hdr->segments_left != 1) {
 335                         IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 336                         kfree_skb(skb);
 337                         return -1;
 338                 }
 339                 break;
 340 #endif
 341         default:
 342                 IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 343                 icmpv6_param_prob(skb, ICMPV6_HDR_FIELD, (&hdr->type) - skb->nh.raw);
 344                 return -1;
 345         }
 346
 347         /*
 348          *      This is the routing header forwarding algorithm from
 349          *      RFC 2460, page 16.
 350          */
 351
 352         n = hdr->hdrlen >> 1;
 353
 354         if (hdr->segments_left > n) {
 355                 IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 356                 icmpv6_param_prob(skb, ICMPV6_HDR_FIELD, (&hdr->segments_left) - skb->nh.raw);
 357                 return -1;
 358         }
 359
 360         /* We are about to mangle packet header. Be careful!
 361            Do not damage packets queued somewhere.
 362          */
 363         if (skb_cloned(skb)) {
 364                 struct sk_buff *skb2 = skb_copy(skb, GFP_ATOMIC);
 365                 kfree_skb(skb);
 366                 /* the copy is a forwarded packet */
 367                 if (skb2 == NULL) {
 368                         IP6_INC_STATS_BH(IPSTATS_MIB_OUTDISCARDS);
 369                         return -1;
 370                 }
 371                 *skbp = skb = skb2;
 372                 opt = IP6CB(skb2);
 373                 hdr = (struct ipv6_rt_hdr *) skb2->h.raw;
 374         }
 375
 376         if (skb->ip_summed == CHECKSUM_COMPLETE)
 377                 skb->ip_summed = CHECKSUM_NONE;
 378
 379         i = n - --hdr->segments_left;
 380
 381         rthdr = (struct rt0_hdr *) hdr;
 382         addr = rthdr->addr;
 383         addr += i - 1;
 384
 385         switch (hdr->type) {
 386 #ifdef CONFIG_IPV6_MIP6
 387         case IPV6_SRCRT_TYPE_2:
 388                 if (xfrm6_input_addr(skb, (xfrm_address_t *)addr,
 389                                      (xfrm_address_t *)&skb->nh.ipv6h->saddr,
 390                                      IPPROTO_ROUTING) < 0) {
 391                         IP6_INC_STATS_BH(IPSTATS_MIB_INADDRERRORS);
 392                         kfree_skb(skb);
 393                         return -1;
 394                 }
 395                 if (!ipv6_chk_home_addr(addr)) {
 396                         IP6_INC_STATS_BH(IPSTATS_MIB_INADDRERRORS);
 397                         kfree_skb(skb);
 398                         return -1;
 399                 }
 400                 break;
 401 #endif
 402         default:
 403                 break;
 404         }
 405
 406         if (ipv6_addr_is_multicast(addr)) {
 407                 IP6_INC_STATS_BH(IPSTATS_MIB_INADDRERRORS);
 408                 kfree_skb(skb);
 409                 return -1;
 410         }
 411
 412         ipv6_addr_copy(&daddr, addr);
 413         ipv6_addr_copy(addr, &skb->nh.ipv6h->daddr);
 414         ipv6_addr_copy(&skb->nh.ipv6h->daddr, &daddr);
 415
 416         dst_release(xchg(&skb->dst, NULL));
 417         ip6_route_input(skb);
 418         if (skb->dst->error) {
 419                 skb_push(skb, skb->data - skb->nh.raw);
 420                 dst_input(skb);
 421                 return -1;
 422         }
 423
 424         if (skb->dst->dev->flags&IFF_LOOPBACK) {
 425                 if (skb->nh.ipv6h->hop_limit <= 1) {
 426                         IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 427                         icmpv6_send(skb, ICMPV6_TIME_EXCEED, ICMPV6_EXC_HOPLIMIT,
 428                                     0, skb->dev);
 429                         kfree_skb(skb);
 430                         return -1;
 431                 }
 432                 skb->nh.ipv6h->hop_limit--;
 433                 goto looped_back;
 434         }
 435
 436         skb_push(skb, skb->data - skb->nh.raw);
 437         dst_input(skb);
 438         return -1;
 439 }
 440
 441 static struct inet6_protocol rthdr_protocol = {
 442         .handler        =       ipv6_rthdr_rcv,
 443         .flags          =       INET6_PROTO_NOPOLICY | INET6_PROTO_GSO_EXTHDR,
 444 };
 445
 446 void __init ipv6_rthdr_init(void)
 447 {
 448         if (inet6_add_protocol(&rthdr_protocol, IPPROTO_ROUTING) < 0)
 449                 printk(KERN_ERR "ipv6_rthdr_init: Could not register protocol\n");
 450 };
 451
 452 /*
 453    This function inverts received rthdr.
 454    NOTE: specs allow to make it automatically only if
 455    packet authenticated.
 456
 457    I will not discuss it here (though, I am really pissed off at
 458    this stupid requirement making rthdr idea useless)
 459
 460    Actually, it creates severe problems  for us.
 461    Embryonic requests has no associated sockets,
 462    so that user have no control over it and
 463    cannot not only to set reply options, but
 464    even to know, that someone wants to connect
 465    without success. :-(
 466
 467    For now we need to test the engine, so that I created
 468    temporary (or permanent) backdoor.
 469    If listening socket set IPV6_RTHDR to 2, then we invert header.
 470                                                    --ANK (980729)
 471  */
 472
 473 struct ipv6_txoptions *
 474 ipv6_invert_rthdr(struct sock *sk, struct ipv6_rt_hdr *hdr)
 475 {
 476         /* Received rthdr:
 477
 478            [ H1 -> H2 -> ... H_prev ]  daddr=ME
 479
 480            Inverted result:
 481            [ H_prev -> ... -> H1 ] daddr =sender
 482
 483            Note, that IP output engine will rewrite this rthdr
 484            by rotating it left by one addr.
 485          */
 486
 487         int n, i;
 488         struct rt0_hdr *rthdr = (struct rt0_hdr*)hdr;
 489         struct rt0_hdr *irthdr;
 490         struct ipv6_txoptions *opt;
 491         int hdrlen = ipv6_optlen(hdr);
 492
 493         if (hdr->segments_left ||
 494             hdr->type != IPV6_SRCRT_TYPE_0 ||
 495             hdr->hdrlen & 0x01)
 496                 return NULL;
 497
 498         n = hdr->hdrlen >> 1;
 499         opt = sock_kmalloc(sk, sizeof(*opt) + hdrlen, GFP_ATOMIC);
 500         if (opt == NULL)
 501                 return NULL;
 502         memset(opt, 0, sizeof(*opt));
 503         opt->tot_len = sizeof(*opt) + hdrlen;
 504         opt->srcrt = (void*)(opt+1);
 505         opt->opt_nflen = hdrlen;
 506
 507         memcpy(opt->srcrt, hdr, sizeof(*hdr));
 508         irthdr = (struct rt0_hdr*)opt->srcrt;
 509         irthdr->reserved = 0;
 510         opt->srcrt->segments_left = n;
 511         for (i=0; i<n; i++)
 512                 memcpy(irthdr->addr+i, rthdr->addr+(n-1-i), 16);
 513         return opt;
 514 }
 515
 516 EXPORT_SYMBOL_GPL(ipv6_invert_rthdr);
 517
 518 /**********************************
 519   Hop-by-hop options.
 520  **********************************/
 521
 522 /* Router Alert as of RFC 2711 */
 523
 524 static int ipv6_hop_ra(struct sk_buff **skbp, int optoff)
 525 {
 526         struct sk_buff *skb = *skbp;
 527
 528         if (skb->nh.raw[optoff+1] == 2) {
 529                 IP6CB(skb)->ra = optoff;
 530                 return 1;
 531         }
 532         LIMIT_NETDEBUG(KERN_DEBUG "ipv6_hop_ra: wrong RA length %d\n",
 533                        skb->nh.raw[optoff+1]);
 534         kfree_skb(skb);
 535         return 0;
 536 }
 537
 538 /* Jumbo payload */
 539
 540 static int ipv6_hop_jumbo(struct sk_buff **skbp, int optoff)
 541 {
 542         struct sk_buff *skb = *skbp;
 543         u32 pkt_len;
 544
 545         if (skb->nh.raw[optoff+1] != 4 || (optoff&3) != 2) {
 546                 LIMIT_NETDEBUG(KERN_DEBUG "ipv6_hop_jumbo: wrong jumbo opt length/alignment %d\n",
 547                                skb->nh.raw[optoff+1]);
 548                 IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 549                 goto drop;
 550         }
 551
 552         pkt_len = ntohl(*(u32*)(skb->nh.raw+optoff+2));
 553         if (pkt_len <= IPV6_MAXPLEN) {
 554                 IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 555                 icmpv6_param_prob(skb, ICMPV6_HDR_FIELD, optoff+2);
 556                 return 0;
 557         }
 558         if (skb->nh.ipv6h->payload_len) {
 559                 IP6_INC_STATS_BH(IPSTATS_MIB_INHDRERRORS);
 560                 icmpv6_param_prob(skb, ICMPV6_HDR_FIELD, optoff);
 561                 return 0;
 562         }
 563
 564         if (pkt_len > skb->len - sizeof(struct ipv6hdr)) {
 565                 IP6_INC_STATS_BH(IPSTATS_MIB_INTRUNCATEDPKTS);
 566                 goto drop;
 567         }
 568
 569         if (pskb_trim_rcsum(skb, pkt_len + sizeof(struct ipv6hdr)))
 570                 goto drop;
 571
 572         return 1;
 573
 574 drop:
 575         kfree_skb(skb);
 576         return 0;
 577 }
 578
 579 static struct tlvtype_proc tlvprochopopt_lst[] = {
 580         {
 581                 .type   = IPV6_TLV_ROUTERALERT,
 582                 .func   = ipv6_hop_ra,
 583         },
 584         {
 585                 .type   = IPV6_TLV_JUMBO,
 586                 .func   = ipv6_hop_jumbo,
 587         },
 588         { -1, }
 589 };
 590
 591 int ipv6_parse_hopopts(struct sk_buff **skbp)
 592 {
 593         struct sk_buff *skb = *skbp;
 594         struct inet6_skb_parm *opt = IP6CB(skb);
 595
 596         /*
 597          * skb->nh.raw is equal to skb->data, and
 598          * skb->h.raw - skb->nh.raw is always equal to
 599          * sizeof(struct ipv6hdr) by definition of
 600          * hop-by-hop options.
 601          */
 602         if (!pskb_may_pull(skb, sizeof(struct ipv6hdr) + 8) ||
 603             !pskb_may_pull(skb, sizeof(struct ipv6hdr) + ((skb->h.raw[1] + 1) << 3))) {
 604                 kfree_skb(skb);
 605                 return -1;
 606         }
 607
 608         opt->hop = sizeof(struct ipv6hdr);
 609         if (ip6_parse_tlv(tlvprochopopt_lst, skbp)) {
 610                 skb = *skbp;
 611                 skb->h.raw += (skb->h.raw[1]+1)<<3;
 612                 opt->nhoff = sizeof(struct ipv6hdr);
 613                 return 1;
 614         }
 615         return -1;
 616 }
 617
 618 /*
 619  *      Creating outbound headers.
 620  *
 621  *      "build" functions work when skb is filled from head to tail (datagram)
 622  *      "push"  functions work when headers are added from tail to head (tcp)
 623  *
 624  *      In both cases we assume, that caller reserved enough room
 625  *      for headers.
 626  */
 627
 628 static void ipv6_push_rthdr(struct sk_buff *skb, u8 *proto,
 629                             struct ipv6_rt_hdr *opt,
 630                             struct in6_addr **addr_p)
 631 {
 632         struct rt0_hdr *phdr, *ihdr;
 633         int hops;
 634
 635         ihdr = (struct rt0_hdr *) opt;
 636
 637         phdr = (struct rt0_hdr *) skb_push(skb, (ihdr->rt_hdr.hdrlen + 1) << 3);
 638         memcpy(phdr, ihdr, sizeof(struct rt0_hdr));
 639
 640         hops = ihdr->rt_hdr.hdrlen >> 1;
 641
 642         if (hops > 1)
 643                 memcpy(phdr->addr, ihdr->addr + 1,
 644                        (hops - 1) * sizeof(struct in6_addr));
 645
 646         ipv6_addr_copy(phdr->addr + (hops - 1), *addr_p);
 647         *addr_p = ihdr->addr;
 648
 649         phdr->rt_hdr.nexthdr = *proto;
 650         *proto = NEXTHDR_ROUTING;
 651 }
 652
 653 static void ipv6_push_exthdr(struct sk_buff *skb, u8 *proto, u8 type, struct ipv6_opt_hdr *opt)
 654 {
 655         struct ipv6_opt_hdr *h = (struct ipv6_opt_hdr *)skb_push(skb, ipv6_optlen(opt));
 656
 657         memcpy(h, opt, ipv6_optlen(opt));
 658         h->nexthdr = *proto;
 659         *proto = type;
 660 }
 661
 662 void ipv6_push_nfrag_opts(struct sk_buff *skb, struct ipv6_txoptions *opt,
 663                           u8 *proto,
 664                           struct in6_addr **daddr)
 665 {
 666         if (opt->srcrt) {
 667                 ipv6_push_rthdr(skb, proto, opt->srcrt, daddr);
 668                 /*
 669                  * IPV6_RTHDRDSTOPTS is ignored
 670                  * unless IPV6_RTHDR is set (RFC3542).
 671                  */
 672                 if (opt->dst0opt)
 673                         ipv6_push_exthdr(skb, proto, NEXTHDR_DEST, opt->dst0opt);
 674         }
 675         if (opt->hopopt)
 676                 ipv6_push_exthdr(skb, proto, NEXTHDR_HOP, opt->hopopt);
 677 }
 678
 679 void ipv6_push_frag_opts(struct sk_buff *skb, struct ipv6_txoptions *opt, u8 *proto)
 680 {
 681         if (opt->dst1opt)
 682                 ipv6_push_exthdr(skb, proto, NEXTHDR_DEST, opt->dst1opt);
 683 }
 684
 685 struct ipv6_txoptions *
 686 ipv6_dup_options(struct sock *sk, struct ipv6_txoptions *opt)
 687 {
 688         struct ipv6_txoptions *opt2;
 689
 690         opt2 = sock_kmalloc(sk, opt->tot_len, GFP_ATOMIC);
 691         if (opt2) {
 692                 long dif = (char*)opt2 - (char*)opt;
 693                 memcpy(opt2, opt, opt->tot_len);
 694                 if (opt2->hopopt)
 695                         *((char**)&opt2->hopopt) += dif;
 696                 if (opt2->dst0opt)
 697                         *((char**)&opt2->dst0opt) += dif;
 698                 if (opt2->dst1opt)
 699                         *((char**)&opt2->dst1opt) += dif;
 700                 if (opt2->srcrt)
 701                         *((char**)&opt2->srcrt) += dif;
 702         }
 703         return opt2;
 704 }
 705
 706 EXPORT_SYMBOL_GPL(ipv6_dup_options);
 707
 708 static int ipv6_renew_option(void *ohdr,
 709                              struct ipv6_opt_hdr __user *newopt, int newoptlen,
 710                              int inherit,
 711                              struct ipv6_opt_hdr **hdr,
 712                              char **p)
 713 {
 714         if (inherit) {
 715                 if (ohdr) {
 716                         memcpy(*p, ohdr, ipv6_optlen((struct ipv6_opt_hdr *)ohdr));
 717                         *hdr = (struct ipv6_opt_hdr *)*p;
 718                         *p += CMSG_ALIGN(ipv6_optlen(*(struct ipv6_opt_hdr **)hdr));
 719                 }
 720         } else {
 721                 if (newopt) {
 722                         if (copy_from_user(*p, newopt, newoptlen))
 723                                 return -EFAULT;
 724                         *hdr = (struct ipv6_opt_hdr *)*p;
 725                         if (ipv6_optlen(*(struct ipv6_opt_hdr **)hdr) > newoptlen)
 726                                 return -EINVAL;
 727                         *p += CMSG_ALIGN(newoptlen);
 728                 }
 729         }
 730         return 0;
 731 }
 732
 733 struct ipv6_txoptions *
 734 ipv6_renew_options(struct sock *sk, struct ipv6_txoptions *opt,
 735                    int newtype,
 736                    struct ipv6_opt_hdr __user *newopt, int newoptlen)
 737 {
 738         int tot_len = 0;
 739         char *p;
 740         struct ipv6_txoptions *opt2;
 741         int err;
 742
 743         if (opt) {
 744                 if (newtype != IPV6_HOPOPTS && opt->hopopt)
 745                         tot_len += CMSG_ALIGN(ipv6_optlen(opt->hopopt));
 746                 if (newtype != IPV6_RTHDRDSTOPTS && opt->dst0opt)
 747                         tot_len += CMSG_ALIGN(ipv6_optlen(opt->dst0opt));
 748                 if (newtype != IPV6_RTHDR && opt->srcrt)
 749                         tot_len += CMSG_ALIGN(ipv6_optlen(opt->srcrt));
 750                 if (newtype != IPV6_DSTOPTS && opt->dst1opt)
 751                         tot_len += CMSG_ALIGN(ipv6_optlen(opt->dst1opt));
 752         }
 753
 754         if (newopt && newoptlen)
 755                 tot_len += CMSG_ALIGN(newoptlen);
 756
 757         if (!tot_len)
 758                 return NULL;
 759
 760         tot_len += sizeof(*opt2);
 761         opt2 = sock_kmalloc(sk, tot_len, GFP_ATOMIC);
 762         if (!opt2)
 763                 return ERR_PTR(-ENOBUFS);
 764
 765         memset(opt2, 0, tot_len);
 766
 767         opt2->tot_len = tot_len;
 768         p = (char *)(opt2 + 1);
 769
 770         err = ipv6_renew_option(opt ? opt->hopopt : NULL, newopt, newoptlen,
 771                                 newtype != IPV6_HOPOPTS,
 772                                 &opt2->hopopt, &p);
 773         if (err)
 774                 goto out;
 775
 776         err = ipv6_renew_option(opt ? opt->dst0opt : NULL, newopt, newoptlen,
 777                                 newtype != IPV6_RTHDRDSTOPTS,
 778                                 &opt2->dst0opt, &p);
 779         if (err)
 780                 goto out;
 781
 782         err = ipv6_renew_option(opt ? opt->srcrt : NULL, newopt, newoptlen,
 783                                 newtype != IPV6_RTHDR,
 784                                 (struct ipv6_opt_hdr **)&opt2->srcrt, &p);
 785         if (err)
 786                 goto out;
 787
 788         err = ipv6_renew_option(opt ? opt->dst1opt : NULL, newopt, newoptlen,
 789                                 newtype != IPV6_DSTOPTS,
 790                                 &opt2->dst1opt, &p);
 791         if (err)
 792                 goto out;
 793
 794         opt2->opt_nflen = (opt2->hopopt ? ipv6_optlen(opt2->hopopt) : 0) +
 795                           (opt2->dst0opt ? ipv6_optlen(opt2->dst0opt) : 0) +
 796                           (opt2->srcrt ? ipv6_optlen(opt2->srcrt) : 0);
 797         opt2->opt_flen = (opt2->dst1opt ? ipv6_optlen(opt2->dst1opt) : 0);
 798
 799         return opt2;
 800 out:
 801         sock_kfree_s(sk, opt2, opt2->tot_len);
 802         return ERR_PTR(err);
 803 }
 804
 805 struct ipv6_txoptions *ipv6_fixup_options(struct ipv6_txoptions *opt_space,
 806                                           struct ipv6_txoptions *opt)
 807 {
 808         /*
 809          * ignore the dest before srcrt unless srcrt is being included.
 810          * --yoshfuji
 811          */
 812         if (opt && opt->dst0opt && !opt->srcrt) {
 813                 if (opt_space != opt) {
 814                         memcpy(opt_space, opt, sizeof(*opt_space));
 815                         opt = opt_space;
 816                 }
 817                 opt->opt_nflen -= ipv6_optlen(opt->dst0opt);
 818                 opt->dst0opt = NULL;
 819         }
 820
 821         return opt;
 822 }
 823