1 /**
2 * @file
3 *
4 * Common 6LowPAN routines for IPv6. Uses ND tables for link-layer addressing. Fragments packets to 6LowPAN units.
5 *
6 * This implementation aims to conform to IEEE 802.15.4(-2015), RFC 4944 and RFC 6282.
7 * @todo: RFC 6775.
8 */
9
10 /*
11 * Copyright (c) 2015 Inico Technologies Ltd.
12 * All rights reserved.
13 *
14 * Redistribution and use in source and binary forms, with or without modification,
15 * are permitted provided that the following conditions are met:
16 *
17 * 1. Redistributions of source code must retain the above copyright notice,
18 * this list of conditions and the following disclaimer.
19 * 2. Redistributions in binary form must reproduce the above copyright notice,
20 * this list of conditions and the following disclaimer in the documentation
21 * and/or other materials provided with the distribution.
22 * 3. The name of the author may not be used to endorse or promote products
23 * derived from this software without specific prior written permission.
24 *
25 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
26 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
27 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
28 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
29 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
30 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
31 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
32 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
33 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
34 * OF SUCH DAMAGE.
35 *
36 * This file is part of the lwIP TCP/IP stack.
37 *
38 * Author: Ivan Delamer <delamer@inicotech.com>
39 *
40 *
41 * Please coordinate changes and requests with Ivan Delamer
42 * <delamer@inicotech.com>
43 */
44
45 /**
46 * @defgroup sixlowpan 6LoWPAN (RFC4944)
47 * @ingroup netifs
48 * 6LowPAN netif implementation
49 */
50
51 #include "netif/lowpan6_common.h"
52
53 #if LWIP_IPV6
54
55 #include "lwip/ip.h"
56 #include "lwip/pbuf.h"
57 #include "lwip/ip_addr.h"
58 #include "lwip/netif.h"
59 #include "lwip/udp.h"
60
61 #include <string.h>
62
63 /* Determine compression mode for unicast address. */
64 s8_t
lowpan6_get_address_mode(const ip6_addr_t * ip6addr,const struct lowpan6_link_addr * mac_addr)65 lowpan6_get_address_mode(const ip6_addr_t *ip6addr, const struct lowpan6_link_addr *mac_addr)
66 {
67 if (mac_addr->addr_len == 2) {
68 if ((ip6addr->addr[2] == (u32_t)PP_HTONL(0x000000ff)) &&
69 ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000))) {
70 if ((ip6addr->addr[3] & PP_HTONL(0x0000ffff)) == lwip_ntohl((mac_addr->addr[0] << 8) | mac_addr->addr[1])) {
71 return 3;
72 }
73 }
74 } else if (mac_addr->addr_len == 8) {
75 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])) &&
76 (ip6addr->addr[3] == lwip_ntohl((mac_addr->addr[4] << 24) | (mac_addr->addr[5] << 16) | mac_addr->addr[6] << 8 | mac_addr->addr[7]))) {
77 return 3;
78 }
79 }
80
81 if ((ip6addr->addr[2] == PP_HTONL(0x000000ffUL)) &&
82 ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000UL))) {
83 return 2;
84 }
85
86 return 1;
87 }
88
89 #if LWIP_6LOWPAN_IPHC
90
91 /* Determine compression mode for multicast address. */
92 static s8_t
lowpan6_get_address_mode_mc(const ip6_addr_t * ip6addr)93 lowpan6_get_address_mode_mc(const ip6_addr_t *ip6addr)
94 {
95 if ((ip6addr->addr[0] == PP_HTONL(0xff020000)) &&
96 (ip6addr->addr[1] == 0) &&
97 (ip6addr->addr[2] == 0) &&
98 ((ip6addr->addr[3] & PP_HTONL(0xffffff00)) == 0)) {
99 return 3;
100 } else if (((ip6addr->addr[0] & PP_HTONL(0xff00ffff)) == PP_HTONL(0xff000000)) &&
101 (ip6addr->addr[1] == 0)) {
102 if ((ip6addr->addr[2] == 0) &&
103 ((ip6addr->addr[3] & PP_HTONL(0xff000000)) == 0)) {
104 return 2;
105 } else if ((ip6addr->addr[2] & PP_HTONL(0xffffff00)) == 0) {
106 return 1;
107 }
108 }
109
110 return 0;
111 }
112
113 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
114 static s8_t
lowpan6_context_lookup(const ip6_addr_t * lowpan6_contexts,const ip6_addr_t * ip6addr)115 lowpan6_context_lookup(const ip6_addr_t *lowpan6_contexts, const ip6_addr_t *ip6addr)
116 {
117 s8_t i;
118
119 for (i = 0; i < LWIP_6LOWPAN_NUM_CONTEXTS; i++) {
120 if (ip6_addr_netcmp(&lowpan6_contexts[i], ip6addr)) {
121 return i;
122 }
123 }
124 return -1;
125 }
126 #endif /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */
127
128 /*
129 * Compress IPv6 and/or UDP headers.
130 * */
131 err_t
lowpan6_compress_headers(struct netif * netif,u8_t * inbuf,size_t inbuf_size,u8_t * outbuf,size_t outbuf_size,u8_t * lowpan6_header_len_out,u8_t * hidden_header_len_out,ip6_addr_t * lowpan6_contexts,const struct lowpan6_link_addr * src,const struct lowpan6_link_addr * dst)132 lowpan6_compress_headers(struct netif *netif, u8_t *inbuf, size_t inbuf_size, u8_t *outbuf, size_t outbuf_size,
133 u8_t *lowpan6_header_len_out, u8_t *hidden_header_len_out, ip6_addr_t *lowpan6_contexts,
134 const struct lowpan6_link_addr *src, const struct lowpan6_link_addr *dst)
135 {
136 u8_t *buffer, *inptr;
137 u8_t lowpan6_header_len;
138 u8_t hidden_header_len = 0;
139 s8_t i;
140 struct ip6_hdr *ip6hdr;
141 ip_addr_t ip6src, ip6dst;
142
143 LWIP_ASSERT("netif != NULL", netif != NULL);
144 LWIP_ASSERT("inbuf != NULL", inbuf != NULL);
145 LWIP_ASSERT("outbuf != NULL", outbuf != NULL);
146 LWIP_ASSERT("lowpan6_header_len_out != NULL", lowpan6_header_len_out != NULL);
147 LWIP_ASSERT("hidden_header_len_out != NULL", hidden_header_len_out != NULL);
148
149 /* Perform 6LowPAN IPv6 header compression according to RFC 6282 */
150 buffer = outbuf;
151 inptr = inbuf;
152
153 if (inbuf_size < IP6_HLEN) {
154 /* input buffer too short */
155 return ERR_VAL;
156 }
157 if (outbuf_size < IP6_HLEN) {
158 /* output buffer too short for worst case */
159 return ERR_MEM;
160 }
161
162 /* Point to ip6 header and align copies of src/dest addresses. */
163 ip6hdr = (struct ip6_hdr *)inptr;
164 ip_addr_copy_from_ip6_packed(ip6dst, ip6hdr->dest);
165 ip6_addr_assign_zone(ip_2_ip6(&ip6dst), IP6_UNKNOWN, netif);
166 ip_addr_copy_from_ip6_packed(ip6src, ip6hdr->src);
167 ip6_addr_assign_zone(ip_2_ip6(&ip6src), IP6_UNKNOWN, netif);
168 LWIP_UNUSED_ARG(netif);
169
170 /* Basic length of 6LowPAN header, set dispatch and clear fields. */
171 lowpan6_header_len = 2;
172 buffer[0] = 0x60;
173 buffer[1] = 0;
174
175 /* Determine whether there will be a Context Identifier Extension byte or not.
176 * If so, set it already. */
177 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
178 buffer[2] = 0;
179
180 i = lowpan6_context_lookup(lowpan6_contexts, ip_2_ip6(&ip6src));
181 if (i >= 0) {
182 /* Stateful source address compression. */
183 buffer[1] |= 0x40;
184 buffer[2] |= (i & 0x0f) << 4;
185 }
186
187 i = lowpan6_context_lookup(lowpan6_contexts, ip_2_ip6(&ip6dst));
188 if (i >= 0) {
189 /* Stateful destination address compression. */
190 buffer[1] |= 0x04;
191 buffer[2] |= i & 0x0f;
192 }
193
194 if (buffer[2] != 0x00) {
195 /* Context identifier extension byte is appended. */
196 buffer[1] |= 0x80;
197 lowpan6_header_len++;
198 }
199 #else /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */
200 LWIP_UNUSED_ARG(lowpan6_contexts);
201 #endif /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */
202
203 /* Determine TF field: Traffic Class, Flow Label */
204 if (IP6H_FL(ip6hdr) == 0) {
205 /* Flow label is elided. */
206 buffer[0] |= 0x10;
207 if (IP6H_TC(ip6hdr) == 0) {
208 /* Traffic class (ECN+DSCP) elided too. */
209 buffer[0] |= 0x08;
210 } else {
211 /* Traffic class (ECN+DSCP) appended. */
212 buffer[lowpan6_header_len++] = IP6H_TC(ip6hdr);
213 }
214 } else {
215 if (((IP6H_TC(ip6hdr) & 0x3f) == 0)) {
216 /* DSCP portion of Traffic Class is elided, ECN and FL are appended (3 bytes) */
217 buffer[0] |= 0x08;
218
219 buffer[lowpan6_header_len] = IP6H_TC(ip6hdr) & 0xc0;
220 buffer[lowpan6_header_len++] |= (IP6H_FL(ip6hdr) >> 16) & 0x0f;
221 buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff;
222 buffer[lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff;
223 } else {
224 /* Traffic class and flow label are appended (4 bytes) */
225 buffer[lowpan6_header_len++] = IP6H_TC(ip6hdr);
226 buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 16) & 0x0f;
227 buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff;
228 buffer[lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff;
229 }
230 }
231
232 /* Compress NH?
233 * Only if UDP for now. @todo support other NH compression. */
234 if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) {
235 buffer[0] |= 0x04;
236 } else {
237 /* append nexth. */
238 buffer[lowpan6_header_len++] = IP6H_NEXTH(ip6hdr);
239 }
240
241 /* Compress hop limit? */
242 if (IP6H_HOPLIM(ip6hdr) == 255) {
243 buffer[0] |= 0x03;
244 } else if (IP6H_HOPLIM(ip6hdr) == 64) {
245 buffer[0] |= 0x02;
246 } else if (IP6H_HOPLIM(ip6hdr) == 1) {
247 buffer[0] |= 0x01;
248 } else {
249 /* append hop limit */
250 buffer[lowpan6_header_len++] = IP6H_HOPLIM(ip6hdr);
251 }
252
253 /* Compress source address */
254 if (((buffer[1] & 0x40) != 0) ||
255 (ip6_addr_islinklocal(ip_2_ip6(&ip6src)))) {
256 /* Context-based or link-local source address compression. */
257 i = lowpan6_get_address_mode(ip_2_ip6(&ip6src), src);
258 buffer[1] |= (i & 0x03) << 4;
259 if (i == 1) {
260 MEMCPY(buffer + lowpan6_header_len, inptr + 16, 8);
261 lowpan6_header_len += 8;
262 } else if (i == 2) {
263 MEMCPY(buffer + lowpan6_header_len, inptr + 22, 2);
264 lowpan6_header_len += 2;
265 }
266 } else if (ip6_addr_isany_val(*ip_2_ip6(&ip6src))) {
267 /* Special case: mark SAC and leave SAM=0 */
268 buffer[1] |= 0x40;
269 } else {
270 /* Append full address. */
271 MEMCPY(buffer + lowpan6_header_len, inptr + 8, 16);
272 lowpan6_header_len += 16;
273 }
274
275 /* Compress destination address */
276 if (ip6_addr_ismulticast(ip_2_ip6(&ip6dst))) {
277 /* @todo support stateful multicast address compression */
278
279 buffer[1] |= 0x08;
280
281 i = lowpan6_get_address_mode_mc(ip_2_ip6(&ip6dst));
282 buffer[1] |= i & 0x03;
283 if (i == 0) {
284 MEMCPY(buffer + lowpan6_header_len, inptr + 24, 16);
285 lowpan6_header_len += 16;
286 } else if (i == 1) {
287 buffer[lowpan6_header_len++] = inptr[25];
288 MEMCPY(buffer + lowpan6_header_len, inptr + 35, 5);
289 lowpan6_header_len += 5;
290 } else if (i == 2) {
291 buffer[lowpan6_header_len++] = inptr[25];
292 MEMCPY(buffer + lowpan6_header_len, inptr + 37, 3);
293 lowpan6_header_len += 3;
294 } else if (i == 3) {
295 buffer[lowpan6_header_len++] = (inptr)[39];
296 }
297 } else if (((buffer[1] & 0x04) != 0) ||
298 (ip6_addr_islinklocal(ip_2_ip6(&ip6dst)))) {
299 /* Context-based or link-local destination address compression. */
300 i = lowpan6_get_address_mode(ip_2_ip6(&ip6dst), dst);
301 buffer[1] |= i & 0x03;
302 if (i == 1) {
303 MEMCPY(buffer + lowpan6_header_len, inptr + 32, 8);
304 lowpan6_header_len += 8;
305 } else if (i == 2) {
306 MEMCPY(buffer + lowpan6_header_len, inptr + 38, 2);
307 lowpan6_header_len += 2;
308 }
309 } else {
310 /* Append full address. */
311 MEMCPY(buffer + lowpan6_header_len, inptr + 24, 16);
312 lowpan6_header_len += 16;
313 }
314
315 /* Move to payload. */
316 inptr += IP6_HLEN;
317 hidden_header_len += IP6_HLEN;
318
319 #if LWIP_UDP
320 /* Compress UDP header? */
321 if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) {
322 /* @todo support optional checksum compression */
323
324 if (inbuf_size < IP6_HLEN + UDP_HLEN) {
325 /* input buffer too short */
326 return ERR_VAL;
327 }
328 if (outbuf_size < (size_t)(hidden_header_len + 7)) {
329 /* output buffer too short for worst case */
330 return ERR_MEM;
331 }
332
333 buffer[lowpan6_header_len] = 0xf0;
334
335 /* determine port compression mode. */
336 if ((inptr[0] == 0xf0) && ((inptr[1] & 0xf0) == 0xb0) &&
337 (inptr[2] == 0xf0) && ((inptr[3] & 0xf0) == 0xb0)) {
338 /* Compress source and dest ports. */
339 buffer[lowpan6_header_len++] |= 0x03;
340 buffer[lowpan6_header_len++] = ((inptr[1] & 0x0f) << 4) | (inptr[3] & 0x0f);
341 } else if (inptr[0] == 0xf0) {
342 /* Compress source port. */
343 buffer[lowpan6_header_len++] |= 0x02;
344 buffer[lowpan6_header_len++] = inptr[1];
345 buffer[lowpan6_header_len++] = inptr[2];
346 buffer[lowpan6_header_len++] = inptr[3];
347 } else if (inptr[2] == 0xf0) {
348 /* Compress dest port. */
349 buffer[lowpan6_header_len++] |= 0x01;
350 buffer[lowpan6_header_len++] = inptr[0];
351 buffer[lowpan6_header_len++] = inptr[1];
352 buffer[lowpan6_header_len++] = inptr[3];
353 } else {
354 /* append full ports. */
355 lowpan6_header_len++;
356 buffer[lowpan6_header_len++] = inptr[0];
357 buffer[lowpan6_header_len++] = inptr[1];
358 buffer[lowpan6_header_len++] = inptr[2];
359 buffer[lowpan6_header_len++] = inptr[3];
360 }
361
362 /* elide length and copy checksum */
363 buffer[lowpan6_header_len++] = inptr[6];
364 buffer[lowpan6_header_len++] = inptr[7];
365
366 hidden_header_len += UDP_HLEN;
367 }
368 #endif /* LWIP_UDP */
369
370 *lowpan6_header_len_out = lowpan6_header_len;
371 *hidden_header_len_out = hidden_header_len;
372
373 return ERR_OK;
374 }
375
376 /** Decompress IPv6 and UDP headers compressed according to RFC 6282
377 *
378 * @param lowpan6_buffer compressed headers, first byte is the dispatch byte
379 * @param lowpan6_bufsize size of lowpan6_buffer (may include data after headers)
380 * @param decomp_buffer buffer where the decompressed headers are stored
381 * @param decomp_bufsize size of decomp_buffer
382 * @param hdr_size_comp returns the size of the compressed headers (skip to get to data)
383 * @param hdr_size_decomp returns the size of the decompressed headers (IPv6 + UDP)
384 * @param datagram_size datagram size from fragments or 0 if unfragmented
385 * @param compressed_size compressed datagram size (for unfragmented rx)
386 * @param lowpan6_contexts context addresses
387 * @param src source address of the outer layer, used for address compression
388 * @param dest destination address of the outer layer, used for address compression
389 * @return ERR_OK if decompression succeeded, an error otherwise
390 */
391 static err_t
lowpan6_decompress_hdr(u8_t * lowpan6_buffer,u16_t lowpan6_bufsize,u8_t * decomp_buffer,u16_t decomp_bufsize,u16_t * hdr_size_comp,u16_t * hdr_size_decomp,u16_t datagram_size,u16_t compressed_size,ip6_addr_t * lowpan6_contexts,struct lowpan6_link_addr * src,struct lowpan6_link_addr * dest)392 lowpan6_decompress_hdr(u8_t *lowpan6_buffer, u16_t lowpan6_bufsize,
393 u8_t *decomp_buffer, u16_t decomp_bufsize,
394 u16_t *hdr_size_comp, u16_t *hdr_size_decomp,
395 u16_t datagram_size, u16_t compressed_size,
396 ip6_addr_t *lowpan6_contexts,
397 struct lowpan6_link_addr *src, struct lowpan6_link_addr *dest)
398 {
399 u16_t lowpan6_offset;
400 struct ip6_hdr *ip6hdr;
401 s8_t i;
402 u32_t header_temp;
403 u16_t ip6_offset = IP6_HLEN;
404
405 LWIP_ASSERT("lowpan6_buffer != NULL", lowpan6_buffer != NULL);
406 LWIP_ASSERT("decomp_buffer != NULL", decomp_buffer != NULL);
407 LWIP_ASSERT("src != NULL", src != NULL);
408 LWIP_ASSERT("dest != NULL", dest != NULL);
409 LWIP_ASSERT("hdr_size_comp != NULL", hdr_size_comp != NULL);
410 LWIP_ASSERT("dehdr_size_decompst != NULL", hdr_size_decomp != NULL);
411
412 ip6hdr = (struct ip6_hdr *)decomp_buffer;
413 if (decomp_bufsize < IP6_HLEN) {
414 return ERR_MEM;
415 }
416
417 /* output the full compressed packet, if set in @see lowpan6_opts.h */
418 #if LWIP_LOWPAN6_IP_COMPRESSED_DEBUG
419 {
420 u16_t j;
421 LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("lowpan6_decompress_hdr: IP6 payload (compressed): \n"));
422 for (j = 0; j < lowpan6_bufsize; j++) {
423 if ((j % 4) == 0) {
424 LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("\n"));
425 }
426 LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("%2X ", lowpan6_buffer[j]));
427 }
428 LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("\np->len: %d", lowpan6_bufsize));
429 }
430 #endif
431
432 #define LOWPAN6_OFFSET_ADVANCE_CHECK(inc) \
433 LWIP_ERROR("lowpan6 offset would exceed bufsize!\n", \
434 (lowpan6_offset + (inc) <= lowpan6_bufsize), return ERR_VAL)
435
436 /* offset for inline IP headers (RFC 6282 ch3)*/
437 lowpan6_offset = 2;
438 /* if CID is set (context identifier), the context byte
439 * follows immediately after the header, so other IPHC fields are @+3 */
440 if (lowpan6_buffer[1] & 0x80) {
441 LOWPAN6_OFFSET_ADVANCE_CHECK(1);
442 lowpan6_offset++;
443 }
444
445 /* Set IPv6 version, traffic class and flow label. (RFC6282, ch 3.1.1.)*/
446 if ((lowpan6_buffer[0] & 0x18) == 0x00) {
447 LOWPAN6_OFFSET_ADVANCE_CHECK(4);
448 header_temp = ((lowpan6_buffer[lowpan6_offset+1] & 0x0f) << 16) | \
449 (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset+3];
450 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 00, ECN: 0x%"X8_F", Flowlabel+DSCP: 0x%8"X32_F"\n", \
451 lowpan6_buffer[lowpan6_offset],header_temp));
452 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset], header_temp);
453 /* increase offset, processed 4 bytes here:
454 * TF=00: ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)*/
455 lowpan6_offset += 4;
456 } else if ((lowpan6_buffer[0] & 0x18) == 0x08) {
457 LOWPAN6_OFFSET_ADVANCE_CHECK(3);
458 header_temp = ((lowpan6_buffer[lowpan6_offset] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset+2];
459 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 01, ECN: 0x%"X8_F", Flowlabel: 0x%2"X32_F", DSCP ignored\n", \
460 lowpan6_buffer[lowpan6_offset] & 0xc0,header_temp));
461 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset] & 0xc0, header_temp);
462 /* increase offset, processed 3 bytes here:
463 * TF=01: ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided.*/
464 lowpan6_offset += 3;
465 } else if ((lowpan6_buffer[0] & 0x18) == 0x10) {
466 LOWPAN6_OFFSET_ADVANCE_CHECK(1);
467 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 10, DCSP+ECN: 0x%"X8_F", Flowlabel ignored\n", lowpan6_buffer[lowpan6_offset]));
468 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset],0);
469 /* increase offset, processed 1 byte here:
470 * ECN + DSCP (1 byte), Flow Label is elided.*/
471 lowpan6_offset += 1;
472 } else if ((lowpan6_buffer[0] & 0x18) == 0x18) {
473 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 11, DCSP/ECN & Flowlabel ignored\n"));
474 /* don't increase offset, no bytes processed here */
475 IP6H_VTCFL_SET(ip6hdr, 6, 0, 0);
476 }
477
478 /* Set Next Header (NH) */
479 if ((lowpan6_buffer[0] & 0x04) == 0x00) {
480 /* 0: full next header byte carried inline (increase offset)*/
481 LOWPAN6_OFFSET_ADVANCE_CHECK(1);
482 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NH: 0x%2X\n", lowpan6_buffer[lowpan6_offset+1]));
483 IP6H_NEXTH_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
484 } else {
485 /* 1: NH compression, LOWPAN_NHC (RFC6282, ch 4.1) */
486 /* We should fill this later with NHC decoding */
487 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NH: skipped, later done with NHC\n"));
488 IP6H_NEXTH_SET(ip6hdr, 0);
489 }
490
491 /* Set Hop Limit, either carried inline or 3 different hops (1,64,255) */
492 if ((lowpan6_buffer[0] & 0x03) == 0x00) {
493 LOWPAN6_OFFSET_ADVANCE_CHECK(1);
494 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: full value: %d\n", lowpan6_buffer[lowpan6_offset+1]));
495 IP6H_HOPLIM_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
496 } else if ((lowpan6_buffer[0] & 0x03) == 0x01) {
497 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: compressed: 1\n"));
498 IP6H_HOPLIM_SET(ip6hdr, 1);
499 } else if ((lowpan6_buffer[0] & 0x03) == 0x02) {
500 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: compressed: 64\n"));
501 IP6H_HOPLIM_SET(ip6hdr, 64);
502 } else if ((lowpan6_buffer[0] & 0x03) == 0x03) {
503 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: compressed: 255\n"));
504 IP6H_HOPLIM_SET(ip6hdr, 255);
505 }
506
507 /* Source address decoding. */
508 if ((lowpan6_buffer[1] & 0x40) == 0x00) {
509 /* Source address compression (SAC) = 0 -> stateless compression */
510 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAC == 0, no context byte\n"));
511 /* Stateless compression */
512 if ((lowpan6_buffer[1] & 0x30) == 0x00) {
513 LOWPAN6_OFFSET_ADVANCE_CHECK(16);
514 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 00, no src compression, fetching 128bits inline\n"));
515 /* copy full address, increase offset by 16 Bytes */
516 MEMCPY(&ip6hdr->src.addr[0], lowpan6_buffer + lowpan6_offset, 16);
517 lowpan6_offset += 16;
518 } else if ((lowpan6_buffer[1] & 0x30) == 0x10) {
519 LOWPAN6_OFFSET_ADVANCE_CHECK(8);
520 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 01, src compression, 64bits inline\n"));
521 /* set 64 bits to link local */
522 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
523 ip6hdr->src.addr[1] = 0;
524 /* copy 8 Bytes, increase offset */
525 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
526 lowpan6_offset += 8;
527 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
528 LOWPAN6_OFFSET_ADVANCE_CHECK(2);
529 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 10, src compression, 16bits inline\n"));
530 /* set 96 bits to link local */
531 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
532 ip6hdr->src.addr[1] = 0;
533 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
534 /* extract remaining 16bits from inline bytes, increase offset */
535 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) |
536 lowpan6_buffer[lowpan6_offset + 1]);
537 lowpan6_offset += 2;
538 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
539 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 11, src compression, 0bits inline, using other headers\n"));
540 /* no information avalaible, using other layers, see RFC6282 ch 3.2.2 */
541 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
542 ip6hdr->src.addr[1] = 0;
543 if (src->addr_len == 2) {
544 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
545 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
546 } else if (src->addr_len == 8) {
547 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) |
548 (src->addr[2] << 8) | src->addr[3]);
549 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) |
550 (src->addr[6] << 8) | src->addr[7]);
551 } else {
552 /* invalid source address length */
553 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Invalid source address length\n"));
554 return ERR_VAL;
555 }
556 }
557 } else {
558 /* Source address compression (SAC) = 1 -> stateful/context-based compression */
559 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAC == 1, additional context byte\n"));
560 if ((lowpan6_buffer[1] & 0x30) == 0x00) {
561 /* SAM=00, address=> :: (ANY) */
562 ip6hdr->src.addr[0] = 0;
563 ip6hdr->src.addr[1] = 0;
564 ip6hdr->src.addr[2] = 0;
565 ip6hdr->src.addr[3] = 0;
566 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 00, context compression, ANY (::)\n"));
567 } else {
568 /* Set prefix from context info */
569 if (lowpan6_buffer[1] & 0x80) {
570 i = (lowpan6_buffer[2] >> 4) & 0x0f;
571 } else {
572 i = 0;
573 }
574 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
575 /* Error */
576 return ERR_VAL;
577 }
578 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
579 ip6hdr->src.addr[0] = lowpan6_contexts[i].addr[0];
580 ip6hdr->src.addr[1] = lowpan6_contexts[i].addr[1];
581 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == xx, context compression found @%d: %8"X32_F", %8"X32_F"\n", (int)i, ip6hdr->src.addr[0], ip6hdr->src.addr[1]));
582 #else
583 LWIP_UNUSED_ARG(lowpan6_contexts);
584 #endif
585 }
586
587 /* determine further address bits */
588 if ((lowpan6_buffer[1] & 0x30) == 0x10) {
589 /* SAM=01, load additional 64bits */
590 LOWPAN6_OFFSET_ADVANCE_CHECK(8);
591 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
592 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 01, context compression, 64bits inline\n"));
593 lowpan6_offset += 8;
594 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
595 /* SAM=01, load additional 16bits */
596 LOWPAN6_OFFSET_ADVANCE_CHECK(2);
597 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
598 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]);
599 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 10, context compression, 16bits inline\n"));
600 lowpan6_offset += 2;
601 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
602 /* SAM=11, address is fully elided, load from other layers */
603 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 11, context compression, 0bits inline, using other headers\n"));
604 if (src->addr_len == 2) {
605 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
606 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
607 } else if (src->addr_len == 8) {
608 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | (src->addr[2] << 8) | src->addr[3]);
609 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) | (src->addr[6] << 8) | src->addr[7]);
610 } else {
611 /* invalid source address length */
612 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Invalid source address length\n"));
613 return ERR_VAL;
614 }
615 }
616 }
617
618 /* Destination address decoding. */
619 if (lowpan6_buffer[1] & 0x08) {
620 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("M=1: multicast\n"));
621 /* Multicast destination */
622 if (lowpan6_buffer[1] & 0x04) {
623 LWIP_DEBUGF(LWIP_DBG_ON,("DAC == 1, context multicast: unsupported!!!\n"));
624 /* @todo support stateful multicast addressing */
625 return ERR_VAL;
626 }
627
628 if ((lowpan6_buffer[1] & 0x03) == 0x00) {
629 /* DAM = 00, copy full address (128bits) */
630 LOWPAN6_OFFSET_ADVANCE_CHECK(16);
631 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 00, no dst compression, fetching 128bits inline\n"));
632 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
633 lowpan6_offset += 16;
634 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
635 /* DAM = 01, copy 4 bytes (32bits) */
636 LOWPAN6_OFFSET_ADVANCE_CHECK(4);
637 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 01, dst address form (48bits): ffXX::00XX:XXXX:XXXX\n"));
638 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16));
639 ip6hdr->dest.addr[1] = 0;
640 ip6hdr->dest.addr[2] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
641 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]);
642 lowpan6_offset += 4;
643 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
644 /* DAM = 10, copy 3 bytes (24bits) */
645 LOWPAN6_OFFSET_ADVANCE_CHECK(3);
646 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 10, dst address form (32bits): ffXX::00XX:XXXX\n"));
647 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16));
648 ip6hdr->dest.addr[1] = 0;
649 ip6hdr->dest.addr[2] = 0;
650 ip6hdr->dest.addr[3] = lwip_htonl((lowpan6_buffer[lowpan6_offset] << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset + 2]);
651 lowpan6_offset += 3;
652 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
653 /* DAM = 11, copy 1 byte (8bits) */
654 LOWPAN6_OFFSET_ADVANCE_CHECK(1);
655 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 11, dst address form (8bits): ff02::00XX\n"));
656 ip6hdr->dest.addr[0] = PP_HTONL(0xff020000UL);
657 ip6hdr->dest.addr[1] = 0;
658 ip6hdr->dest.addr[2] = 0;
659 ip6hdr->dest.addr[3] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
660 }
661
662 } else {
663 /* no Multicast (M=0) */
664 if (lowpan6_buffer[1] & 0x04) {
665 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAC == 1, stateful compression\n"));
666 /* Stateful destination compression */
667 /* Set prefix from context info */
668 if (lowpan6_buffer[1] & 0x80) {
669 i = lowpan6_buffer[2] & 0x0f;
670 } else {
671 i = 0;
672 }
673 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
674 /* Error */
675 return ERR_VAL;
676 }
677 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
678 ip6hdr->dest.addr[0] = lowpan6_contexts[i].addr[0];
679 ip6hdr->dest.addr[1] = lowpan6_contexts[i].addr[1];
680 #endif
681 } else {
682 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAC == 0, stateless compression, setting link local prefix\n"));
683 /* Link local address compression */
684 ip6hdr->dest.addr[0] = PP_HTONL(0xfe800000UL);
685 ip6hdr->dest.addr[1] = 0;
686 }
687
688 /* M=0, DAC=0, determining destination address length via DAM=xx */
689 if ((lowpan6_buffer[1] & 0x03) == 0x00) {
690 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 00, no dst compression, fetching 128bits inline"));
691 /* DAM=00, copy full address */
692 LOWPAN6_OFFSET_ADVANCE_CHECK(16);
693 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
694 lowpan6_offset += 16;
695 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
696 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 01, dst compression, 64bits inline\n"));
697 /* DAM=01, copy 64 inline bits, increase offset */
698 LOWPAN6_OFFSET_ADVANCE_CHECK(8);
699 MEMCPY(&ip6hdr->dest.addr[2], lowpan6_buffer + lowpan6_offset, 8);
700 lowpan6_offset += 8;
701 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
702 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 01, dst compression, 16bits inline\n"));
703 /* DAM=10, copy 16 inline bits, increase offset */
704 LOWPAN6_OFFSET_ADVANCE_CHECK(2);
705 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
706 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]);
707 lowpan6_offset += 2;
708 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
709 /* DAM=11, no bits available, use other headers (not done here) */
710 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG,("DAM == 01, dst compression, 0bits inline, using other headers\n"));
711 if (dest->addr_len == 2) {
712 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
713 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (dest->addr[0] << 8) | dest->addr[1]);
714 } else if (dest->addr_len == 8) {
715 ip6hdr->dest.addr[2] = lwip_htonl(((dest->addr[0] ^ 2) << 24) | (dest->addr[1] << 16) | dest->addr[2] << 8 | dest->addr[3]);
716 ip6hdr->dest.addr[3] = lwip_htonl((dest->addr[4] << 24) | (dest->addr[5] << 16) | dest->addr[6] << 8 | dest->addr[7]);
717 } else {
718 /* invalid destination address length */
719 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Invalid destination address length\n"));
720 return ERR_VAL;
721 }
722 }
723 }
724
725
726 /* Next Header Compression (NHC) decoding? */
727 if (lowpan6_buffer[0] & 0x04) {
728 LOWPAN6_OFFSET_ADVANCE_CHECK(1);
729 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NHC decoding\n"));
730 #if LWIP_UDP
731 if ((lowpan6_buffer[lowpan6_offset] & 0xf8) == 0xf0) {
732 /* NHC: UDP */
733 struct udp_hdr *udphdr;
734 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NHC: UDP\n"));
735
736 /* UDP compression */
737 IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_UDP);
738 udphdr = (struct udp_hdr *)((u8_t *)decomp_buffer + ip6_offset);
739 if (decomp_bufsize < IP6_HLEN + UDP_HLEN) {
740 return ERR_MEM;
741 }
742
743 /* Checksum decompression */
744 if (lowpan6_buffer[lowpan6_offset] & 0x04) {
745 /* @todo support checksum decompress */
746 LWIP_DEBUGF(LWIP_DBG_ON, ("NHC: UDP chechsum decompression UNSUPPORTED\n"));
747 return ERR_VAL;
748 }
749
750 /* Decompress ports, according to RFC4944 */
751 i = lowpan6_buffer[lowpan6_offset++] & 0x03;
752 if (i == 0) {
753 LOWPAN6_OFFSET_ADVANCE_CHECK(4);
754 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
755 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 2] << 8 | lowpan6_buffer[lowpan6_offset + 3]);
756 lowpan6_offset += 4;
757 } else if (i == 0x01) {
758 LOWPAN6_OFFSET_ADVANCE_CHECK(3);
759 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
760 udphdr->dest = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset + 2]);
761 lowpan6_offset += 3;
762 } else if (i == 0x02) {
763 LOWPAN6_OFFSET_ADVANCE_CHECK(3);
764 udphdr->src = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset]);
765 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 1] << 8 | lowpan6_buffer[lowpan6_offset + 2]);
766 lowpan6_offset += 3;
767 } else if (i == 0x03) {
768 LOWPAN6_OFFSET_ADVANCE_CHECK(1);
769 udphdr->src = lwip_htons(0xf0b0 | ((lowpan6_buffer[lowpan6_offset] >> 4) & 0x0f));
770 udphdr->dest = lwip_htons(0xf0b0 | (lowpan6_buffer[lowpan6_offset] & 0x0f));
771 lowpan6_offset += 1;
772 }
773
774 LOWPAN6_OFFSET_ADVANCE_CHECK(2);
775 udphdr->chksum = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
776 lowpan6_offset += 2;
777 ip6_offset += UDP_HLEN;
778 if (datagram_size == 0) {
779 datagram_size = compressed_size - lowpan6_offset + ip6_offset;
780 }
781 udphdr->len = lwip_htons(datagram_size - IP6_HLEN);
782
783 } else
784 #endif /* LWIP_UDP */
785 {
786 LWIP_DEBUGF(LWIP_DBG_ON,("NHC: unsupported protocol!\n"));
787 /* @todo support NHC other than UDP */
788 return ERR_VAL;
789 }
790 }
791 if (datagram_size == 0) {
792 datagram_size = compressed_size - lowpan6_offset + ip6_offset;
793 }
794 /* Infer IPv6 payload length for header */
795 IP6H_PLEN_SET(ip6hdr, datagram_size - IP6_HLEN);
796
797 #undef LOWPAN6_OFFSET_ADVANCE_CHECK
798
799 *hdr_size_comp = lowpan6_offset;
800 *hdr_size_decomp = ip6_offset;
801
802 return ERR_OK;
803 }
804
805 struct pbuf *
lowpan6_decompress(struct pbuf * p,u16_t datagram_size,ip6_addr_t * lowpan6_contexts,struct lowpan6_link_addr * src,struct lowpan6_link_addr * dest)806 lowpan6_decompress(struct pbuf *p, u16_t datagram_size, ip6_addr_t *lowpan6_contexts,
807 struct lowpan6_link_addr *src, struct lowpan6_link_addr *dest)
808 {
809 struct pbuf *q;
810 u16_t lowpan6_offset, ip6_offset;
811 err_t err;
812
813 #if LWIP_UDP
814 #define UDP_HLEN_ALLOC UDP_HLEN
815 #else
816 #define UDP_HLEN_ALLOC 0
817 #endif
818
819 /* Allocate a buffer for decompression. This buffer will be too big and will be
820 trimmed once the final size is known. */
821 q = pbuf_alloc(PBUF_IP, p->len + IP6_HLEN + UDP_HLEN_ALLOC, PBUF_POOL);
822 if (q == NULL) {
823 pbuf_free(p);
824 return NULL;
825 }
826 if (q->len < IP6_HLEN + UDP_HLEN_ALLOC) {
827 /* The headers need to fit into the first pbuf */
828 pbuf_free(p);
829 pbuf_free(q);
830 return NULL;
831 }
832
833 /* Decompress the IPv6 (and possibly UDP) header(s) into the new pbuf */
834 err = lowpan6_decompress_hdr((u8_t *)p->payload, p->len, (u8_t *)q->payload, q->len,
835 &lowpan6_offset, &ip6_offset, datagram_size, p->tot_len, lowpan6_contexts, src, dest);
836 if (err != ERR_OK) {
837 pbuf_free(p);
838 pbuf_free(q);
839 return NULL;
840 }
841
842 /* Now we copy leftover contents from p to q, so we have all L2 and L3 headers
843 (and L4?) in a single pbuf: */
844
845 /* Hide the compressed headers in p */
846 pbuf_remove_header(p, lowpan6_offset);
847 /* Temporarily hide the headers in q... */
848 pbuf_remove_header(q, ip6_offset);
849 /* ... copy the rest of p into q... */
850 pbuf_copy(q, p);
851 /* ... and reveal the headers again... */
852 pbuf_add_header_force(q, ip6_offset);
853 /* ... trim the pbuf to its correct size... */
854 pbuf_realloc(q, ip6_offset + p->len);
855 /* ... and cat possibly remaining (data-only) pbufs */
856 if (p->next != NULL) {
857 pbuf_cat(q, p->next);
858 }
859 /* the original (first) pbuf can now be freed */
860 p->next = NULL;
861 pbuf_free(p);
862
863 /* all done */
864 return q;
865 }
866
867 #endif /* LWIP_6LOWPAN_IPHC */
868 #endif /* LWIP_IPV6 */
869