lwip_lowpan6_common.c

00001 /**
00002  * @file
00003  *
00004  * Common 6LowPAN routines for IPv6. Uses ND tables for link-layer addressing. Fragments packets to 6LowPAN units.
00005  *
00006  * This implementation aims to conform to IEEE 802.15.4(-2015), RFC 4944 and RFC 6282.
00007  * @todo: RFC 6775.
00008  */
00009 
00010 /*
00011  * Copyright (c) 2015 Inico Technologies Ltd.
00012  * All rights reserved.
00013  *
00014  * Redistribution and use in source and binary forms, with or without modification,
00015  * are permitted provided that the following conditions are met:
00016  *
00017  * 1. Redistributions of source code must retain the above copyright notice,
00018  *    this list of conditions and the following disclaimer.
00019  * 2. Redistributions in binary form must reproduce the above copyright notice,
00020  *    this list of conditions and the following disclaimer in the documentation
00021  *    and/or other materials provided with the distribution.
00022  * 3. The name of the author may not be used to endorse or promote products
00023  *    derived from this software without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
00026  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00027  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
00028  * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00029  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
00030  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00031  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00032  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
00033  * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
00034  * OF SUCH DAMAGE.
00035  *
00036  * This file is part of the lwIP TCP/IP stack.
00037  *
00038  * Author: Ivan Delamer <delamer@inicotech.com>
00039  *
00040  *
00041  * Please coordinate changes and requests with Ivan Delamer
00042  * <delamer@inicotech.com>
00043  */
00044 
00045 /**
00046  * @defgroup sixlowpan 6LoWPAN (RFC4944)
00047  * @ingroup netifs
00048  * 6LowPAN netif implementation
00049  */
00050 
00051 #include "netif/lowpan6_common.h"
00052 
00053 #if LWIP_IPV6
00054 
00055 #include "lwip/ip.h"
00056 #include "lwip/pbuf.h"
00057 #include "lwip/ip_addr.h"
00058 #include "lwip/netif.h"
00059 #include "lwip/udp.h"
00060 
00061 #include <string.h>
00062 
00063 /* Determine compression mode for unicast address. */
00064 s8_t
00065 lowpan6_get_address_mode(const ip6_addr_t *ip6addr, const struct lowpan6_link_addr *mac_addr)
00066 {
00067   if (mac_addr->addr_len == 2) {
00068     if ((ip6addr->addr[2] == (u32_t)PP_HTONL(0x000000ff)) &&
00069         ((ip6addr->addr[3]  & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000))) {
00070       if ((ip6addr->addr[3]  & PP_HTONL(0x0000ffff)) == lwip_ntohl((mac_addr->addr[0] << 8) | mac_addr->addr[1])) {
00071         return 3;
00072       }
00073     }
00074   } else if (mac_addr->addr_len == 8) {
00075     if ((ip6addr->addr[2] == lwip_ntohl(((mac_addr->addr[0] ^ 2) << 24) | (mac_addr->addr[1] << 16) | mac_addr->addr[2] << 8 | mac_addr->addr[3])) &&
00076         (ip6addr->addr[3] == lwip_ntohl((mac_addr->addr[4] << 24) | (mac_addr->addr[5] << 16) | mac_addr->addr[6] << 8 | mac_addr->addr[7]))) {
00077       return 3;
00078     }
00079   }
00080 
00081   if ((ip6addr->addr[2] == PP_HTONL(0x000000ffUL)) &&
00082       ((ip6addr->addr[3]  & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000UL))) {
00083     return 2;
00084   }
00085 
00086   return 1;
00087 }
00088 
00089 #if LWIP_6LOWPAN_IPHC
00090 
00091 /* Determine compression mode for multicast address. */
00092 static s8_t
00093 lowpan6_get_address_mode_mc(const ip6_addr_t *ip6addr)
00094 {
00095   if ((ip6addr->addr[0] == PP_HTONL(0xff020000)) &&
00096       (ip6addr->addr[1] == 0) &&
00097       (ip6addr->addr[2] == 0) &&
00098       ((ip6addr->addr[3]  & PP_HTONL(0xffffff00)) == 0)) {
00099     return 3;
00100   } else if (((ip6addr->addr[0] & PP_HTONL(0xff00ffff)) == PP_HTONL(0xff000000)) &&
00101              (ip6addr->addr[1] == 0)) {
00102     if ((ip6addr->addr[2] == 0) &&
00103         ((ip6addr->addr[3]  & PP_HTONL(0xff000000)) == 0)) {
00104       return 2;
00105     } else if ((ip6addr->addr[2]  & PP_HTONL(0xffffff00)) == 0) {
00106       return 1;
00107     }
00108   }
00109 
00110   return 0;
00111 }
00112 
00113 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
00114 static s8_t
00115 lowpan6_context_lookup(const ip6_addr_t *lowpan6_contexts, const ip6_addr_t *ip6addr)
00116 {
00117   s8_t i;
00118 
00119   for (i = 0; i < LWIP_6LOWPAN_NUM_CONTEXTS; i++) {
00120     if (ip6_addr_netcmp(&lowpan6_contexts[i], ip6addr)) {
00121       return i;
00122     }
00123   }
00124   return -1;
00125 }
00126 #endif /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */
00127 
00128 /*
00129  * Compress IPv6 and/or UDP headers.
00130  * */
00131 err_t
00132 lowpan6_compress_headers(struct netif *netif, u8_t *inbuf, size_t inbuf_size, u8_t *outbuf, size_t outbuf_size,
00133                          u8_t *lowpan6_header_len_out, u8_t *hidden_header_len_out, ip6_addr_t *lowpan6_contexts,
00134                          const struct lowpan6_link_addr *src, const struct lowpan6_link_addr *dst)
00135 {
00136   u8_t *buffer, *inptr;
00137   u8_t lowpan6_header_len;
00138   u8_t hidden_header_len = 0;
00139   s8_t i;
00140   struct ip6_hdr *ip6hdr;
00141   ip_addr_t ip6src, ip6dst;
00142 
00143   LWIP_ASSERT("netif != NULL", netif != NULL);
00144   LWIP_ASSERT("inbuf != NULL", inbuf != NULL);
00145   LWIP_ASSERT("outbuf != NULL", outbuf != NULL);
00146   LWIP_ASSERT("lowpan6_header_len_out != NULL", lowpan6_header_len_out != NULL);
00147   LWIP_ASSERT("hidden_header_len_out != NULL", hidden_header_len_out != NULL);
00148 
00149   /* Perform 6LowPAN IPv6 header compression according to RFC 6282 */
00150   buffer = outbuf;
00151   inptr = inbuf;
00152 
00153   if (inbuf_size < IP6_HLEN) {
00154     /* input buffer too short */
00155     return ERR_VAL;
00156   }
00157   if (outbuf_size < IP6_HLEN) {
00158     /* output buffer too short for worst case */
00159     return ERR_MEM;
00160   }
00161 
00162   /* Point to ip6 header and align copies of src/dest addresses. */
00163   ip6hdr = (struct ip6_hdr *)inptr;
00164   ip_addr_copy_from_ip6_packed(ip6dst, ip6hdr->dest);
00165   ip6_addr_assign_zone(ip_2_ip6(&ip6dst), IP6_UNKNOWN, netif);
00166   ip_addr_copy_from_ip6_packed(ip6src, ip6hdr->src);
00167   ip6_addr_assign_zone(ip_2_ip6(&ip6src), IP6_UNKNOWN, netif);
00168 
00169   /* Basic length of 6LowPAN header, set dispatch and clear fields. */
00170   lowpan6_header_len = 2;
00171   buffer[0] = 0x60;
00172   buffer[1] = 0;
00173 
00174   /* Determine whether there will be a Context Identifier Extension byte or not.
00175    * If so, set it already. */
00176 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
00177   buffer[2] = 0;
00178 
00179   i = lowpan6_context_lookup(lowpan6_contexts, ip_2_ip6(&ip6src));
00180   if (i >= 0) {
00181     /* Stateful source address compression. */
00182     buffer[1] |= 0x40;
00183     buffer[2] |= (i & 0x0f) << 4;
00184   }
00185 
00186   i = lowpan6_context_lookup(lowpan6_contexts, ip_2_ip6(&ip6dst));
00187   if (i >= 0) {
00188     /* Stateful destination address compression. */
00189     buffer[1] |= 0x04;
00190     buffer[2] |= i & 0x0f;
00191   }
00192 
00193   if (buffer[2] != 0x00) {
00194     /* Context identifier extension byte is appended. */
00195     buffer[1] |= 0x80;
00196     lowpan6_header_len++;
00197   }
00198 #else /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */
00199   LWIP_UNUSED_ARG(lowpan6_contexts);
00200 #endif /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */
00201 
00202   /* Determine TF field: Traffic Class, Flow Label */
00203   if (IP6H_FL(ip6hdr) == 0) {
00204     /* Flow label is elided. */
00205     buffer[0] |= 0x10;
00206     if (IP6H_TC(ip6hdr) == 0) {
00207       /* Traffic class (ECN+DSCP) elided too. */
00208       buffer[0] |= 0x08;
00209     } else {
00210       /* Traffic class (ECN+DSCP) appended. */
00211       buffer[lowpan6_header_len++] = IP6H_TC(ip6hdr);
00212     }
00213   } else {
00214     if (((IP6H_TC(ip6hdr) & 0x3f) == 0)) {
00215       /* DSCP portion of Traffic Class is elided, ECN and FL are appended (3 bytes) */
00216       buffer[0] |= 0x08;
00217 
00218       buffer[lowpan6_header_len] = IP6H_TC(ip6hdr) & 0xc0;
00219       buffer[lowpan6_header_len++] |= (IP6H_FL(ip6hdr) >> 16) & 0x0f;
00220       buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff;
00221       buffer[lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff;
00222     } else {
00223       /* Traffic class and flow label are appended (4 bytes) */
00224       buffer[lowpan6_header_len++] = IP6H_TC(ip6hdr);
00225       buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 16) & 0x0f;
00226       buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff;
00227       buffer[lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff;
00228     }
00229   }
00230 
00231   /* Compress NH?
00232   * Only if UDP for now. @todo support other NH compression. */
00233   if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) {
00234     buffer[0] |= 0x04;
00235   } else {
00236     /* append nexth. */
00237     buffer[lowpan6_header_len++] = IP6H_NEXTH(ip6hdr);
00238   }
00239 
00240   /* Compress hop limit? */
00241   if (IP6H_HOPLIM(ip6hdr) == 255) {
00242     buffer[0] |= 0x03;
00243   } else if (IP6H_HOPLIM(ip6hdr) == 64) {
00244     buffer[0] |= 0x02;
00245   } else if (IP6H_HOPLIM(ip6hdr) == 1) {
00246     buffer[0] |= 0x01;
00247   } else {
00248     /* append hop limit */
00249     buffer[lowpan6_header_len++] = IP6H_HOPLIM(ip6hdr);
00250   }
00251 
00252   /* Compress source address */
00253   if (((buffer[1] & 0x40) != 0) ||
00254       (ip6_addr_islinklocal(ip_2_ip6(&ip6src)))) {
00255     /* Context-based or link-local source address compression. */
00256     i = lowpan6_get_address_mode(ip_2_ip6(&ip6src), src);
00257     buffer[1] |= (i & 0x03) << 4;
00258     if (i == 1) {
00259       MEMCPY(buffer + lowpan6_header_len, inptr + 16, 8);
00260       lowpan6_header_len += 8;
00261     } else if (i == 2) {
00262       MEMCPY(buffer + lowpan6_header_len, inptr + 22, 2);
00263       lowpan6_header_len += 2;
00264     }
00265   } else if (ip6_addr_isany(ip_2_ip6(&ip6src))) {
00266     /* Special case: mark SAC and leave SAM=0 */
00267     buffer[1] |= 0x40;
00268   } else {
00269     /* Append full address. */
00270     MEMCPY(buffer + lowpan6_header_len, inptr + 8, 16);
00271     lowpan6_header_len += 16;
00272   }
00273 
00274   /* Compress destination address */
00275   if (ip6_addr_ismulticast(ip_2_ip6(&ip6dst))) {
00276     /* @todo support stateful multicast address compression */
00277 
00278     buffer[1] |= 0x08;
00279 
00280     i = lowpan6_get_address_mode_mc(ip_2_ip6(&ip6dst));
00281     buffer[1] |= i & 0x03;
00282     if (i == 0) {
00283       MEMCPY(buffer + lowpan6_header_len, inptr + 24, 16);
00284       lowpan6_header_len += 16;
00285     } else if (i == 1) {
00286       buffer[lowpan6_header_len++] = inptr[25];
00287       MEMCPY(buffer + lowpan6_header_len, inptr + 35, 5);
00288       lowpan6_header_len += 5;
00289     } else if (i == 2) {
00290       buffer[lowpan6_header_len++] = inptr[25];
00291       MEMCPY(buffer + lowpan6_header_len, inptr + 37, 3);
00292       lowpan6_header_len += 3;
00293     } else if (i == 3) {
00294       buffer[lowpan6_header_len++] = (inptr)[39];
00295     }
00296   } else if (((buffer[1] & 0x04) != 0) ||
00297               (ip6_addr_islinklocal(ip_2_ip6(&ip6dst)))) {
00298     /* Context-based or link-local destination address compression. */
00299     i = lowpan6_get_address_mode(ip_2_ip6(&ip6dst), dst);
00300     buffer[1] |= i & 0x03;
00301     if (i == 1) {
00302       MEMCPY(buffer + lowpan6_header_len, inptr + 32, 8);
00303       lowpan6_header_len += 8;
00304     } else if (i == 2) {
00305       MEMCPY(buffer + lowpan6_header_len, inptr + 38, 2);
00306       lowpan6_header_len += 2;
00307     }
00308   } else {
00309     /* Append full address. */
00310     MEMCPY(buffer + lowpan6_header_len, inptr + 24, 16);
00311     lowpan6_header_len += 16;
00312   }
00313 
00314   /* Move to payload. */
00315   inptr += IP6_HLEN;
00316   hidden_header_len += IP6_HLEN;
00317 
00318 #if LWIP_UDP
00319   /* Compress UDP header? */
00320   if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) {
00321     /* @todo support optional checksum compression */
00322 
00323     if (inbuf_size < IP6_HLEN + UDP_HLEN) {
00324       /* input buffer too short */
00325       return ERR_VAL;
00326     }
00327     if (outbuf_size < (size_t)(hidden_header_len + 7)) {
00328       /* output buffer too short for worst case */
00329       return ERR_MEM;
00330     }
00331 
00332     buffer[lowpan6_header_len] = 0xf0;
00333 
00334     /* determine port compression mode. */
00335     if ((inptr[0] == 0xf0) && ((inptr[1] & 0xf0) == 0xb0) &&
00336         (inptr[2] == 0xf0) && ((inptr[3] & 0xf0) == 0xb0)) {
00337       /* Compress source and dest ports. */
00338       buffer[lowpan6_header_len++] |= 0x03;
00339       buffer[lowpan6_header_len++] = ((inptr[1] & 0x0f) << 4) | (inptr[3] & 0x0f);
00340     } else if (inptr[0] == 0xf0) {
00341       /* Compress source port. */
00342       buffer[lowpan6_header_len++] |= 0x02;
00343       buffer[lowpan6_header_len++] = inptr[1];
00344       buffer[lowpan6_header_len++] = inptr[2];
00345       buffer[lowpan6_header_len++] = inptr[3];
00346     } else if (inptr[2] == 0xf0) {
00347       /* Compress dest port. */
00348       buffer[lowpan6_header_len++] |= 0x01;
00349       buffer[lowpan6_header_len++] = inptr[0];
00350       buffer[lowpan6_header_len++] = inptr[1];
00351       buffer[lowpan6_header_len++] = inptr[3];
00352     } else {
00353       /* append full ports. */
00354       lowpan6_header_len++;
00355       buffer[lowpan6_header_len++] = inptr[0];
00356       buffer[lowpan6_header_len++] = inptr[1];
00357       buffer[lowpan6_header_len++] = inptr[2];
00358       buffer[lowpan6_header_len++] = inptr[3];
00359     }
00360 
00361     /* elide length and copy checksum */
00362     buffer[lowpan6_header_len++] = inptr[6];
00363     buffer[lowpan6_header_len++] = inptr[7];
00364 
00365     hidden_header_len += UDP_HLEN;
00366   }
00367 #endif /* LWIP_UDP */
00368 
00369   *lowpan6_header_len_out = lowpan6_header_len;
00370   *hidden_header_len_out = hidden_header_len;
00371 
00372   return ERR_OK;
00373 }
00374 
00375 /** Decompress IPv6 and UDP headers compressed according to RFC 6282
00376  *
00377  * @param lowpan6_buffer compressed headers, first byte is the dispatch byte
00378  * @param lowpan6_bufsize size of lowpan6_buffer (may include data after headers)
00379  * @param decomp_buffer buffer where the decompressed headers are stored
00380  * @param decomp_bufsize size of decomp_buffer
00381  * @param hdr_size_comp returns the size of the compressed headers (skip to get to data)
00382  * @param hdr_size_decomp returns the size of the decompressed headers (IPv6 + UDP)
00383  * @param datagram_size datagram size from fragments or 0 if unfragmented
00384  * @param compressed_size compressed datagram size (for unfragmented rx)
00385  * @param lowpan6_contexts context addresses
00386  * @param src source address of the outer layer, used for address compression
00387  * @param dest destination address of the outer layer, used for address compression
00388  * @return ERR_OK if decompression succeeded, an error otherwise
00389  */
00390 static err_t
00391 lowpan6_decompress_hdr(u8_t *lowpan6_buffer, size_t lowpan6_bufsize,
00392                        u8_t *decomp_buffer, size_t decomp_bufsize,
00393                        u16_t *hdr_size_comp, u16_t *hdr_size_decomp,
00394                        u16_t datagram_size, u16_t compressed_size,
00395                        ip6_addr_t *lowpan6_contexts,
00396                        struct lowpan6_link_addr *src, struct lowpan6_link_addr *dest)
00397 {
00398   u16_t lowpan6_offset;
00399   struct ip6_hdr *ip6hdr;
00400   s8_t i;
00401   u32_t header_temp;
00402   u16_t ip6_offset = IP6_HLEN;
00403 
00404   LWIP_ASSERT("lowpan6_buffer != NULL", lowpan6_buffer != NULL);
00405   LWIP_ASSERT("decomp_buffer != NULL", decomp_buffer != NULL);
00406   LWIP_ASSERT("src != NULL", src != NULL);
00407   LWIP_ASSERT("dest != NULL", dest != NULL);
00408   LWIP_ASSERT("hdr_size_comp != NULL", hdr_size_comp != NULL);
00409   LWIP_ASSERT("dehdr_size_decompst != NULL", hdr_size_decomp != NULL);
00410 
00411   ip6hdr = (struct ip6_hdr *)decomp_buffer;
00412   if (decomp_bufsize < IP6_HLEN) {
00413     return ERR_MEM;
00414   }
00415 
00416   /* output the full compressed packet, if set in @see lowpan6_opts.h */
00417 #if LWIP_LOWPAN6_IP_COMPRESSED_DEBUG
00418   {
00419     u16_t j;
00420     LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("lowpan6_decompress_hdr: IP6 payload (compressed): \n"));
00421     for (j = 0; j < lowpan6_bufsize; j++) {
00422       if ((j % 4) == 0) {
00423         LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("\n"));
00424       }
00425       LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("%2X ", lowpan6_buffer[j]));
00426     }
00427     LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("\np->len: %d", lowpan6_bufsize));
00428   }
00429 #endif
00430 
00431   /* offset for inline IP headers (RFC 6282 ch3)*/
00432   lowpan6_offset = 2;
00433   /* if CID is set (context identifier), the context byte 
00434    * follows immediately after the header, so other IPHC fields are @+3 */
00435   if (lowpan6_buffer[1] & 0x80) {
00436     lowpan6_offset++;
00437   }
00438 
00439   /* Set IPv6 version, traffic class and flow label. (RFC6282, ch 3.1.1.)*/
00440   if ((lowpan6_buffer[0] & 0x18) == 0x00) {
00441     header_temp = ((lowpan6_buffer[lowpan6_offset+1] & 0x0f) << 16) | \
00442       (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset+3];
00443     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 00, ECN: 0x%2x, Flowlabel+DSCP: 0x%8X\n", \
00444       lowpan6_buffer[lowpan6_offset],header_temp));
00445     IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset], header_temp);
00446     /* increase offset, processed 4 bytes here:
00447      * TF=00:  ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)*/
00448     lowpan6_offset += 4;
00449   } else if ((lowpan6_buffer[0] & 0x18) == 0x08) {
00450     header_temp = ((lowpan6_buffer[lowpan6_offset] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset+2];
00451     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 01, ECN: 0x%2x, Flowlabel: 0x%2X, DSCP ignored\n", \
00452       lowpan6_buffer[lowpan6_offset] & 0xc0,header_temp));
00453     IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset] & 0xc0, header_temp);
00454     /* increase offset, processed 3 bytes here:
00455      * TF=01:  ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided.*/
00456     lowpan6_offset += 3;
00457   } else if ((lowpan6_buffer[0] & 0x18) == 0x10) {
00458     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 10, DCSP+ECN: 0x%2x, Flowlabel ignored\n", lowpan6_buffer[lowpan6_offset]));
00459     IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset],0);
00460     /* increase offset, processed 1 byte here:
00461      * ECN + DSCP (1 byte), Flow Label is elided.*/
00462     lowpan6_offset += 1;
00463   } else if ((lowpan6_buffer[0] & 0x18) == 0x18) {
00464     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 11, DCSP/ECN & Flowlabel ignored\n"));
00465     /* don't increase offset, no bytes processed here */
00466     IP6H_VTCFL_SET(ip6hdr, 6, 0, 0);
00467   }
00468 
00469   /* Set Next Header (NH) */
00470   if ((lowpan6_buffer[0] & 0x04) == 0x00) {
00471     /* 0: full next header byte carried inline (increase offset)*/
00472     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NH: 0x%2X\n", lowpan6_buffer[lowpan6_offset+1]));
00473     IP6H_NEXTH_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
00474   } else {
00475     /* 1: NH compression, LOWPAN_NHC (RFC6282, ch 4.1) */
00476     /* We should fill this later with NHC decoding */
00477     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NH: skipped, later done with NHC\n"));
00478     IP6H_NEXTH_SET(ip6hdr, 0);
00479   }
00480 
00481   /* Set Hop Limit, either carried inline or 3 different hops (1,64,255) */
00482   if ((lowpan6_buffer[0] & 0x03) == 0x00) {
00483     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: full value: %d\n", lowpan6_buffer[lowpan6_offset+1]));
00484     IP6H_HOPLIM_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
00485   } else if ((lowpan6_buffer[0] & 0x03) == 0x01) {
00486     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: compressed: 1\n"));
00487     IP6H_HOPLIM_SET(ip6hdr, 1);
00488   } else if ((lowpan6_buffer[0] & 0x03) == 0x02) {
00489     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: compressed: 64\n"));
00490     IP6H_HOPLIM_SET(ip6hdr, 64);
00491   } else if ((lowpan6_buffer[0] & 0x03) == 0x03) {
00492     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: compressed: 255\n"));
00493     IP6H_HOPLIM_SET(ip6hdr, 255);
00494   }
00495 
00496   /* Source address decoding. */
00497   if ((lowpan6_buffer[1] & 0x40) == 0x00) {
00498     /* Source address compression (SAC) = 0 -> stateless compression */
00499     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAC == 0, no context byte\n"));
00500     /* Stateless compression */
00501     if ((lowpan6_buffer[1] & 0x30) == 0x00) {
00502       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 00, no src compression, fetching 128bits inline\n"));
00503       /* copy full address, increase offset by 16 Bytes */
00504       MEMCPY(&ip6hdr->src.addr[0], lowpan6_buffer + lowpan6_offset, 16);
00505       lowpan6_offset += 16;
00506     } else if ((lowpan6_buffer[1] & 0x30) == 0x10) {
00507       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 01, src compression, 64bits inline\n"));
00508       /* set 64 bits to link local */
00509       ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
00510       ip6hdr->src.addr[1] = 0;
00511       /* copy 8 Bytes, increase offset */
00512       MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
00513       lowpan6_offset += 8;
00514     } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
00515       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 10, src compression, 16bits inline\n"));
00516       /* set 96 bits to link local */
00517       ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
00518       ip6hdr->src.addr[1] = 0;
00519       ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
00520       /* extract remaining 16bits from inline bytes, increase offset */
00521       ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) |
00522                                        lowpan6_buffer[lowpan6_offset + 1]);
00523       lowpan6_offset += 2;
00524     } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
00525       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 11, src compression, 0bits inline, using other headers\n"));
00526       /* no information avalaible, using other layers, see RFC6282 ch 3.2.2 */
00527       ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
00528       ip6hdr->src.addr[1] = 0;
00529       if (src->addr_len == 2) {
00530         ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
00531         ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
00532       } else if (src->addr_len == 8) {
00533         ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) |
00534                                          (src->addr[2] << 8) | src->addr[3]);
00535         ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) |
00536                                          (src->addr[6] << 8) | src->addr[7]);
00537       } else {
00538         /* invalid source address length */
00539         LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Invalid source address length\n"));
00540         return ERR_VAL;
00541       }
00542     }
00543   } else {
00544     /* Source address compression (SAC) = 1 -> stateful/context-based compression */
00545     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAC == 1, additional context byte\n"));
00546     if ((lowpan6_buffer[1] & 0x30) == 0x00) {
00547       /* SAM=00, address=> :: (ANY) */
00548       ip6hdr->src.addr[0] = 0;
00549       ip6hdr->src.addr[1] = 0;
00550       ip6hdr->src.addr[2] = 0;
00551       ip6hdr->src.addr[3] = 0;
00552       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 00, context compression, ANY (::)\n"));
00553     } else {
00554       /* Set prefix from context info */
00555       if (lowpan6_buffer[1] & 0x80) {
00556         i = (lowpan6_buffer[2] >> 4) & 0x0f;
00557       } else {
00558         i = 0;
00559       }
00560       if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
00561         /* Error */
00562         return ERR_VAL;
00563       }
00564 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
00565       ip6hdr->src.addr[0] = lowpan6_contexts[i].addr[0];
00566       ip6hdr->src.addr[1] = lowpan6_contexts[i].addr[1];
00567       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == xx, context compression found @%d: %8X, %8X\n", (int)i, ip6hdr->src.addr[0], ip6hdr->src.addr[1]));
00568 #else
00569       LWIP_UNUSED_ARG(lowpan6_contexts);
00570 #endif
00571     }
00572 
00573     /* determine further address bits */
00574     if ((lowpan6_buffer[1] & 0x30) == 0x10) {
00575       /* SAM=01, load additional 64bits */
00576       MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
00577       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 01, context compression, 64bits inline\n"));
00578       lowpan6_offset += 8;
00579     } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
00580       /* SAM=01, load additional 16bits */
00581       ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
00582       ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]);
00583       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 10, context compression, 16bits inline\n"));
00584       lowpan6_offset += 2;
00585     } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
00586       /* SAM=11, address is fully elided, load from other layers */
00587       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 11, context compression, 0bits inline, using other headers\n"));
00588       if (src->addr_len == 2) {
00589         ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
00590         ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
00591       } else if (src->addr_len == 8) {
00592         ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | (src->addr[2] << 8) | src->addr[3]);
00593         ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) | (src->addr[6] << 8) | src->addr[7]);
00594       } else {
00595         /* invalid source address length */
00596         LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Invalid source address length\n"));
00597         return ERR_VAL;
00598       }
00599     }
00600   }
00601 
00602   /* Destination address decoding. */
00603   if (lowpan6_buffer[1] & 0x08) {
00604     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("M=1: multicast\n"));
00605     /* Multicast destination */
00606     if (lowpan6_buffer[1] & 0x04) {
00607       LWIP_DEBUGF(LWIP_DBG_ON,("DAC == 1, context multicast: unsupported!!!\n"));
00608       /* @todo support stateful multicast addressing */
00609       return ERR_VAL;
00610     }
00611 
00612     if ((lowpan6_buffer[1] & 0x03) == 0x00) {
00613       /* DAM = 00, copy full address (128bits) */
00614       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 00, no dst compression, fetching 128bits inline\n"));
00615       MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
00616       lowpan6_offset += 16;
00617     } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
00618       /* DAM = 01, copy 4 bytes (32bits) */
00619       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 01, dst address form (48bits): ffXX::00XX:XXXX:XXXX\n"));
00620       ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16));
00621       ip6hdr->dest.addr[1] = 0;
00622       ip6hdr->dest.addr[2] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
00623       ip6hdr->dest.addr[3] = lwip_htonl((lowpan6_buffer[lowpan6_offset] << 24) | (lowpan6_buffer[lowpan6_offset + 1] << 16) | (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset + 3]);
00624       lowpan6_offset += 4;
00625     } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
00626       /* DAM = 10, copy 3 bytes (24bits) */
00627       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 10, dst address form (32bits): ffXX::00XX:XXXX\n"));
00628       ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16));
00629       ip6hdr->dest.addr[1] = 0;
00630       ip6hdr->dest.addr[2] = 0;
00631       ip6hdr->dest.addr[3] = lwip_htonl((lowpan6_buffer[lowpan6_offset] << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset + 2]);
00632       lowpan6_offset += 3;
00633     } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
00634       /* DAM = 11, copy 1 byte (8bits) */
00635       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 11, dst address form (8bits): ff02::00XX\n"));
00636       ip6hdr->dest.addr[0] = PP_HTONL(0xff020000UL);
00637       ip6hdr->dest.addr[1] = 0;
00638       ip6hdr->dest.addr[2] = 0;
00639       ip6hdr->dest.addr[3] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
00640     }
00641 
00642   } else {
00643     /* no Multicast (M=0) */
00644     if (lowpan6_buffer[1] & 0x04) {
00645       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAC == 1, stateful compression\n"));
00646       /* Stateful destination compression */
00647       /* Set prefix from context info */
00648       if (lowpan6_buffer[1] & 0x80) {
00649         i = lowpan6_buffer[2] & 0x0f;
00650       } else {
00651         i = 0;
00652       }
00653       if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
00654         /* Error */
00655         return ERR_VAL;
00656       }
00657 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
00658       ip6hdr->dest.addr[0] = lowpan6_contexts[i].addr[0];
00659       ip6hdr->dest.addr[1] = lowpan6_contexts[i].addr[1];
00660 #endif
00661     } else {
00662       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAC == 0, stateless compression, setting link local prefix\n"));
00663       /* Link local address compression */
00664       ip6hdr->dest.addr[0] = PP_HTONL(0xfe800000UL);
00665       ip6hdr->dest.addr[1] = 0;
00666     }
00667 
00668     /* M=0, DAC=0, determining destination address length via DAM=xx */
00669     if ((lowpan6_buffer[1] & 0x03) == 0x00) {
00670       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 00, no dst compression, fetching 128bits inline"));
00671       /* DAM=00, copy full address */
00672       MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
00673       lowpan6_offset += 16;
00674     } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
00675       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 01, dst compression, 64bits inline\n"));
00676       /* DAM=01, copy 64 inline bits, increase offset */
00677       MEMCPY(&ip6hdr->dest.addr[2], lowpan6_buffer + lowpan6_offset, 8);
00678       lowpan6_offset += 8;
00679     } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
00680       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 01, dst compression, 16bits inline\n"));
00681       /* DAM=10, copy 16 inline bits, increase offset */
00682       ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
00683       ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]);
00684       lowpan6_offset += 2;
00685     } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
00686       /* DAM=11, no bits available, use other headers (not done here) */
00687       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG,("DAM == 01, dst compression, 0bits inline, using other headers\n"));
00688       if (dest->addr_len == 2) {
00689         ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
00690         ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (dest->addr[0] << 8) | dest->addr[1]);
00691       } else if (dest->addr_len == 8) {
00692         ip6hdr->dest.addr[2] = lwip_htonl(((dest->addr[0] ^ 2) << 24) | (dest->addr[1] << 16) | dest->addr[2] << 8 | dest->addr[3]);
00693         ip6hdr->dest.addr[3] = lwip_htonl((dest->addr[4] << 24) | (dest->addr[5] << 16) | dest->addr[6] << 8 | dest->addr[7]);
00694       } else {
00695         /* invalid destination address length */
00696         LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Invalid destination address length\n"));
00697         return ERR_VAL;
00698       }
00699     }
00700   }
00701 
00702 
00703   /* Next Header Compression (NHC) decoding? */
00704   if (lowpan6_buffer[0] & 0x04) {
00705     LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NHC decoding\n"));
00706 #if LWIP_UDP
00707     if ((lowpan6_buffer[lowpan6_offset] & 0xf8) == 0xf0) {
00708       /* NHC: UDP */
00709       struct udp_hdr *udphdr;
00710       LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NHC: UDP\n"));
00711 
00712       /* UDP compression */
00713       IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_UDP);
00714       udphdr = (struct udp_hdr *)((u8_t *)decomp_buffer + ip6_offset);
00715       if (decomp_bufsize < IP6_HLEN + UDP_HLEN) {
00716         return ERR_MEM;
00717       }
00718 
00719       /* Checksum decompression */
00720       if (lowpan6_buffer[lowpan6_offset] & 0x04) {
00721         /* @todo support checksum decompress */
00722         LWIP_DEBUGF(LWIP_DBG_ON, ("NHC: UDP chechsum decompression UNSUPPORTED\n"));
00723         return ERR_VAL;
00724       }
00725 
00726       /* Decompress ports, according to RFC4944 */
00727       i = lowpan6_buffer[lowpan6_offset++] & 0x03;
00728       if (i == 0) {
00729         udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
00730         udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 2] << 8 | lowpan6_buffer[lowpan6_offset + 3]);
00731         lowpan6_offset += 4;
00732       } else if (i == 0x01) {
00733         udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
00734         udphdr->dest = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset + 2]);
00735         lowpan6_offset += 3;
00736       } else if (i == 0x02) {
00737         udphdr->src = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset]);
00738         udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 1] << 8 | lowpan6_buffer[lowpan6_offset + 2]);
00739         lowpan6_offset += 3;
00740       } else if (i == 0x03) {
00741         udphdr->src = lwip_htons(0xf0b0 | ((lowpan6_buffer[lowpan6_offset] >> 4) & 0x0f));
00742         udphdr->dest = lwip_htons(0xf0b0 | (lowpan6_buffer[lowpan6_offset] & 0x0f));
00743         lowpan6_offset += 1;
00744       }
00745 
00746       udphdr->chksum = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
00747       lowpan6_offset += 2;
00748       ip6_offset += UDP_HLEN;
00749       if (datagram_size == 0) {
00750         datagram_size = compressed_size - lowpan6_offset + ip6_offset;
00751       }
00752       udphdr->len = lwip_htons(datagram_size - IP6_HLEN);
00753 
00754     } else
00755 #endif /* LWIP_UDP */
00756     {
00757       LWIP_DEBUGF(LWIP_DBG_ON,("NHC: unsupported protocol!\n"));
00758       /* @todo support NHC other than UDP */
00759       return ERR_VAL;
00760     }
00761   }
00762   if (datagram_size == 0) {
00763     datagram_size = compressed_size - lowpan6_offset + ip6_offset;
00764   }
00765   /* Infer IPv6 payload length for header */
00766   IP6H_PLEN_SET(ip6hdr, datagram_size - IP6_HLEN);
00767 
00768   if (lowpan6_offset > lowpan6_bufsize) {
00769     /* input buffer overflow */
00770     return ERR_VAL;
00771   }
00772   *hdr_size_comp = lowpan6_offset;
00773   *hdr_size_decomp = ip6_offset;
00774 
00775   return ERR_OK;
00776 }
00777 
00778 struct pbuf *
00779 lowpan6_decompress(struct pbuf *p, u16_t datagram_size, ip6_addr_t *lowpan6_contexts,
00780                    struct lowpan6_link_addr *src, struct lowpan6_link_addr *dest)
00781 {
00782   struct pbuf *q;
00783   u16_t lowpan6_offset, ip6_offset;
00784   err_t err;
00785 
00786 #if LWIP_UDP
00787 #define UDP_HLEN_ALLOC UDP_HLEN
00788 #else
00789 #define UDP_HLEN_ALLOC 0
00790 #endif
00791 
00792   /* Allocate a buffer for decompression. This buffer will be too big and will be
00793      trimmed once the final size is known. */
00794   q = pbuf_alloc(PBUF_IP, p->len + IP6_HLEN + UDP_HLEN_ALLOC, PBUF_POOL);
00795   if (q == NULL) {
00796     pbuf_free(p);
00797     return NULL;
00798   }
00799   if (q->len < IP6_HLEN + UDP_HLEN_ALLOC) {
00800     /* The headers need to fit into the first pbuf */
00801     pbuf_free(p);
00802     pbuf_free(q);
00803     return NULL;
00804   }
00805 
00806   /* Decompress the IPv6 (and possibly UDP) header(s) into the new pbuf */
00807   err = lowpan6_decompress_hdr((u8_t *)p->payload, p->len, (u8_t *)q->payload, q->len,
00808     &lowpan6_offset, &ip6_offset, datagram_size, p->tot_len, lowpan6_contexts, src, dest);
00809   if (err != ERR_OK) {
00810     pbuf_free(p);
00811     pbuf_free(q);
00812     return NULL;
00813   }
00814 
00815   /* Now we copy leftover contents from p to q, so we have all L2 and L3 headers
00816      (and L4?) in a single pbuf: */
00817 
00818   /* Hide the compressed headers in p */
00819   pbuf_remove_header(p, lowpan6_offset);
00820   /* Temporarily hide the headers in q... */
00821   pbuf_remove_header(q, ip6_offset);
00822   /* ... copy the rest of p into q... */
00823   pbuf_copy(q, p);
00824   /* ... and reveal the headers again... */
00825   pbuf_add_header_force(q, ip6_offset);
00826   /* ... trim the pbuf to its correct size... */
00827   pbuf_realloc(q, ip6_offset + p->len);
00828   /* ... and cat possibly remaining (data-only) pbufs */
00829   if (p->next != NULL) {
00830     pbuf_cat(q, p->next);
00831   }
00832   /* the original (first) pbuf can now be freed */
00833   p->next = NULL;
00834   pbuf_free(p);
00835 
00836   /* all done */
00837   return q;
00838 }
00839 
00840 #endif /* LWIP_6LOWPAN_IPHC */
00841 #endif /* LWIP_IPV6 */