source: azure_iot_hub_f767zi/trunk/asp_baseplatform/lwip/lwip-2.1.2/src/netif/lowpan6_common.c@ 457

Last change on this file since 457 was 457, checked in by coas-nagasima, 4 years ago

ファイルを追加
  • Property svn:eol-style set to native
  • Property svn:mime-type set to text/x-csrc;charset=UTF-8
File size: 33.0 KB
Line 
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. */
64s8_t
65lowpan6_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. */
92static s8_t
93lowpan6_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
114static s8_t
115lowpan6_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 * */
131err_t
132lowpan6_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
169 /* Basic length of 6LowPAN header, set dispatch and clear fields. */
170 lowpan6_header_len = 2;
171 buffer[0] = 0x60;
172 buffer[1] = 0;
173
174 /* Determine whether there will be a Context Identifier Extension byte or not.
175 * If so, set it already. */
176#if LWIP_6LOWPAN_NUM_CONTEXTS > 0
177 buffer[2] = 0;
178
179 i = lowpan6_context_lookup(lowpan6_contexts, ip_2_ip6(&ip6src));
180 if (i >= 0) {
181 /* Stateful source address compression. */
182 buffer[1] |= 0x40;
183 buffer[2] |= (i & 0x0f) << 4;
184 }
185
186 i = lowpan6_context_lookup(lowpan6_contexts, ip_2_ip6(&ip6dst));
187 if (i >= 0) {
188 /* Stateful destination address compression. */
189 buffer[1] |= 0x04;
190 buffer[2] |= i & 0x0f;
191 }
192
193 if (buffer[2] != 0x00) {
194 /* Context identifier extension byte is appended. */
195 buffer[1] |= 0x80;
196 lowpan6_header_len++;
197 }
198#else /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */
199 LWIP_UNUSED_ARG(lowpan6_contexts);
200#endif /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */
201
202 /* Determine TF field: Traffic Class, Flow Label */
203 if (IP6H_FL(ip6hdr) == 0) {
204 /* Flow label is elided. */
205 buffer[0] |= 0x10;
206 if (IP6H_TC(ip6hdr) == 0) {
207 /* Traffic class (ECN+DSCP) elided too. */
208 buffer[0] |= 0x08;
209 } else {
210 /* Traffic class (ECN+DSCP) appended. */
211 buffer[lowpan6_header_len++] = IP6H_TC(ip6hdr);
212 }
213 } else {
214 if (((IP6H_TC(ip6hdr) & 0x3f) == 0)) {
215 /* DSCP portion of Traffic Class is elided, ECN and FL are appended (3 bytes) */
216 buffer[0] |= 0x08;
217
218 buffer[lowpan6_header_len] = IP6H_TC(ip6hdr) & 0xc0;
219 buffer[lowpan6_header_len++] |= (IP6H_FL(ip6hdr) >> 16) & 0x0f;
220 buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff;
221 buffer[lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff;
222 } else {
223 /* Traffic class and flow label are appended (4 bytes) */
224 buffer[lowpan6_header_len++] = IP6H_TC(ip6hdr);
225 buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 16) & 0x0f;
226 buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff;
227 buffer[lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff;
228 }
229 }
230
231 /* Compress NH?
232 * Only if UDP for now. @todo support other NH compression. */
233 if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) {
234 buffer[0] |= 0x04;
235 } else {
236 /* append nexth. */
237 buffer[lowpan6_header_len++] = IP6H_NEXTH(ip6hdr);
238 }
239
240 /* Compress hop limit? */
241 if (IP6H_HOPLIM(ip6hdr) == 255) {
242 buffer[0] |= 0x03;
243 } else if (IP6H_HOPLIM(ip6hdr) == 64) {
244 buffer[0] |= 0x02;
245 } else if (IP6H_HOPLIM(ip6hdr) == 1) {
246 buffer[0] |= 0x01;
247 } else {
248 /* append hop limit */
249 buffer[lowpan6_header_len++] = IP6H_HOPLIM(ip6hdr);
250 }
251
252 /* Compress source address */
253 if (((buffer[1] & 0x40) != 0) ||
254 (ip6_addr_islinklocal(ip_2_ip6(&ip6src)))) {
255 /* Context-based or link-local source address compression. */
256 i = lowpan6_get_address_mode(ip_2_ip6(&ip6src), src);
257 buffer[1] |= (i & 0x03) << 4;
258 if (i == 1) {
259 MEMCPY(buffer + lowpan6_header_len, inptr + 16, 8);
260 lowpan6_header_len += 8;
261 } else if (i == 2) {
262 MEMCPY(buffer + lowpan6_header_len, inptr + 22, 2);
263 lowpan6_header_len += 2;
264 }
265 } else if (ip6_addr_isany(ip_2_ip6(&ip6src))) {
266 /* Special case: mark SAC and leave SAM=0 */
267 buffer[1] |= 0x40;
268 } else {
269 /* Append full address. */
270 MEMCPY(buffer + lowpan6_header_len, inptr + 8, 16);
271 lowpan6_header_len += 16;
272 }
273
274 /* Compress destination address */
275 if (ip6_addr_ismulticast(ip_2_ip6(&ip6dst))) {
276 /* @todo support stateful multicast address compression */
277
278 buffer[1] |= 0x08;
279
280 i = lowpan6_get_address_mode_mc(ip_2_ip6(&ip6dst));
281 buffer[1] |= i & 0x03;
282 if (i == 0) {
283 MEMCPY(buffer + lowpan6_header_len, inptr + 24, 16);
284 lowpan6_header_len += 16;
285 } else if (i == 1) {
286 buffer[lowpan6_header_len++] = inptr[25];
287 MEMCPY(buffer + lowpan6_header_len, inptr + 35, 5);
288 lowpan6_header_len += 5;
289 } else if (i == 2) {
290 buffer[lowpan6_header_len++] = inptr[25];
291 MEMCPY(buffer + lowpan6_header_len, inptr + 37, 3);
292 lowpan6_header_len += 3;
293 } else if (i == 3) {
294 buffer[lowpan6_header_len++] = (inptr)[39];
295 }
296 } else if (((buffer[1] & 0x04) != 0) ||
297 (ip6_addr_islinklocal(ip_2_ip6(&ip6dst)))) {
298 /* Context-based or link-local destination address compression. */
299 i = lowpan6_get_address_mode(ip_2_ip6(&ip6dst), dst);
300 buffer[1] |= i & 0x03;
301 if (i == 1) {
302 MEMCPY(buffer + lowpan6_header_len, inptr + 32, 8);
303 lowpan6_header_len += 8;
304 } else if (i == 2) {
305 MEMCPY(buffer + lowpan6_header_len, inptr + 38, 2);
306 lowpan6_header_len += 2;
307 }
308 } else {
309 /* Append full address. */
310 MEMCPY(buffer + lowpan6_header_len, inptr + 24, 16);
311 lowpan6_header_len += 16;
312 }
313
314 /* Move to payload. */
315 inptr += IP6_HLEN;
316 hidden_header_len += IP6_HLEN;
317
318#if LWIP_UDP
319 /* Compress UDP header? */
320 if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) {
321 /* @todo support optional checksum compression */
322
323 if (inbuf_size < IP6_HLEN + UDP_HLEN) {
324 /* input buffer too short */
325 return ERR_VAL;
326 }
327 if (outbuf_size < (size_t)(hidden_header_len + 7)) {
328 /* output buffer too short for worst case */
329 return ERR_MEM;
330 }
331
332 buffer[lowpan6_header_len] = 0xf0;
333
334 /* determine port compression mode. */
335 if ((inptr[0] == 0xf0) && ((inptr[1] & 0xf0) == 0xb0) &&
336 (inptr[2] == 0xf0) && ((inptr[3] & 0xf0) == 0xb0)) {
337 /* Compress source and dest ports. */
338 buffer[lowpan6_header_len++] |= 0x03;
339 buffer[lowpan6_header_len++] = ((inptr[1] & 0x0f) << 4) | (inptr[3] & 0x0f);
340 } else if (inptr[0] == 0xf0) {
341 /* Compress source port. */
342 buffer[lowpan6_header_len++] |= 0x02;
343 buffer[lowpan6_header_len++] = inptr[1];
344 buffer[lowpan6_header_len++] = inptr[2];
345 buffer[lowpan6_header_len++] = inptr[3];
346 } else if (inptr[2] == 0xf0) {
347 /* Compress dest port. */
348 buffer[lowpan6_header_len++] |= 0x01;
349 buffer[lowpan6_header_len++] = inptr[0];
350 buffer[lowpan6_header_len++] = inptr[1];
351 buffer[lowpan6_header_len++] = inptr[3];
352 } else {
353 /* append full ports. */
354 lowpan6_header_len++;
355 buffer[lowpan6_header_len++] = inptr[0];
356 buffer[lowpan6_header_len++] = inptr[1];
357 buffer[lowpan6_header_len++] = inptr[2];
358 buffer[lowpan6_header_len++] = inptr[3];
359 }
360
361 /* elide length and copy checksum */
362 buffer[lowpan6_header_len++] = inptr[6];
363 buffer[lowpan6_header_len++] = inptr[7];
364
365 hidden_header_len += UDP_HLEN;
366 }
367#endif /* LWIP_UDP */
368
369 *lowpan6_header_len_out = lowpan6_header_len;
370 *hidden_header_len_out = hidden_header_len;
371
372 return ERR_OK;
373}
374
375/** Decompress IPv6 and UDP headers compressed according to RFC 6282
376 *
377 * @param lowpan6_buffer compressed headers, first byte is the dispatch byte
378 * @param lowpan6_bufsize size of lowpan6_buffer (may include data after headers)
379 * @param decomp_buffer buffer where the decompressed headers are stored
380 * @param decomp_bufsize size of decomp_buffer
381 * @param hdr_size_comp returns the size of the compressed headers (skip to get to data)
382 * @param hdr_size_decomp returns the size of the decompressed headers (IPv6 + UDP)
383 * @param datagram_size datagram size from fragments or 0 if unfragmented
384 * @param compressed_size compressed datagram size (for unfragmented rx)
385 * @param lowpan6_contexts context addresses
386 * @param src source address of the outer layer, used for address compression
387 * @param dest destination address of the outer layer, used for address compression
388 * @return ERR_OK if decompression succeeded, an error otherwise
389 */
390static err_t
391lowpan6_decompress_hdr(u8_t *lowpan6_buffer, size_t lowpan6_bufsize,
392 u8_t *decomp_buffer, size_t decomp_bufsize,
393 u16_t *hdr_size_comp, u16_t *hdr_size_decomp,
394 u16_t datagram_size, u16_t compressed_size,
395 ip6_addr_t *lowpan6_contexts,
396 struct lowpan6_link_addr *src, struct lowpan6_link_addr *dest)
397{
398 u16_t lowpan6_offset;
399 struct ip6_hdr *ip6hdr;
400 s8_t i;
401 u32_t header_temp;
402 u16_t ip6_offset = IP6_HLEN;
403
404 LWIP_ASSERT("lowpan6_buffer != NULL", lowpan6_buffer != NULL);
405 LWIP_ASSERT("decomp_buffer != NULL", decomp_buffer != NULL);
406 LWIP_ASSERT("src != NULL", src != NULL);
407 LWIP_ASSERT("dest != NULL", dest != NULL);
408 LWIP_ASSERT("hdr_size_comp != NULL", hdr_size_comp != NULL);
409 LWIP_ASSERT("dehdr_size_decompst != NULL", hdr_size_decomp != NULL);
410
411 ip6hdr = (struct ip6_hdr *)decomp_buffer;
412 if (decomp_bufsize < IP6_HLEN) {
413 return ERR_MEM;
414 }
415
416 /* output the full compressed packet, if set in @see lowpan6_opts.h */
417#if LWIP_LOWPAN6_IP_COMPRESSED_DEBUG
418 {
419 u16_t j;
420 LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("lowpan6_decompress_hdr: IP6 payload (compressed): \n"));
421 for (j = 0; j < lowpan6_bufsize; j++) {
422 if ((j % 4) == 0) {
423 LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("\n"));
424 }
425 LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("%2X ", lowpan6_buffer[j]));
426 }
427 LWIP_DEBUGF(LWIP_LOWPAN6_IP_COMPRESSED_DEBUG, ("\np->len: %d", lowpan6_bufsize));
428 }
429#endif
430
431 /* offset for inline IP headers (RFC 6282 ch3)*/
432 lowpan6_offset = 2;
433 /* if CID is set (context identifier), the context byte
434 * follows immediately after the header, so other IPHC fields are @+3 */
435 if (lowpan6_buffer[1] & 0x80) {
436 lowpan6_offset++;
437 }
438
439 /* Set IPv6 version, traffic class and flow label. (RFC6282, ch 3.1.1.)*/
440 if ((lowpan6_buffer[0] & 0x18) == 0x00) {
441 header_temp = ((lowpan6_buffer[lowpan6_offset+1] & 0x0f) << 16) | \
442 (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset+3];
443 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 00, ECN: 0x%2x, Flowlabel+DSCP: 0x%8X\n", \
444 lowpan6_buffer[lowpan6_offset],header_temp));
445 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset], header_temp);
446 /* increase offset, processed 4 bytes here:
447 * TF=00: ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)*/
448 lowpan6_offset += 4;
449 } else if ((lowpan6_buffer[0] & 0x18) == 0x08) {
450 header_temp = ((lowpan6_buffer[lowpan6_offset] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset+2];
451 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 01, ECN: 0x%2x, Flowlabel: 0x%2X, DSCP ignored\n", \
452 lowpan6_buffer[lowpan6_offset] & 0xc0,header_temp));
453 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset] & 0xc0, header_temp);
454 /* increase offset, processed 3 bytes here:
455 * TF=01: ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided.*/
456 lowpan6_offset += 3;
457 } else if ((lowpan6_buffer[0] & 0x18) == 0x10) {
458 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 10, DCSP+ECN: 0x%2x, Flowlabel ignored\n", lowpan6_buffer[lowpan6_offset]));
459 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset],0);
460 /* increase offset, processed 1 byte here:
461 * ECN + DSCP (1 byte), Flow Label is elided.*/
462 lowpan6_offset += 1;
463 } else if ((lowpan6_buffer[0] & 0x18) == 0x18) {
464 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("TF: 11, DCSP/ECN & Flowlabel ignored\n"));
465 /* don't increase offset, no bytes processed here */
466 IP6H_VTCFL_SET(ip6hdr, 6, 0, 0);
467 }
468
469 /* Set Next Header (NH) */
470 if ((lowpan6_buffer[0] & 0x04) == 0x00) {
471 /* 0: full next header byte carried inline (increase offset)*/
472 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NH: 0x%2X\n", lowpan6_buffer[lowpan6_offset+1]));
473 IP6H_NEXTH_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
474 } else {
475 /* 1: NH compression, LOWPAN_NHC (RFC6282, ch 4.1) */
476 /* We should fill this later with NHC decoding */
477 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NH: skipped, later done with NHC\n"));
478 IP6H_NEXTH_SET(ip6hdr, 0);
479 }
480
481 /* Set Hop Limit, either carried inline or 3 different hops (1,64,255) */
482 if ((lowpan6_buffer[0] & 0x03) == 0x00) {
483 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: full value: %d\n", lowpan6_buffer[lowpan6_offset+1]));
484 IP6H_HOPLIM_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
485 } else if ((lowpan6_buffer[0] & 0x03) == 0x01) {
486 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: compressed: 1\n"));
487 IP6H_HOPLIM_SET(ip6hdr, 1);
488 } else if ((lowpan6_buffer[0] & 0x03) == 0x02) {
489 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: compressed: 64\n"));
490 IP6H_HOPLIM_SET(ip6hdr, 64);
491 } else if ((lowpan6_buffer[0] & 0x03) == 0x03) {
492 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Hops: compressed: 255\n"));
493 IP6H_HOPLIM_SET(ip6hdr, 255);
494 }
495
496 /* Source address decoding. */
497 if ((lowpan6_buffer[1] & 0x40) == 0x00) {
498 /* Source address compression (SAC) = 0 -> stateless compression */
499 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAC == 0, no context byte\n"));
500 /* Stateless compression */
501 if ((lowpan6_buffer[1] & 0x30) == 0x00) {
502 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 00, no src compression, fetching 128bits inline\n"));
503 /* copy full address, increase offset by 16 Bytes */
504 MEMCPY(&ip6hdr->src.addr[0], lowpan6_buffer + lowpan6_offset, 16);
505 lowpan6_offset += 16;
506 } else if ((lowpan6_buffer[1] & 0x30) == 0x10) {
507 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 01, src compression, 64bits inline\n"));
508 /* set 64 bits to link local */
509 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
510 ip6hdr->src.addr[1] = 0;
511 /* copy 8 Bytes, increase offset */
512 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
513 lowpan6_offset += 8;
514 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
515 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 10, src compression, 16bits inline\n"));
516 /* set 96 bits to link local */
517 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
518 ip6hdr->src.addr[1] = 0;
519 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
520 /* extract remaining 16bits from inline bytes, increase offset */
521 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) |
522 lowpan6_buffer[lowpan6_offset + 1]);
523 lowpan6_offset += 2;
524 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
525 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 11, src compression, 0bits inline, using other headers\n"));
526 /* no information avalaible, using other layers, see RFC6282 ch 3.2.2 */
527 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
528 ip6hdr->src.addr[1] = 0;
529 if (src->addr_len == 2) {
530 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
531 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
532 } else if (src->addr_len == 8) {
533 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) |
534 (src->addr[2] << 8) | src->addr[3]);
535 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) |
536 (src->addr[6] << 8) | src->addr[7]);
537 } else {
538 /* invalid source address length */
539 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Invalid source address length\n"));
540 return ERR_VAL;
541 }
542 }
543 } else {
544 /* Source address compression (SAC) = 1 -> stateful/context-based compression */
545 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAC == 1, additional context byte\n"));
546 if ((lowpan6_buffer[1] & 0x30) == 0x00) {
547 /* SAM=00, address=> :: (ANY) */
548 ip6hdr->src.addr[0] = 0;
549 ip6hdr->src.addr[1] = 0;
550 ip6hdr->src.addr[2] = 0;
551 ip6hdr->src.addr[3] = 0;
552 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 00, context compression, ANY (::)\n"));
553 } else {
554 /* Set prefix from context info */
555 if (lowpan6_buffer[1] & 0x80) {
556 i = (lowpan6_buffer[2] >> 4) & 0x0f;
557 } else {
558 i = 0;
559 }
560 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
561 /* Error */
562 return ERR_VAL;
563 }
564#if LWIP_6LOWPAN_NUM_CONTEXTS > 0
565 ip6hdr->src.addr[0] = lowpan6_contexts[i].addr[0];
566 ip6hdr->src.addr[1] = lowpan6_contexts[i].addr[1];
567 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]));
568#else
569 LWIP_UNUSED_ARG(lowpan6_contexts);
570#endif
571 }
572
573 /* determine further address bits */
574 if ((lowpan6_buffer[1] & 0x30) == 0x10) {
575 /* SAM=01, load additional 64bits */
576 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
577 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 01, context compression, 64bits inline\n"));
578 lowpan6_offset += 8;
579 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
580 /* SAM=01, load additional 16bits */
581 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
582 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]);
583 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 10, context compression, 16bits inline\n"));
584 lowpan6_offset += 2;
585 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
586 /* SAM=11, address is fully elided, load from other layers */
587 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("SAM == 11, context compression, 0bits inline, using other headers\n"));
588 if (src->addr_len == 2) {
589 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
590 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
591 } else if (src->addr_len == 8) {
592 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | (src->addr[2] << 8) | src->addr[3]);
593 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) | (src->addr[6] << 8) | src->addr[7]);
594 } else {
595 /* invalid source address length */
596 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Invalid source address length\n"));
597 return ERR_VAL;
598 }
599 }
600 }
601
602 /* Destination address decoding. */
603 if (lowpan6_buffer[1] & 0x08) {
604 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("M=1: multicast\n"));
605 /* Multicast destination */
606 if (lowpan6_buffer[1] & 0x04) {
607 LWIP_DEBUGF(LWIP_DBG_ON,("DAC == 1, context multicast: unsupported!!!\n"));
608 /* @todo support stateful multicast addressing */
609 return ERR_VAL;
610 }
611
612 if ((lowpan6_buffer[1] & 0x03) == 0x00) {
613 /* DAM = 00, copy full address (128bits) */
614 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 00, no dst compression, fetching 128bits inline\n"));
615 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
616 lowpan6_offset += 16;
617 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
618 /* DAM = 01, copy 4 bytes (32bits) */
619 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 01, dst address form (48bits): ffXX::00XX:XXXX:XXXX\n"));
620 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16));
621 ip6hdr->dest.addr[1] = 0;
622 ip6hdr->dest.addr[2] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
623 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]);
624 lowpan6_offset += 4;
625 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
626 /* DAM = 10, copy 3 bytes (24bits) */
627 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 10, dst address form (32bits): ffXX::00XX:XXXX\n"));
628 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16));
629 ip6hdr->dest.addr[1] = 0;
630 ip6hdr->dest.addr[2] = 0;
631 ip6hdr->dest.addr[3] = lwip_htonl((lowpan6_buffer[lowpan6_offset] << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset + 2]);
632 lowpan6_offset += 3;
633 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
634 /* DAM = 11, copy 1 byte (8bits) */
635 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 11, dst address form (8bits): ff02::00XX\n"));
636 ip6hdr->dest.addr[0] = PP_HTONL(0xff020000UL);
637 ip6hdr->dest.addr[1] = 0;
638 ip6hdr->dest.addr[2] = 0;
639 ip6hdr->dest.addr[3] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
640 }
641
642 } else {
643 /* no Multicast (M=0) */
644 if (lowpan6_buffer[1] & 0x04) {
645 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAC == 1, stateful compression\n"));
646 /* Stateful destination compression */
647 /* Set prefix from context info */
648 if (lowpan6_buffer[1] & 0x80) {
649 i = lowpan6_buffer[2] & 0x0f;
650 } else {
651 i = 0;
652 }
653 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
654 /* Error */
655 return ERR_VAL;
656 }
657#if LWIP_6LOWPAN_NUM_CONTEXTS > 0
658 ip6hdr->dest.addr[0] = lowpan6_contexts[i].addr[0];
659 ip6hdr->dest.addr[1] = lowpan6_contexts[i].addr[1];
660#endif
661 } else {
662 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAC == 0, stateless compression, setting link local prefix\n"));
663 /* Link local address compression */
664 ip6hdr->dest.addr[0] = PP_HTONL(0xfe800000UL);
665 ip6hdr->dest.addr[1] = 0;
666 }
667
668 /* M=0, DAC=0, determining destination address length via DAM=xx */
669 if ((lowpan6_buffer[1] & 0x03) == 0x00) {
670 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 00, no dst compression, fetching 128bits inline"));
671 /* DAM=00, copy full address */
672 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
673 lowpan6_offset += 16;
674 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
675 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 01, dst compression, 64bits inline\n"));
676 /* DAM=01, copy 64 inline bits, increase offset */
677 MEMCPY(&ip6hdr->dest.addr[2], lowpan6_buffer + lowpan6_offset, 8);
678 lowpan6_offset += 8;
679 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
680 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("DAM == 01, dst compression, 16bits inline\n"));
681 /* DAM=10, copy 16 inline bits, increase offset */
682 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
683 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]);
684 lowpan6_offset += 2;
685 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
686 /* DAM=11, no bits available, use other headers (not done here) */
687 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG,("DAM == 01, dst compression, 0bits inline, using other headers\n"));
688 if (dest->addr_len == 2) {
689 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
690 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (dest->addr[0] << 8) | dest->addr[1]);
691 } else if (dest->addr_len == 8) {
692 ip6hdr->dest.addr[2] = lwip_htonl(((dest->addr[0] ^ 2) << 24) | (dest->addr[1] << 16) | dest->addr[2] << 8 | dest->addr[3]);
693 ip6hdr->dest.addr[3] = lwip_htonl((dest->addr[4] << 24) | (dest->addr[5] << 16) | dest->addr[6] << 8 | dest->addr[7]);
694 } else {
695 /* invalid destination address length */
696 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Invalid destination address length\n"));
697 return ERR_VAL;
698 }
699 }
700 }
701
702
703 /* Next Header Compression (NHC) decoding? */
704 if (lowpan6_buffer[0] & 0x04) {
705 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NHC decoding\n"));
706#if LWIP_UDP
707 if ((lowpan6_buffer[lowpan6_offset] & 0xf8) == 0xf0) {
708 /* NHC: UDP */
709 struct udp_hdr *udphdr;
710 LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("NHC: UDP\n"));
711
712 /* UDP compression */
713 IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_UDP);
714 udphdr = (struct udp_hdr *)((u8_t *)decomp_buffer + ip6_offset);
715 if (decomp_bufsize < IP6_HLEN + UDP_HLEN) {
716 return ERR_MEM;
717 }
718
719 /* Checksum decompression */
720 if (lowpan6_buffer[lowpan6_offset] & 0x04) {
721 /* @todo support checksum decompress */
722 LWIP_DEBUGF(LWIP_DBG_ON, ("NHC: UDP chechsum decompression UNSUPPORTED\n"));
723 return ERR_VAL;
724 }
725
726 /* Decompress ports, according to RFC4944 */
727 i = lowpan6_buffer[lowpan6_offset++] & 0x03;
728 if (i == 0) {
729 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
730 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 2] << 8 | lowpan6_buffer[lowpan6_offset + 3]);
731 lowpan6_offset += 4;
732 } else if (i == 0x01) {
733 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
734 udphdr->dest = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset + 2]);
735 lowpan6_offset += 3;
736 } else if (i == 0x02) {
737 udphdr->src = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset]);
738 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 1] << 8 | lowpan6_buffer[lowpan6_offset + 2]);
739 lowpan6_offset += 3;
740 } else if (i == 0x03) {
741 udphdr->src = lwip_htons(0xf0b0 | ((lowpan6_buffer[lowpan6_offset] >> 4) & 0x0f));
742 udphdr->dest = lwip_htons(0xf0b0 | (lowpan6_buffer[lowpan6_offset] & 0x0f));
743 lowpan6_offset += 1;
744 }
745
746 udphdr->chksum = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
747 lowpan6_offset += 2;
748 ip6_offset += UDP_HLEN;
749 if (datagram_size == 0) {
750 datagram_size = compressed_size - lowpan6_offset + ip6_offset;
751 }
752 udphdr->len = lwip_htons(datagram_size - IP6_HLEN);
753
754 } else
755#endif /* LWIP_UDP */
756 {
757 LWIP_DEBUGF(LWIP_DBG_ON,("NHC: unsupported protocol!\n"));
758 /* @todo support NHC other than UDP */
759 return ERR_VAL;
760 }
761 }
762 if (datagram_size == 0) {
763 datagram_size = compressed_size - lowpan6_offset + ip6_offset;
764 }
765 /* Infer IPv6 payload length for header */
766 IP6H_PLEN_SET(ip6hdr, datagram_size - IP6_HLEN);
767
768 if (lowpan6_offset > lowpan6_bufsize) {
769 /* input buffer overflow */
770 return ERR_VAL;
771 }
772 *hdr_size_comp = lowpan6_offset;
773 *hdr_size_decomp = ip6_offset;
774
775 return ERR_OK;
776}
777
778struct pbuf *
779lowpan6_decompress(struct pbuf *p, u16_t datagram_size, ip6_addr_t *lowpan6_contexts,
780 struct lowpan6_link_addr *src, struct lowpan6_link_addr *dest)
781{
782 struct pbuf *q;
783 u16_t lowpan6_offset, ip6_offset;
784 err_t err;
785
786#if LWIP_UDP
787#define UDP_HLEN_ALLOC UDP_HLEN
788#else
789#define UDP_HLEN_ALLOC 0
790#endif
791
792 /* Allocate a buffer for decompression. This buffer will be too big and will be
793 trimmed once the final size is known. */
794 q = pbuf_alloc(PBUF_IP, p->len + IP6_HLEN + UDP_HLEN_ALLOC, PBUF_POOL);
795 if (q == NULL) {
796 pbuf_free(p);
797 return NULL;
798 }
799 if (q->len < IP6_HLEN + UDP_HLEN_ALLOC) {
800 /* The headers need to fit into the first pbuf */
801 pbuf_free(p);
802 pbuf_free(q);
803 return NULL;
804 }
805
806 /* Decompress the IPv6 (and possibly UDP) header(s) into the new pbuf */
807 err = lowpan6_decompress_hdr((u8_t *)p->payload, p->len, (u8_t *)q->payload, q->len,
808 &lowpan6_offset, &ip6_offset, datagram_size, p->tot_len, lowpan6_contexts, src, dest);
809 if (err != ERR_OK) {
810 pbuf_free(p);
811 pbuf_free(q);
812 return NULL;
813 }
814
815 /* Now we copy leftover contents from p to q, so we have all L2 and L3 headers
816 (and L4?) in a single pbuf: */
817
818 /* Hide the compressed headers in p */
819 pbuf_remove_header(p, lowpan6_offset);
820 /* Temporarily hide the headers in q... */
821 pbuf_remove_header(q, ip6_offset);
822 /* ... copy the rest of p into q... */
823 pbuf_copy(q, p);
824 /* ... and reveal the headers again... */
825 pbuf_add_header_force(q, ip6_offset);
826 /* ... trim the pbuf to its correct size... */
827 pbuf_realloc(q, ip6_offset + p->len);
828 /* ... and cat possibly remaining (data-only) pbufs */
829 if (p->next != NULL) {
830 pbuf_cat(q, p->next);
831 }
832 /* the original (first) pbuf can now be freed */
833 p->next = NULL;
834 pbuf_free(p);
835
836 /* all done */
837 return q;
838}
839
840#endif /* LWIP_6LOWPAN_IPHC */
841#endif /* LWIP_IPV6 */
Note: See TracBrowser for help on using the repository browser.