BadVPN – Blame information for rev 1

Subversion Repositories:
Rev:
Rev Author Line No. Line
1 office 1 /**
2 * @file
3 *
4 * 6LowPAN output for IPv6. Uses ND tables for link-layer addressing. Fragments packets to 6LowPAN units.
5 */
6  
7 /*
8 * Copyright (c) 2015 Inico Technologies Ltd.
9 * All rights reserved.
10 *
11 * Redistribution and use in source and binary forms, with or without modification,
12 * are permitted provided that the following conditions are met:
13 *
14 * 1. Redistributions of source code must retain the above copyright notice,
15 * this list of conditions and the following disclaimer.
16 * 2. Redistributions in binary form must reproduce the above copyright notice,
17 * this list of conditions and the following disclaimer in the documentation
18 * and/or other materials provided with the distribution.
19 * 3. The name of the author may not be used to endorse or promote products
20 * derived from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
23 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
24 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
25 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
27 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
30 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
31 * OF SUCH DAMAGE.
32 *
33 * This file is part of the lwIP TCP/IP stack.
34 *
35 * Author: Ivan Delamer <delamer@inicotech.com>
36 *
37 *
38 * Please coordinate changes and requests with Ivan Delamer
39 * <delamer@inicotech.com>
40 */
41  
42 /**
43 * @defgroup sixlowpan 6LowPAN
44 * @ingroup netifs
45 * 6LowPAN netif implementation
46 */
47  
48 #include "netif/lowpan6.h"
49  
50 #if LWIP_IPV6 && LWIP_6LOWPAN
51  
52 #include "lwip/ip.h"
53 #include "lwip/pbuf.h"
54 #include "lwip/ip_addr.h"
55 #include "lwip/netif.h"
56 #include "lwip/nd6.h"
57 #include "lwip/mem.h"
58 #include "lwip/udp.h"
59 #include "lwip/tcpip.h"
60 #include "lwip/snmp.h"
61  
62 #include <string.h>
63  
64 struct ieee_802154_addr {
65 u8_t addr_len;
66 u8_t addr[8];
67 };
68  
69 /** This is a helper struct.
70 */
71 struct lowpan6_reass_helper {
72 struct pbuf *pbuf;
73 struct lowpan6_reass_helper *next_packet;
74 u8_t timer;
75 struct ieee_802154_addr sender_addr;
76 u16_t datagram_size;
77 u16_t datagram_tag;
78 };
79  
80 static struct lowpan6_reass_helper * reass_list;
81  
82 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
83 static ip6_addr_t lowpan6_context[LWIP_6LOWPAN_NUM_CONTEXTS];
84 #endif
85  
86 static u16_t ieee_802154_pan_id;
87  
88 static const struct ieee_802154_addr ieee_802154_broadcast = {2, {0xff, 0xff}};
89  
90 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS
91 static struct ieee_802154_addr short_mac_addr = {2, {0,0}};
92 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */
93  
94 static err_t dequeue_datagram(struct lowpan6_reass_helper *lrh);
95  
96 /**
97 * Periodic timer for 6LowPAN functions:
98 *
99 * - Remove incomplete/old packets
100 */
101 void
102 lowpan6_tmr(void)
103 {
104 struct lowpan6_reass_helper *lrh, *lrh_temp;
105  
106 lrh = reass_list;
107 while (lrh != NULL) {
108 lrh_temp = lrh->next_packet;
109 if ((--lrh->timer) == 0) {
110 dequeue_datagram(lrh);
111 pbuf_free(lrh->pbuf);
112 mem_free(lrh);
113 }
114 lrh = lrh_temp;
115 }
116 }
117  
118 /**
119 * Removes a datagram from the reassembly queue.
120 **/
121 static err_t
122 dequeue_datagram(struct lowpan6_reass_helper *lrh)
123 {
124 struct lowpan6_reass_helper *lrh_temp;
125  
126 if (reass_list == lrh) {
127 reass_list = reass_list->next_packet;
128 } else {
129 lrh_temp = reass_list;
130 while (lrh_temp != NULL) {
131 if (lrh_temp->next_packet == lrh) {
132 lrh_temp->next_packet = lrh->next_packet;
133 break;
134 }
135 lrh_temp = lrh_temp->next_packet;
136 }
137 }
138  
139 return ERR_OK;
140 }
141  
142 static s8_t
143 lowpan6_context_lookup(const ip6_addr_t *ip6addr)
144 {
145 s8_t i;
146  
147 for (i = 0; i < LWIP_6LOWPAN_NUM_CONTEXTS; i++) {
148 if (ip6_addr_netcmp(&lowpan6_context[i], ip6addr)) {
149 return i;
150 }
151 }
152  
153 return -1;
154 }
155  
156 /* Determine compression mode for unicast address. */
157 static s8_t
158 lowpan6_get_address_mode(const ip6_addr_t *ip6addr, const struct ieee_802154_addr *mac_addr)
159 {
160 if (mac_addr->addr_len == 2) {
161 if ((ip6addr->addr[2] == (u32_t)PP_HTONL(0x000000ff)) &&
162 ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000))) {
163 if ((ip6addr->addr[3] & PP_HTONL(0x0000ffff)) == lwip_ntohl((mac_addr->addr[0] << 8) | mac_addr->addr[1])) {
164 return 3;
165 }
166 }
167 } else if (mac_addr->addr_len == 8) {
168 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])) &&
169 (ip6addr->addr[3] == lwip_ntohl((mac_addr->addr[4] << 24) | (mac_addr->addr[5] << 16) | mac_addr->addr[6] << 8 | mac_addr->addr[7]))) {
170 return 3;
171 }
172 }
173  
174 if ((ip6addr->addr[2] == PP_HTONL(0x000000ffUL)) &&
175 ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000UL))) {
176 return 2;
177 }
178  
179 return 1;
180 }
181  
182 /* Determine compression mode for multicast address. */
183 static s8_t
184 lowpan6_get_address_mode_mc(const ip6_addr_t *ip6addr)
185 {
186 if ((ip6addr->addr[0] == PP_HTONL(0xff020000)) &&
187 (ip6addr->addr[1] == 0) &&
188 (ip6addr->addr[2] == 0) &&
189 ((ip6addr->addr[3] & PP_HTONL(0xffffff00)) == 0)) {
190 return 3;
191 } else if (((ip6addr->addr[0] & PP_HTONL(0xff00ffff)) == PP_HTONL(0xff000000)) &&
192 (ip6addr->addr[1] == 0)) {
193 if ((ip6addr->addr[2] == 0) &&
194 ((ip6addr->addr[3] & PP_HTONL(0xff000000)) == 0)) {
195 return 2;
196 } else if ((ip6addr->addr[2] & PP_HTONL(0xffffff00)) == 0) {
197 return 1;
198 }
199 }
200  
201 return 0;
202 }
203  
204 /*
205 * Encapsulates data into IEEE 802.15.4 frames.
206 * Fragments an IPv6 datagram into 6LowPAN units, which fit into IEEE 802.15.4 frames.
207 * If configured, will compress IPv6 and or UDP headers.
208 * */
209 static err_t
210 lowpan6_frag(struct netif *netif, struct pbuf *p, const struct ieee_802154_addr *src, const struct ieee_802154_addr *dst)
211 {
212 struct pbuf * p_frag;
213 u16_t frag_len, remaining_len;
214 u8_t * buffer;
215 u8_t ieee_header_len;
216 u8_t lowpan6_header_len;
217 s8_t i;
218 static u8_t frame_seq_num;
219 static u16_t datagram_tag;
220 u16_t datagram_offset;
221 err_t err = ERR_IF;
222  
223 /* We'll use a dedicated pbuf for building 6LowPAN fragments. */
224 p_frag = pbuf_alloc(PBUF_RAW, 127, PBUF_RAM);
225 if (p_frag == NULL) {
226 MIB2_STATS_NETIF_INC(netif, ifoutdiscards);
227 return ERR_MEM;
228 }
229  
230 /* Write IEEE 802.15.4 header. */
231 buffer = (u8_t*)p_frag->payload;
232 ieee_header_len = 0;
233 if (dst == &ieee_802154_broadcast) {
234 buffer[ieee_header_len++] = 0x01; /* data packet, no ack required. */
235 } else {
236 buffer[ieee_header_len++] = 0x21; /* data packet, ack required. */
237 }
238 buffer[ieee_header_len] = (0x00 << 4); /* 2003 frame version */
239 buffer[ieee_header_len] |= (dst->addr_len == 2) ? (0x02 << 2) : (0x03 << 2); /* destination addressing mode */
240 buffer[ieee_header_len] |= (src->addr_len == 2) ? (0x02 << 6) : (0x03 << 6); /* source addressing mode */
241 ieee_header_len++;
242 buffer[ieee_header_len++] = frame_seq_num++;
243  
244 buffer[ieee_header_len++] = ieee_802154_pan_id & 0xff; /* pan id */
245 buffer[ieee_header_len++] = (ieee_802154_pan_id >> 8) & 0xff; /* pan id */
246 i = dst->addr_len;
247 while (i-- > 0) {
248 buffer[ieee_header_len++] = dst->addr[i];
249 }
250  
251 buffer[ieee_header_len++] = ieee_802154_pan_id & 0xff; /* pan id */
252 buffer[ieee_header_len++] = (ieee_802154_pan_id >> 8) & 0xff; /* pan id */
253 i = src->addr_len;
254 while (i-- > 0) {
255 buffer[ieee_header_len++] = src->addr[i];
256 }
257  
258 #if LWIP_6LOWPAN_IPHC
259 /* Perform 6LowPAN IPv6 header compression according to RFC 6282 */
260 {
261 struct ip6_hdr *ip6hdr;
262  
263 /* Point to ip6 header and align copies of src/dest addresses. */
264 ip6hdr = (struct ip6_hdr *)p->payload;
265 ip_addr_copy_from_ip6_packed(ip_data.current_iphdr_dest, ip6hdr->dest);
266 ip6_addr_assign_zone(ip_2_ip6(&ip_data.current_iphdr_dest), IP6_UNKNOWN, netif);
267 ip_addr_copy_from_ip6_packed(ip_data.current_iphdr_src, ip6hdr->src);
268 ip6_addr_assign_zone(ip_2_ip6(&ip_data.current_iphdr_src), IP6_UNKNOWN, netif);
269  
270 /* Basic length of 6LowPAN header, set dispatch and clear fields. */
271 lowpan6_header_len = 2;
272 buffer[ieee_header_len] = 0x60;
273 buffer[ieee_header_len + 1] = 0;
274  
275 /* Determine whether there will be a Context Identifier Extension byte or not.
276 * If so, set it already. */
277 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0
278 buffer[ieee_header_len + 2] = 0;
279  
280 i = lowpan6_context_lookup(ip_2_ip6(&ip_data.current_iphdr_src));
281 if (i >= 0) {
282 /* Stateful source address compression. */
283 buffer[ieee_header_len + 1] |= 0x40;
284 buffer[ieee_header_len + 2] |= (i & 0x0f) << 4;
285 }
286  
287 i = lowpan6_context_lookup(ip_2_ip6(&ip_data.current_iphdr_dest));
288 if (i >= 0) {
289 /* Stateful destination address compression. */
290 buffer[ieee_header_len + 1] |= 0x04;
291 buffer[ieee_header_len + 2] |= i & 0x0f;
292 }
293  
294 if (buffer[ieee_header_len + 2] != 0x00) {
295 /* Context identifier extension byte is appended. */
296 buffer[ieee_header_len + 1] |= 0x80;
297 lowpan6_header_len++;
298 }
299 #endif /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */
300  
301 /* Determine TF field: Traffic Class, Flow Label */
302 if (IP6H_FL(ip6hdr) == 0) {
303 /* Flow label is elided. */
304 buffer[ieee_header_len] |= 0x10;
305 if (IP6H_TC(ip6hdr) == 0) {
306 /* Traffic class (ECN+DSCP) elided too. */
307 buffer[ieee_header_len] |= 0x08;
308 } else {
309 /* Traffic class (ECN+DSCP) appended. */
310 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_TC(ip6hdr);
311 }
312 } else {
313 if (((IP6H_TC(ip6hdr) & 0x3f) == 0)) {
314 /* DSCP portion of Traffic Class is elided, ECN and FL are appended (3 bytes) */
315 buffer[ieee_header_len] |= 0x08;
316  
317 buffer[ieee_header_len + lowpan6_header_len] = IP6H_TC(ip6hdr) & 0xc0;
318 buffer[ieee_header_len + lowpan6_header_len++] |= (IP6H_FL(ip6hdr) >> 16) & 0x0f;
319 buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff;
320 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff;
321 } else {
322 /* Traffic class and flow label are appended (4 bytes) */
323 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_TC(ip6hdr);
324 buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 16) & 0x0f;
325 buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff;
326 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff;
327 }
328 }
329  
330 /* Compress NH?
331 * Only if UDP for now. @todo support other NH compression. */
332 if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) {
333 buffer[ieee_header_len] |= 0x04;
334 } else {
335 /* append nexth. */
336 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_NEXTH(ip6hdr);
337 }
338  
339 /* Compress hop limit? */
340 if (IP6H_HOPLIM(ip6hdr) == 255) {
341 buffer[ieee_header_len] |= 0x03;
342 } else if (IP6H_HOPLIM(ip6hdr) == 64) {
343 buffer[ieee_header_len] |= 0x02;
344 } else if (IP6H_HOPLIM(ip6hdr) == 1) {
345 buffer[ieee_header_len] |= 0x01;
346 } else {
347 /* append hop limit */
348 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_HOPLIM(ip6hdr);
349 }
350  
351 /* Compress source address */
352 if (((buffer[ieee_header_len + 1] & 0x40) != 0) ||
353 (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_src)))) {
354 /* Context-based or link-local source address compression. */
355 i = lowpan6_get_address_mode(ip_2_ip6(&ip_data.current_iphdr_src), src);
356 buffer[ieee_header_len + 1] |= (i & 0x03) << 4;
357 if (i == 1) {
358 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 16, 8);
359 lowpan6_header_len += 8;
360 } else if (i == 2) {
361 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 22, 2);
362 lowpan6_header_len += 2;
363 }
364 } else if (ip6_addr_isany(ip_2_ip6(&ip_data.current_iphdr_src))) {
365 /* Special case: mark SAC and leave SAM=0 */
366 buffer[ieee_header_len + 1] |= 0x40;
367 } else {
368 /* Append full address. */
369 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 8, 16);
370 lowpan6_header_len += 16;
371 }
372  
373 /* Compress destination address */
374 if (ip6_addr_ismulticast(ip_2_ip6(&ip_data.current_iphdr_dest))) {
375 /* @todo support stateful multicast address compression */
376  
377 buffer[ieee_header_len + 1] |= 0x08;
378  
379 i = lowpan6_get_address_mode_mc(ip_2_ip6(&ip_data.current_iphdr_dest));
380 buffer[ieee_header_len + 1] |= i & 0x03;
381 if (i == 0) {
382 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 24, 16);
383 lowpan6_header_len += 16;
384 } else if (i == 1) {
385 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[25];
386 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 35, 5);
387 lowpan6_header_len += 5;
388 } else if (i == 2) {
389 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[25];
390 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 37, 3);
391 lowpan6_header_len += 3;
392 } else if (i == 3) {
393 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[39];
394 }
395 } else if (((buffer[ieee_header_len + 1] & 0x04) != 0) ||
396 (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_dest)))) {
397 /* Context-based or link-local destination address compression. */
398 i = lowpan6_get_address_mode(ip_2_ip6(&ip_data.current_iphdr_dest), dst);
399 buffer[ieee_header_len + 1] |= i & 0x03;
400 if (i == 1) {
401 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 32, 8);
402 lowpan6_header_len += 8;
403 } else if (i == 2) {
404 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 38, 2);
405 lowpan6_header_len += 2;
406 }
407 } else {
408 /* Append full address. */
409 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 24, 16);
410 lowpan6_header_len += 16;
411 }
412  
413 /* Move to payload. */
414 pbuf_remove_header(p, IP6_HLEN);
415  
416 #if LWIP_UDP
417 /* Compress UDP header? */
418 if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) {
419 /* @todo support optional checksum compression */
420  
421 buffer[ieee_header_len + lowpan6_header_len] = 0xf0;
422  
423 /* determine port compression mode. */
424 if ((((u8_t *)p->payload)[0] == 0xf0) && ((((u8_t *)p->payload)[1] & 0xf0) == 0xb0) &&
425 (((u8_t *)p->payload)[2] == 0xf0) && ((((u8_t *)p->payload)[3] & 0xf0) == 0xb0)) {
426 /* Compress source and dest ports. */
427 buffer[ieee_header_len + lowpan6_header_len++] |= 0x03;
428 buffer[ieee_header_len + lowpan6_header_len++] = ((((u8_t *)p->payload)[1] & 0x0f) << 4) | (((u8_t *)p->payload)[3] & 0x0f);
429 } else if (((u8_t *)p->payload)[0] == 0xf0) {
430 /* Compress source port. */
431 buffer[ieee_header_len + lowpan6_header_len++] |= 0x02;
432 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1];
433 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2];
434 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3];
435 } else if (((u8_t *)p->payload)[2] == 0xf0) {
436 /* Compress dest port. */
437 buffer[ieee_header_len + lowpan6_header_len++] |= 0x01;
438 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0];
439 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1];
440 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3];
441 } else {
442 /* append full ports. */
443 lowpan6_header_len++;
444 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0];
445 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1];
446 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2];
447 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3];
448 }
449  
450 /* elide length and copy checksum */
451 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[6];
452 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[7];
453  
454 pbuf_remove_header(p, UDP_HLEN);
455 }
456 #endif /* LWIP_UDP */
457 }
458  
459 #else /* LWIP_6LOWPAN_HC */
460 /* Send uncompressed IPv6 header with appropriate dispatch byte. */
461 lowpan6_header_len = 1;
462 buffer[ieee_header_len] = 0x41; /* IPv6 dispatch */
463 #endif /* LWIP_6LOWPAN_HC */
464  
465 /* Calculate remaining packet length */
466 remaining_len = p->tot_len;
467  
468 if (remaining_len > 0x7FF) {
469 MIB2_STATS_NETIF_INC(netif, ifoutdiscards);
470 /* datagram_size must fit into 11 bit */
471 pbuf_free(p_frag);
472 return ERR_VAL;
473 }
474  
475 /* Fragment, or 1 packet? */
476 if (remaining_len > (127 - ieee_header_len - lowpan6_header_len - 3)) { /* 127 - header - 1 byte dispatch - 2 bytes CRC */
477 /* We must move the 6LowPAN header to make room for the FRAG header. */
478 i = lowpan6_header_len;
479 while (i-- != 0) {
480 buffer[ieee_header_len + i + 4] = buffer[ieee_header_len + i];
481 }
482  
483 /* Now we need to fragment the packet. FRAG1 header first */
484 buffer[ieee_header_len] = 0xc0 | (((p->tot_len + lowpan6_header_len) >> 8) & 0x7);
485 buffer[ieee_header_len + 1] = (p->tot_len + lowpan6_header_len) & 0xff;
486  
487 datagram_tag++;
488 buffer[ieee_header_len + 2] = datagram_tag & 0xff;
489 buffer[ieee_header_len + 3] = (datagram_tag >> 8) & 0xff;
490  
491 /* Fragment follows. */
492 frag_len = (127 - ieee_header_len - 4 - 2) & 0xf8;
493  
494 pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len + 4, frag_len - lowpan6_header_len, 0);
495 remaining_len -= frag_len - lowpan6_header_len;
496 datagram_offset = frag_len;
497  
498 /* 2 bytes CRC */
499 #if LWIP_6LOWPAN_HW_CRC
500 /* Leave blank, will be filled by HW. */
501 #else /* LWIP_6LOWPAN_HW_CRC */
502 /* @todo calculate CRC */
503 #endif /* LWIP_6LOWPAN_HW_CRC */
504  
505 /* Calculate frame length */
506 p_frag->len = p_frag->tot_len = ieee_header_len + 4 + frag_len + 2; /* add 2 dummy bytes for crc*/
507  
508 /* send the packet */
509 MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len);
510 LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p));
511 err = netif->linkoutput(netif, p_frag);
512  
513 while ((remaining_len > 0) && (err == ERR_OK)) {
514 /* new frame, new seq num for ACK */
515 buffer[2] = frame_seq_num++;
516  
517 buffer[ieee_header_len] |= 0x20; /* Change FRAG1 to FRAGN */
518  
519 buffer[ieee_header_len + 4] = (u8_t)(datagram_offset >> 3); /* datagram offset in FRAGN header (datagram_offset is max. 11 bit) */
520  
521 frag_len = (127 - ieee_header_len - 5 - 2) & 0xf8;
522 if (frag_len > remaining_len) {
523 frag_len = remaining_len;
524 }
525  
526 pbuf_copy_partial(p, buffer + ieee_header_len + 5, frag_len, p->tot_len - remaining_len);
527 remaining_len -= frag_len;
528 datagram_offset += frag_len;
529  
530 /* 2 bytes CRC */
531 #if LWIP_6LOWPAN_HW_CRC
532 /* Leave blank, will be filled by HW. */
533 #else /* LWIP_6LOWPAN_HW_CRC */
534 /* @todo calculate CRC */
535 #endif /* LWIP_6LOWPAN_HW_CRC */
536  
537 /* Calculate frame length */
538 p_frag->len = p_frag->tot_len = frag_len + 5 + ieee_header_len + 2;
539  
540 /* send the packet */
541 MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len);
542 LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p));
543 err = netif->linkoutput(netif, p_frag);
544 }
545 } else {
546 /* It fits in one frame. */
547 frag_len = remaining_len;
548  
549 /* Copy IPv6 packet */
550 pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len, frag_len, 0);
551 remaining_len = 0;
552  
553 /* 2 bytes CRC */
554 #if LWIP_6LOWPAN_HW_CRC
555 /* Leave blank, will be filled by HW. */
556 #else /* LWIP_6LOWPAN_HW_CRC */
557 /* @todo calculate CRC */
558 #endif /* LWIP_6LOWPAN_HW_CRC */
559  
560 /* Calculate frame length */
561 p_frag->len = p_frag->tot_len = frag_len + lowpan6_header_len + ieee_header_len + 2;
562  
563 /* send the packet */
564 MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len);
565 LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p));
566 err = netif->linkoutput(netif, p_frag);
567 }
568  
569 pbuf_free(p_frag);
570  
571 return err;
572 }
573  
574 err_t
575 lowpan6_set_context(u8_t idx, const ip6_addr_t * context)
576 {
577 if (idx >= LWIP_6LOWPAN_NUM_CONTEXTS) {
578 return ERR_ARG;
579 }
580  
581 IP6_ADDR_ZONECHECK(context);
582  
583 ip6_addr_set(&lowpan6_context[idx], context);
584  
585 return ERR_OK;
586 }
587  
588 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS
589 err_t
590 lowpan6_set_short_addr(u8_t addr_high, u8_t addr_low)
591 {
592 short_mac_addr.addr[0] = addr_high;
593 short_mac_addr.addr[1] = addr_low;
594  
595 return ERR_OK;
596 }
597 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */
598  
599 #if LWIP_IPV4
600 err_t
601 lowpan4_output(struct netif *netif, struct pbuf *q, const ip4_addr_t *ipaddr)
602 {
603 (void)netif;
604 (void)q;
605 (void)ipaddr;
606  
607 return ERR_IF;
608 }
609 #endif /* LWIP_IPV4 */
610  
611 /**
612 * Resolve and fill-in IEEE 802.15.4 address header for outgoing IPv6 packet.
613 *
614 * Perform Header Compression and fragment if necessary.
615 *
616 * @param netif The lwIP network interface which the IP packet will be sent on.
617 * @param q The pbuf(s) containing the IP packet to be sent.
618 * @param ip6addr The IP address of the packet destination.
619 *
620 * @return err_t
621 */
622 err_t
623 lowpan6_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr)
624 {
625 err_t result;
626 const u8_t *hwaddr;
627 struct ieee_802154_addr src, dest;
628 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS
629 ip6_addr_t ip6_src;
630 struct ip6_hdr * ip6_hdr;
631 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */
632  
633 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS
634 /* Check if we can compress source address (use aligned copy) */
635 ip6_hdr = (struct ip6_hdr *)q->payload;
636 ip6_addr_copy_from_packed(ip6_src, ip6_hdr->src);
637 ip6_addr_assign_zone(&ip6_src, IP6_UNICAST, netif);
638 if (lowpan6_get_address_mode(&ip6_src, &short_mac_addr) == 3) {
639 src.addr_len = 2;
640 src.addr[0] = short_mac_addr.addr[0];
641 src.addr[1] = short_mac_addr.addr[1];
642 } else
643 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */
644 {
645 src.addr_len = netif->hwaddr_len;
646 SMEMCPY(src.addr, netif->hwaddr, netif->hwaddr_len);
647 }
648  
649 /* multicast destination IP address? */
650 if (ip6_addr_ismulticast(ip6addr)) {
651 MIB2_STATS_NETIF_INC(netif, ifoutnucastpkts);
652 /* We need to send to the broadcast address.*/
653 return lowpan6_frag(netif, q, &src, &ieee_802154_broadcast);
654 }
655  
656 /* We have a unicast destination IP address */
657 /* @todo anycast? */
658  
659 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS
660 if (src.addr_len == 2) {
661 /* If source address was compressable to short_mac_addr, and dest has same subnet and
662 * is also compressable to 2-bytes, assume we can infer dest as a short address too. */
663 dest.addr_len = 2;
664 dest.addr[0] = ((u8_t *)q->payload)[38];
665 dest.addr[1] = ((u8_t *)q->payload)[39];
666 if ((src.addr_len == 2) && (ip6_addr_netcmp_zoneless(&ip6_hdr->src, &ip6_hdr->dest)) &&
667 (lowpan6_get_address_mode(ip6addr, &dest) == 3)) {
668 MIB2_STATS_NETIF_INC(netif, ifoutucastpkts);
669 return lowpan6_frag(netif, q, &src, &dest);
670 }
671 }
672 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */
673  
674 /* Ask ND6 what to do with the packet. */
675 result = nd6_get_next_hop_addr_or_queue(netif, q, ip6addr, &hwaddr);
676 if (result != ERR_OK) {
677 MIB2_STATS_NETIF_INC(netif, ifoutdiscards);
678 return result;
679 }
680  
681 /* If no hardware address is returned, nd6 has queued the packet for later. */
682 if (hwaddr == NULL) {
683 return ERR_OK;
684 }
685  
686 /* Send out the packet using the returned hardware address. */
687 dest.addr_len = netif->hwaddr_len;
688 SMEMCPY(dest.addr, hwaddr, netif->hwaddr_len);
689 MIB2_STATS_NETIF_INC(netif, ifoutucastpkts);
690 return lowpan6_frag(netif, q, &src, &dest);
691 }
692  
693 static struct pbuf *
694 lowpan6_decompress(struct pbuf * p, struct ieee_802154_addr * src, struct ieee_802154_addr * dest)
695 {
696 struct pbuf * q;
697 u8_t * lowpan6_buffer;
698 u16_t lowpan6_offset;
699 struct ip6_hdr *ip6hdr;
700 s8_t i;
701 s8_t ip6_offset = IP6_HLEN;
702  
703 #if LWIP_UDP
704 #define UDP_HLEN_ALLOC UDP_HLEN
705 #else
706 #define UDP_HLEN_ALLOC 0
707 #endif
708  
709 q = pbuf_alloc(PBUF_IP, p->len + IP6_HLEN + UDP_HLEN_ALLOC, PBUF_POOL);
710 if (q == NULL) {
711 pbuf_free(p);
712 return NULL;
713 }
714  
715 lowpan6_buffer = (u8_t *)p->payload;
716 ip6hdr = (struct ip6_hdr *)q->payload;
717  
718 lowpan6_offset = 2;
719 if (lowpan6_buffer[1] & 0x80) {
720 lowpan6_offset++;
721 }
722  
723 /* Set IPv6 version, traffic class and flow label. */
724 if ((lowpan6_buffer[0] & 0x18) == 0x00) {
725 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset], ((lowpan6_buffer[lowpan6_offset+1] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset+3]);
726 lowpan6_offset += 4;
727 } else if ((lowpan6_buffer[0] & 0x18) == 0x08) {
728 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset] & 0xc0, ((lowpan6_buffer[lowpan6_offset] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset+2]);
729 lowpan6_offset += 3;
730 } else if ((lowpan6_buffer[0] & 0x18) == 0x10) {
731 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset],0);
732 lowpan6_offset += 1;
733 } else if ((lowpan6_buffer[0] & 0x18) == 0x18) {
734 IP6H_VTCFL_SET(ip6hdr, 6, 0, 0);
735 }
736  
737 /* Set Next Header */
738 if ((lowpan6_buffer[0] & 0x04) == 0x00) {
739 IP6H_NEXTH_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
740 } else {
741 /* We should fill this later with NHC decoding */
742 IP6H_NEXTH_SET(ip6hdr, 0);
743 }
744  
745 /* Set Hop Limit */
746 if ((lowpan6_buffer[0] & 0x03) == 0x00) {
747 IP6H_HOPLIM_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
748 } else if ((lowpan6_buffer[0] & 0x03) == 0x01) {
749 IP6H_HOPLIM_SET(ip6hdr, 1);
750 } else if ((lowpan6_buffer[0] & 0x03) == 0x02) {
751 IP6H_HOPLIM_SET(ip6hdr, 64);
752 } else if ((lowpan6_buffer[0] & 0x03) == 0x03) {
753 IP6H_HOPLIM_SET(ip6hdr, 255);
754 }
755  
756 /* Source address decoding. */
757 if ((lowpan6_buffer[1] & 0x40) == 0x00) {
758 /* Stateless compression */
759 if ((lowpan6_buffer[1] & 0x30) == 0x00) {
760 /* copy full address */
761 MEMCPY(&ip6hdr->src.addr[0], lowpan6_buffer + lowpan6_offset, 16);
762 lowpan6_offset += 16;
763 } else if ((lowpan6_buffer[1] & 0x30) == 0x10) {
764 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
765 ip6hdr->src.addr[1] = 0;
766 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
767 lowpan6_offset += 8;
768 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
769 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
770 ip6hdr->src.addr[1] = 0;
771 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
772 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) |
773 lowpan6_buffer[lowpan6_offset+1]);
774 lowpan6_offset += 2;
775 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
776 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
777 ip6hdr->src.addr[1] = 0;
778 if (src->addr_len == 2) {
779 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
780 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
781 } else {
782 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) |
783 (src->addr[2] << 8) | src->addr[3]);
784 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) |
785 (src->addr[6] << 8) | src->addr[7]);
786 }
787 }
788 } else {
789 /* Stateful compression */
790 if ((lowpan6_buffer[1] & 0x30) == 0x00) {
791 /* ANY address */
792 ip6hdr->src.addr[0] = 0;
793 ip6hdr->src.addr[1] = 0;
794 ip6hdr->src.addr[2] = 0;
795 ip6hdr->src.addr[3] = 0;
796 } else {
797 /* Set prefix from context info */
798 if (lowpan6_buffer[1] & 0x80) {
799 i = (lowpan6_buffer[2] >> 4) & 0x0f;
800 } else {
801 i = 0;
802 }
803 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
804 /* Error */
805 pbuf_free(p);
806 pbuf_free(q);
807 return NULL;
808 }
809  
810 ip6hdr->src.addr[0] = lowpan6_context[i].addr[0];
811 ip6hdr->src.addr[1] = lowpan6_context[i].addr[1];
812 }
813  
814 if ((lowpan6_buffer[1] & 0x30) == 0x10) {
815 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
816 lowpan6_offset += 8;
817 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
818 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
819 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset+1]);
820 lowpan6_offset += 2;
821 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
822 if (src->addr_len == 2) {
823 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
824 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
825 } else {
826 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | (src->addr[2] << 8) | src->addr[3]);
827 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) | (src->addr[6] << 8) | src->addr[7]);
828 }
829 }
830 }
831  
832 /* Destination address decoding. */
833 if (lowpan6_buffer[1] & 0x08) {
834 /* Multicast destination */
835 if (lowpan6_buffer[1] & 0x04) {
836 /* @todo support stateful multicast addressing */
837 pbuf_free(p);
838 pbuf_free(q);
839 return NULL;
840 }
841  
842 if ((lowpan6_buffer[1] & 0x03) == 0x00) {
843 /* copy full address */
844 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
845 lowpan6_offset += 16;
846 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
847 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16));
848 ip6hdr->dest.addr[1] = 0;
849 ip6hdr->dest.addr[2] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
850 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]);
851 lowpan6_offset += 4;
852 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
853 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16));
854 ip6hdr->dest.addr[1] = 0;
855 ip6hdr->dest.addr[2] = 0;
856 ip6hdr->dest.addr[3] = lwip_htonl((lowpan6_buffer[lowpan6_offset] << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset + 2]);
857 lowpan6_offset += 3;
858 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
859 ip6hdr->dest.addr[0] = PP_HTONL(0xff020000UL);
860 ip6hdr->dest.addr[1] = 0;
861 ip6hdr->dest.addr[2] = 0;
862 ip6hdr->dest.addr[3] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
863 }
864  
865 } else {
866 if (lowpan6_buffer[1] & 0x04) {
867 /* Stateful destination compression */
868 /* Set prefix from context info */
869 if (lowpan6_buffer[1] & 0x80) {
870 i = lowpan6_buffer[2] & 0x0f;
871 } else {
872 i = 0;
873 }
874 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
875 /* Error */
876 pbuf_free(p);
877 pbuf_free(q);
878 return NULL;
879 }
880  
881 ip6hdr->dest.addr[0] = lowpan6_context[i].addr[0];
882 ip6hdr->dest.addr[1] = lowpan6_context[i].addr[1];
883 } else {
884 /* Link local address compression */
885 ip6hdr->dest.addr[0] = PP_HTONL(0xfe800000UL);
886 ip6hdr->dest.addr[1] = 0;
887 }
888  
889 if ((lowpan6_buffer[1] & 0x03) == 0x00) {
890 /* copy full address */
891 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
892 lowpan6_offset += 16;
893 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
894 MEMCPY(&ip6hdr->dest.addr[2], lowpan6_buffer + lowpan6_offset, 8);
895 lowpan6_offset += 8;
896 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
897 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
898 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]);
899 lowpan6_offset += 2;
900 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
901 if (dest->addr_len == 2) {
902 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
903 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (dest->addr[0] << 8) | dest->addr[1]);
904 } else {
905 ip6hdr->dest.addr[2] = lwip_htonl(((dest->addr[0] ^ 2) << 24) | (dest->addr[1] << 16) | dest->addr[2] << 8 | dest->addr[3]);
906 ip6hdr->dest.addr[3] = lwip_htonl((dest->addr[4] << 24) | (dest->addr[5] << 16) | dest->addr[6] << 8 | dest->addr[7]);
907 }
908 }
909 }
910  
911  
912 /* Next Header Compression (NHC) decoding? */
913 if (lowpan6_buffer[0] & 0x04) {
914 #if LWIP_UDP
915 if ((lowpan6_buffer[lowpan6_offset] & 0xf8) == 0xf0) {
916 struct udp_hdr *udphdr;
917  
918 /* UDP compression */
919 IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_UDP);
920 udphdr = (struct udp_hdr *)((u8_t *)q->payload + ip6_offset);
921  
922 if (lowpan6_buffer[lowpan6_offset] & 0x04) {
923 /* @todo support checksum decompress */
924 pbuf_free(p);
925 pbuf_free(q);
926 return NULL;
927 }
928  
929 /* Decompress ports */
930 i = lowpan6_buffer[lowpan6_offset++] & 0x03;
931 if (i == 0) {
932 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
933 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 2] << 8 | lowpan6_buffer[lowpan6_offset + 3]);
934 lowpan6_offset += 4;
935 } else if (i == 0x01) {
936 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
937 udphdr->dest = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset + 2]);
938 lowpan6_offset += 3;
939 } else if (i == 0x02) {
940 udphdr->src = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset]);
941 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 1] << 8 | lowpan6_buffer[lowpan6_offset + 2]);
942 lowpan6_offset += 3;
943 } else if (i == 0x03) {
944 udphdr->src = lwip_htons(0xf0b0 | ((lowpan6_buffer[lowpan6_offset] >> 4) & 0x0f));
945 udphdr->dest = lwip_htons(0xf0b0 | (lowpan6_buffer[lowpan6_offset] & 0x0f));
946 lowpan6_offset += 1;
947 }
948  
949 udphdr->chksum = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
950 lowpan6_offset += 2;
951 udphdr->len = lwip_htons(p->tot_len - lowpan6_offset + UDP_HLEN);
952  
953 ip6_offset += UDP_HLEN;
954 } else
955 #endif /* LWIP_UDP */
956 {
957 /* @todo support NHC other than UDP */
958 pbuf_free(p);
959 pbuf_free(q);
960 return NULL;
961 }
962 }
963  
964 /* Now we copy leftover contents from p to q, so we have all L2 and L3 headers (and L4?) in a single PBUF.
965 * Replace p with q, and free p */
966 pbuf_remove_header(p, lowpan6_offset);
967 MEMCPY((u8_t*)q->payload + ip6_offset, p->payload, p->len);
968 q->len = q->tot_len = ip6_offset + p->len;
969 if (p->next != NULL) {
970 pbuf_cat(q, p->next);
971 }
972 p->next = NULL;
973 pbuf_free(p);
974  
975 /* Infer IPv6 payload length for header */
976 IP6H_PLEN_SET(ip6hdr, q->tot_len - IP6_HLEN);
977  
978 /* all done */
979 return q;
980 }
981  
982 err_t
983 lowpan6_input(struct pbuf * p, struct netif *netif)
984 {
985 u8_t * puc;
986 s8_t i;
987 struct ieee_802154_addr src, dest;
988 u16_t datagram_size, datagram_offset, datagram_tag;
989 struct lowpan6_reass_helper *lrh, *lrh_temp;
990  
991 MIB2_STATS_NETIF_ADD(netif, ifinoctets, p->tot_len);
992  
993 /* Analyze header. @todo validate. */
994 puc = (u8_t*)p->payload;
995 datagram_offset = 5;
996 if ((puc[1] & 0x0c) == 0x0c) {
997 dest.addr_len = 8;
998 for (i = 0; i < 8; i++) {
999 dest.addr[i] = puc[datagram_offset + 7 - i];
1000 }
1001 datagram_offset += 8;
1002 } else {
1003 dest.addr_len = 2;
1004 dest.addr[0] = puc[datagram_offset + 1];
1005 dest.addr[1] = puc[datagram_offset];
1006 datagram_offset += 2;
1007 }
1008  
1009 datagram_offset += 2; /* skip PAN ID. */
1010  
1011 if ((puc[1] & 0xc0) == 0xc0) {
1012 src.addr_len = 8;
1013 for (i = 0; i < 8; i++) {
1014 src.addr[i] = puc[datagram_offset + 7 - i];
1015 }
1016 datagram_offset += 8;
1017 } else {
1018 src.addr_len = 2;
1019 src.addr[0] = puc[datagram_offset + 1];
1020 src.addr[1] = puc[datagram_offset];
1021 datagram_offset += 2;
1022 }
1023  
1024 pbuf_remove_header(p, datagram_offset); /* hide IEEE802.15.4 header. */
1025  
1026 /* Check dispatch. */
1027 puc = (u8_t*)p->payload;
1028  
1029 if ((*puc & 0xf8) == 0xc0) {
1030 /* FRAG1 dispatch. add this packet to reassembly list. */
1031 datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1];
1032 datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3];
1033  
1034 /* check for duplicate */
1035 lrh = reass_list;
1036 while (lrh != NULL) {
1037 if ((lrh->sender_addr.addr_len == src.addr_len) &&
1038 (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0)) {
1039 /* address match with packet in reassembly. */
1040 if ((datagram_tag == lrh->datagram_tag) && (datagram_size == lrh->datagram_size)) {
1041 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1042 /* duplicate fragment. */
1043 pbuf_free(p);
1044 return ERR_OK;
1045 } else {
1046 /* We are receiving the start of a new datagram. Discard old one (incomplete). */
1047 lrh_temp = lrh->next_packet;
1048 dequeue_datagram(lrh);
1049 pbuf_free(lrh->pbuf);
1050 mem_free(lrh);
1051  
1052 /* Check next datagram in queue. */
1053 lrh = lrh_temp;
1054 }
1055 } else {
1056 /* Check next datagram in queue. */
1057 lrh = lrh->next_packet;
1058 }
1059 }
1060  
1061 pbuf_remove_header(p, 4); /* hide frag1 dispatch */
1062  
1063 lrh = (struct lowpan6_reass_helper *) mem_malloc(sizeof(struct lowpan6_reass_helper));
1064 if (lrh == NULL) {
1065 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1066 pbuf_free(p);
1067 return ERR_MEM;
1068 }
1069  
1070 lrh->sender_addr.addr_len = src.addr_len;
1071 for (i = 0; i < src.addr_len; i++) {
1072 lrh->sender_addr.addr[i] = src.addr[i];
1073 }
1074 lrh->datagram_size = datagram_size;
1075 lrh->datagram_tag = datagram_tag;
1076 lrh->pbuf = p;
1077 lrh->next_packet = reass_list;
1078 lrh->timer = 2;
1079 reass_list = lrh;
1080  
1081 return ERR_OK;
1082 } else if ((*puc & 0xf8) == 0xe0) {
1083 /* FRAGN dispatch, find packet being reassembled. */
1084 datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1];
1085 datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3];
1086 datagram_offset = (u16_t)puc[4] << 3;
1087 pbuf_remove_header(p, 5); /* hide frag1 dispatch */
1088  
1089 for (lrh = reass_list; lrh != NULL; lrh = lrh->next_packet) {
1090 if ((lrh->sender_addr.addr_len == src.addr_len) &&
1091 (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0) &&
1092 (datagram_tag == lrh->datagram_tag) &&
1093 (datagram_size == lrh->datagram_size)) {
1094 break;
1095 }
1096 }
1097 if (lrh == NULL) {
1098 /* rogue fragment */
1099 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1100 pbuf_free(p);
1101 return ERR_OK;
1102 }
1103  
1104 if (lrh->pbuf->tot_len < datagram_offset) {
1105 /* duplicate, ignore. */
1106 pbuf_free(p);
1107 return ERR_OK;
1108 } else if (lrh->pbuf->tot_len > datagram_offset) {
1109 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1110 /* We have missed a fragment. Delete whole reassembly. */
1111 dequeue_datagram(lrh);
1112 pbuf_free(lrh->pbuf);
1113 mem_free(lrh);
1114 pbuf_free(p);
1115 return ERR_OK;
1116 }
1117 pbuf_cat(lrh->pbuf, p);
1118 p = NULL;
1119  
1120 /* is packet now complete?*/
1121 if (lrh->pbuf->tot_len >= lrh->datagram_size) {
1122 /* dequeue from reass list. */
1123 dequeue_datagram(lrh);
1124  
1125 /* get pbuf */
1126 p = lrh->pbuf;
1127  
1128 /* release helper */
1129 mem_free(lrh);
1130 } else {
1131 return ERR_OK;
1132 }
1133 }
1134  
1135 if (p == NULL) {
1136 return ERR_OK;
1137 }
1138  
1139 /* We have a complete packet, check dispatch for headers. */
1140 puc = (u8_t*)p->payload;
1141  
1142 if (*puc == 0x41) {
1143 /* This is a complete IPv6 packet, just skip dispatch byte. */
1144 pbuf_remove_header(p, 1); /* hide dispatch byte. */
1145 } else if ((*puc & 0xe0 )== 0x60) {
1146 /* IPv6 headers are compressed using IPHC. */
1147 p = lowpan6_decompress(p, &src, &dest);
1148 if (p == NULL) {
1149 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1150 return ERR_OK;
1151 }
1152 } else {
1153 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1154 pbuf_free(p);
1155 return ERR_OK;
1156 }
1157  
1158 /* @todo: distinguish unicast/multicast */
1159 MIB2_STATS_NETIF_INC(netif, ifinucastpkts);
1160  
1161 return ip6_input(p, netif);
1162 }
1163  
1164 err_t
1165 lowpan6_if_init(struct netif *netif)
1166 {
1167 netif->name[0] = 'L';
1168 netif->name[1] = '6';
1169 #if LWIP_IPV4
1170 netif->output = lowpan4_output;
1171 #endif /* LWIP_IPV4 */
1172 netif->output_ip6 = lowpan6_output;
1173  
1174 MIB2_INIT_NETIF(netif, snmp_ifType_other, 0);
1175  
1176 /* maximum transfer unit */
1177 netif->mtu = 1280;
1178  
1179 /* broadcast capability */
1180 netif->flags = NETIF_FLAG_BROADCAST /* | NETIF_FLAG_LOWPAN6 */;
1181  
1182 return ERR_OK;
1183 }
1184  
1185 err_t
1186 lowpan6_set_pan_id(u16_t pan_id)
1187 {
1188 ieee_802154_pan_id = pan_id;
1189  
1190 return ERR_OK;
1191 }
1192  
1193 #if !NO_SYS
1194 /**
1195 * Pass a received packet to tcpip_thread for input processing
1196 *
1197 * @param p the received packet, p->payload pointing to the
1198 * IEEE 802.15.4 header.
1199 * @param inp the network interface on which the packet was received
1200 */
1201 err_t
1202 tcpip_6lowpan_input(struct pbuf *p, struct netif *inp)
1203 {
1204 return tcpip_inpkt(p, inp, lowpan6_input);
1205 }
1206 #endif /* !NO_SYS */
1207  
1208 #endif /* LWIP_IPV6 && LWIP_6LOWPAN */