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 netif
44 * @ingroup addons
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
lowpan6_tmr(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
dequeue_datagram(struct lowpan6_reass_helper * lrh)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
lowpan6_context_lookup(const ip6_addr_t * ip6addr)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
lowpan6_get_address_mode(const ip6_addr_t * ip6addr,const struct ieee_802154_addr * mac_addr)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
lowpan6_get_address_mode_mc(const ip6_addr_t * ip6addr)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
lowpan6_frag(struct netif * netif,struct pbuf * p,const struct ieee_802154_addr * src,const struct ieee_802154_addr * dst)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_header(p, -IP6_HLEN);
415
416 /* Compress UDP header? */
417 if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) {
418 /* @todo support optional checksum compression */
419
420 buffer[ieee_header_len + lowpan6_header_len] = 0xf0;
421
422 /* determine port compression mode. */
423 if ((((u8_t *)p->payload)[0] == 0xf0) && ((((u8_t *)p->payload)[1] & 0xf0) == 0xb0) &&
424 (((u8_t *)p->payload)[2] == 0xf0) && ((((u8_t *)p->payload)[3] & 0xf0) == 0xb0)) {
425 /* Compress source and dest ports. */
426 buffer[ieee_header_len + lowpan6_header_len++] |= 0x03;
427 buffer[ieee_header_len + lowpan6_header_len++] = ((((u8_t *)p->payload)[1] & 0x0f) << 4) | (((u8_t *)p->payload)[3] & 0x0f);
428 } else if (((u8_t *)p->payload)[0] == 0xf0) {
429 /* Compress source port. */
430 buffer[ieee_header_len + lowpan6_header_len++] |= 0x02;
431 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1];
432 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2];
433 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3];
434 } else if (((u8_t *)p->payload)[2] == 0xf0) {
435 /* Compress dest port. */
436 buffer[ieee_header_len + lowpan6_header_len++] |= 0x01;
437 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0];
438 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1];
439 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3];
440 } else {
441 /* append full ports. */
442 lowpan6_header_len++;
443 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0];
444 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1];
445 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2];
446 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3];
447 }
448
449 /* elide length and copy checksum */
450 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[6];
451 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[7];
452
453 pbuf_header(p, -UDP_HLEN);
454 }
455 }
456
457 #else /* LWIP_6LOWPAN_HC */
458 /* Send uncompressed IPv6 header with appropriate dispatch byte. */
459 lowpan6_header_len = 1;
460 buffer[ieee_header_len] = 0x41; /* IPv6 dispatch */
461 #endif /* LWIP_6LOWPAN_HC */
462
463 /* Calculate remaining packet length */
464 remaining_len = p->tot_len;
465
466 if (remaining_len > 0x7FF) {
467 MIB2_STATS_NETIF_INC(netif, ifoutdiscards);
468 /* datagram_size must fit into 11 bit */
469 pbuf_free(p_frag);
470 return ERR_VAL;
471 }
472
473 /* Fragment, or 1 packet? */
474 if (remaining_len > (127 - ieee_header_len - lowpan6_header_len - 3)) { /* 127 - header - 1 byte dispatch - 2 bytes CRC */
475 /* We must move the 6LowPAN header to make room for the FRAG header. */
476 i = lowpan6_header_len;
477 while (i-- != 0) {
478 buffer[ieee_header_len + i + 4] = buffer[ieee_header_len + i];
479 }
480
481 /* Now we need to fragment the packet. FRAG1 header first */
482 buffer[ieee_header_len] = 0xc0 | (((p->tot_len + lowpan6_header_len) >> 8) & 0x7);
483 buffer[ieee_header_len + 1] = (p->tot_len + lowpan6_header_len) & 0xff;
484
485 datagram_tag++;
486 buffer[ieee_header_len + 2] = datagram_tag & 0xff;
487 buffer[ieee_header_len + 3] = (datagram_tag >> 8) & 0xff;
488
489 /* Fragment follows. */
490 frag_len = (127 - ieee_header_len - 4 - 2) & 0xf8;
491
492 pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len + 4, frag_len - lowpan6_header_len, 0);
493 remaining_len -= frag_len - lowpan6_header_len;
494 datagram_offset = frag_len;
495
496 /* 2 bytes CRC */
497 #if LWIP_6LOWPAN_HW_CRC
498 /* Leave blank, will be filled by HW. */
499 #else /* LWIP_6LOWPAN_HW_CRC */
500 /* @todo calculate CRC */
501 #endif /* LWIP_6LOWPAN_HW_CRC */
502
503 /* Calculate frame length */
504 p_frag->len = p_frag->tot_len = ieee_header_len + 4 + frag_len + 2; /* add 2 dummy bytes for crc*/
505
506 /* send the packet */
507 MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len);
508 LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p));
509 err = netif->linkoutput(netif, p_frag);
510
511 while ((remaining_len > 0) && (err == ERR_OK)) {
512 /* new frame, new seq num for ACK */
513 buffer[2] = frame_seq_num++;
514
515 buffer[ieee_header_len] |= 0x20; /* Change FRAG1 to FRAGN */
516
517 buffer[ieee_header_len + 4] = (u8_t)(datagram_offset >> 3); /* datagram offset in FRAGN header (datagram_offset is max. 11 bit) */
518
519 frag_len = (127 - ieee_header_len - 5 - 2) & 0xf8;
520 if (frag_len > remaining_len) {
521 frag_len = remaining_len;
522 }
523
524 pbuf_copy_partial(p, buffer + ieee_header_len + 5, frag_len, p->tot_len - remaining_len);
525 remaining_len -= frag_len;
526 datagram_offset += frag_len;
527
528 /* 2 bytes CRC */
529 #if LWIP_6LOWPAN_HW_CRC
530 /* Leave blank, will be filled by HW. */
531 #else /* LWIP_6LOWPAN_HW_CRC */
532 /* @todo calculate CRC */
533 #endif /* LWIP_6LOWPAN_HW_CRC */
534
535 /* Calculate frame length */
536 p_frag->len = p_frag->tot_len = frag_len + 5 + ieee_header_len + 2;
537
538 /* send the packet */
539 MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len);
540 LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p));
541 err = netif->linkoutput(netif, p_frag);
542 }
543 } else {
544 /* It fits in one frame. */
545 frag_len = remaining_len;
546
547 /* Copy IPv6 packet */
548 pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len, frag_len, 0);
549 remaining_len = 0;
550
551 /* 2 bytes CRC */
552 #if LWIP_6LOWPAN_HW_CRC
553 /* Leave blank, will be filled by HW. */
554 #else /* LWIP_6LOWPAN_HW_CRC */
555 /* @todo calculate CRC */
556 #endif /* LWIP_6LOWPAN_HW_CRC */
557
558 /* Calculate frame length */
559 p_frag->len = p_frag->tot_len = frag_len + lowpan6_header_len + ieee_header_len + 2;
560
561 /* send the packet */
562 MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len);
563 LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p));
564 err = netif->linkoutput(netif, p_frag);
565 }
566
567 pbuf_free(p_frag);
568
569 return err;
570 }
571
572 err_t
lowpan6_set_context(u8_t idx,const ip6_addr_t * context)573 lowpan6_set_context(u8_t idx, const ip6_addr_t * context)
574 {
575 if (idx >= LWIP_6LOWPAN_NUM_CONTEXTS) {
576 return ERR_ARG;
577 }
578
579 IP6_ADDR_ZONECHECK(context);
580
581 ip6_addr_set(&lowpan6_context[idx], context);
582
583 return ERR_OK;
584 }
585
586 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS
587 err_t
lowpan6_set_short_addr(u8_t addr_high,u8_t addr_low)588 lowpan6_set_short_addr(u8_t addr_high, u8_t addr_low)
589 {
590 short_mac_addr.addr[0] = addr_high;
591 short_mac_addr.addr[1] = addr_low;
592
593 return ERR_OK;
594 }
595 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */
596
597 #if LWIP_IPV4
598 err_t
lowpan4_output(struct netif * netif,struct pbuf * q,const ip4_addr_t * ipaddr)599 lowpan4_output(struct netif *netif, struct pbuf *q, const ip4_addr_t *ipaddr)
600 {
601 (void)netif;
602 (void)q;
603 (void)ipaddr;
604
605 return ERR_IF;
606 }
607 #endif /* LWIP_IPV4 */
608
609 /**
610 * Resolve and fill-in IEEE 802.15.4 address header for outgoing IPv6 packet.
611 *
612 * Perform Header Compression and fragment if necessary.
613 *
614 * @param netif The lwIP network interface which the IP packet will be sent on.
615 * @param q The pbuf(s) containing the IP packet to be sent.
616 * @param ip6addr The IP address of the packet destination.
617 *
618 * @return err_t
619 */
620 err_t
lowpan6_output(struct netif * netif,struct pbuf * q,const ip6_addr_t * ip6addr)621 lowpan6_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr)
622 {
623 err_t result;
624 const u8_t *hwaddr;
625 struct ieee_802154_addr src, dest;
626 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS
627 ip6_addr_t ip6_src;
628 struct ip6_hdr * ip6_hdr;
629 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */
630
631 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS
632 /* Check if we can compress source address (use aligned copy) */
633 ip6_hdr = (struct ip6_hdr *)q->payload;
634 ip6_addr_copy_from_packed(ip6_src, ip6_hdr->src);
635 ip6_addr_assign_zone(&ip6_src, IP6_UNICAST, netif);
636 if (lowpan6_get_address_mode(&ip6_src, &short_mac_addr) == 3) {
637 src.addr_len = 2;
638 src.addr[0] = short_mac_addr.addr[0];
639 src.addr[1] = short_mac_addr.addr[1];
640 } else
641 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */
642 {
643 src.addr_len = netif->hwaddr_len;
644 SMEMCPY(src.addr, netif->hwaddr, netif->hwaddr_len);
645 }
646
647 /* multicast destination IP address? */
648 if (ip6_addr_ismulticast(ip6addr)) {
649 MIB2_STATS_NETIF_INC(netif, ifoutnucastpkts);
650 /* We need to send to the broadcast address.*/
651 return lowpan6_frag(netif, q, &src, &ieee_802154_broadcast);
652 }
653
654 /* We have a unicast destination IP address */
655 /* @todo anycast? */
656
657 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS
658 if (src.addr_len == 2) {
659 /* If source address was compressable to short_mac_addr, and dest has same subnet and
660 * is also compressable to 2-bytes, assume we can infer dest as a short address too. */
661 dest.addr_len = 2;
662 dest.addr[0] = ((u8_t *)q->payload)[38];
663 dest.addr[1] = ((u8_t *)q->payload)[39];
664 if ((src.addr_len == 2) && (ip6_addr_netcmp_zoneless(&ip6_hdr->src, &ip6_hdr->dest)) &&
665 (lowpan6_get_address_mode(ip6addr, &dest) == 3)) {
666 MIB2_STATS_NETIF_INC(netif, ifoutucastpkts);
667 return lowpan6_frag(netif, q, &src, &dest);
668 }
669 }
670 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */
671
672 /* Ask ND6 what to do with the packet. */
673 result = nd6_get_next_hop_addr_or_queue(netif, q, ip6addr, &hwaddr);
674 if (result != ERR_OK) {
675 MIB2_STATS_NETIF_INC(netif, ifoutdiscards);
676 return result;
677 }
678
679 /* If no hardware address is returned, nd6 has queued the packet for later. */
680 if (hwaddr == NULL) {
681 return ERR_OK;
682 }
683
684 /* Send out the packet using the returned hardware address. */
685 dest.addr_len = netif->hwaddr_len;
686 SMEMCPY(dest.addr, hwaddr, netif->hwaddr_len);
687 MIB2_STATS_NETIF_INC(netif, ifoutucastpkts);
688 return lowpan6_frag(netif, q, &src, &dest);
689 }
690
691 static struct pbuf *
lowpan6_decompress(struct pbuf * p,struct ieee_802154_addr * src,struct ieee_802154_addr * dest)692 lowpan6_decompress(struct pbuf * p, struct ieee_802154_addr * src, struct ieee_802154_addr * dest)
693 {
694 struct pbuf * q;
695 u8_t * lowpan6_buffer;
696 s8_t lowpan6_offset;
697 struct ip6_hdr *ip6hdr;
698 s8_t i;
699 s8_t ip6_offset = IP6_HLEN;
700
701
702 q = pbuf_alloc(PBUF_IP, p->len + IP6_HLEN + UDP_HLEN, PBUF_POOL);
703 if (q == NULL) {
704 pbuf_free(p);
705 return NULL;
706 }
707
708 lowpan6_buffer = (u8_t *)p->payload;
709 ip6hdr = (struct ip6_hdr *)q->payload;
710
711 lowpan6_offset = 2;
712 if (lowpan6_buffer[1] & 0x80) {
713 lowpan6_offset++;
714 }
715
716 /* Set IPv6 version, traffic class and flow label. */
717 if ((lowpan6_buffer[0] & 0x18) == 0x00) {
718 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]);
719 lowpan6_offset += 4;
720 } else if ((lowpan6_buffer[0] & 0x18) == 0x08) {
721 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]);
722 lowpan6_offset += 3;
723 } else if ((lowpan6_buffer[0] & 0x18) == 0x10) {
724 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset],0);
725 lowpan6_offset += 1;
726 } else if ((lowpan6_buffer[0] & 0x18) == 0x18) {
727 IP6H_VTCFL_SET(ip6hdr, 6, 0, 0);
728 }
729
730 /* Set Next Header */
731 if ((lowpan6_buffer[0] & 0x04) == 0x00) {
732 IP6H_NEXTH_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
733 } else {
734 /* We should fill this later with NHC decoding */
735 IP6H_NEXTH_SET(ip6hdr, 0);
736 }
737
738 /* Set Hop Limit */
739 if ((lowpan6_buffer[0] & 0x03) == 0x00) {
740 IP6H_HOPLIM_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]);
741 } else if ((lowpan6_buffer[0] & 0x03) == 0x01) {
742 IP6H_HOPLIM_SET(ip6hdr, 1);
743 } else if ((lowpan6_buffer[0] & 0x03) == 0x02) {
744 IP6H_HOPLIM_SET(ip6hdr, 64);
745 } else if ((lowpan6_buffer[0] & 0x03) == 0x03) {
746 IP6H_HOPLIM_SET(ip6hdr, 255);
747 }
748
749 /* Source address decoding. */
750 if ((lowpan6_buffer[1] & 0x40) == 0x00) {
751 /* Stateless compression */
752 if ((lowpan6_buffer[1] & 0x30) == 0x00) {
753 /* copy full address */
754 MEMCPY(&ip6hdr->src.addr[0], lowpan6_buffer + lowpan6_offset, 16);
755 lowpan6_offset += 16;
756 } else if ((lowpan6_buffer[1] & 0x30) == 0x10) {
757 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
758 ip6hdr->src.addr[1] = 0;
759 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
760 lowpan6_offset += 8;
761 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
762 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
763 ip6hdr->src.addr[1] = 0;
764 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
765 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) |
766 lowpan6_buffer[lowpan6_offset+1]);
767 lowpan6_offset += 2;
768 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
769 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL);
770 ip6hdr->src.addr[1] = 0;
771 if (src->addr_len == 2) {
772 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
773 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
774 } else {
775 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) |
776 (src->addr[2] << 8) | src->addr[3]);
777 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) |
778 (src->addr[6] << 8) | src->addr[7]);
779 }
780 }
781 } else {
782 /* Stateful compression */
783 if ((lowpan6_buffer[1] & 0x30) == 0x00) {
784 /* ANY address */
785 ip6hdr->src.addr[0] = 0;
786 ip6hdr->src.addr[1] = 0;
787 ip6hdr->src.addr[2] = 0;
788 ip6hdr->src.addr[3] = 0;
789 } else {
790 /* Set prefix from context info */
791 if (lowpan6_buffer[1] & 0x80) {
792 i = (lowpan6_buffer[2] >> 4) & 0x0f;
793 } else {
794 i = 0;
795 }
796 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
797 /* Error */
798 pbuf_free(p);
799 pbuf_free(q);
800 return NULL;
801 }
802
803 ip6hdr->src.addr[0] = lowpan6_context[i].addr[0];
804 ip6hdr->src.addr[1] = lowpan6_context[i].addr[1];
805 }
806
807 if ((lowpan6_buffer[1] & 0x30) == 0x10) {
808 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8);
809 lowpan6_offset += 8;
810 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) {
811 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
812 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset+1]);
813 lowpan6_offset += 2;
814 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) {
815 if (src->addr_len == 2) {
816 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL);
817 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]);
818 } else {
819 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | (src->addr[2] << 8) | src->addr[3]);
820 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) | (src->addr[6] << 8) | src->addr[7]);
821 }
822 }
823 }
824
825 /* Destination address decoding. */
826 if (lowpan6_buffer[1] & 0x08) {
827 /* Multicast destination */
828 if (lowpan6_buffer[1] & 0x04) {
829 /* @todo support stateful multicast addressing */
830 pbuf_free(p);
831 pbuf_free(q);
832 return NULL;
833 }
834
835 if ((lowpan6_buffer[1] & 0x03) == 0x00) {
836 /* copy full address */
837 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
838 lowpan6_offset += 16;
839 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
840 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16));
841 ip6hdr->dest.addr[1] = 0;
842 ip6hdr->dest.addr[2] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
843 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]);
844 lowpan6_offset += 4;
845 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
846 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | lowpan6_buffer[lowpan6_offset++]);
847 ip6hdr->dest.addr[1] = 0;
848 ip6hdr->dest.addr[2] = 0;
849 ip6hdr->dest.addr[3] = lwip_htonl((lowpan6_buffer[lowpan6_offset] << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset + 2]);
850 lowpan6_offset += 3;
851 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
852 ip6hdr->dest.addr[0] = PP_HTONL(0xff020000UL);
853 ip6hdr->dest.addr[1] = 0;
854 ip6hdr->dest.addr[2] = 0;
855 ip6hdr->dest.addr[3] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]);
856 }
857
858 } else {
859 if (lowpan6_buffer[1] & 0x04) {
860 /* Stateful destination compression */
861 /* Set prefix from context info */
862 if (lowpan6_buffer[1] & 0x80) {
863 i = lowpan6_buffer[2] & 0x0f;
864 } else {
865 i = 0;
866 }
867 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) {
868 /* Error */
869 pbuf_free(p);
870 pbuf_free(q);
871 return NULL;
872 }
873
874 ip6hdr->dest.addr[0] = lowpan6_context[i].addr[0];
875 ip6hdr->dest.addr[1] = lowpan6_context[i].addr[1];
876 } else {
877 /* Link local address compression */
878 ip6hdr->dest.addr[0] = PP_HTONL(0xfe800000UL);
879 ip6hdr->dest.addr[1] = 0;
880 }
881
882 if ((lowpan6_buffer[1] & 0x03) == 0x00) {
883 /* copy full address */
884 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16);
885 lowpan6_offset += 16;
886 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) {
887 MEMCPY(&ip6hdr->dest.addr[2], lowpan6_buffer + lowpan6_offset, 8);
888 lowpan6_offset += 8;
889 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) {
890 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
891 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]);
892 lowpan6_offset += 2;
893 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) {
894 if (dest->addr_len == 2) {
895 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL);
896 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (dest->addr[0] << 8) | dest->addr[1]);
897 } else {
898 ip6hdr->dest.addr[2] = lwip_htonl(((dest->addr[0] ^ 2) << 24) | (dest->addr[1] << 16) | dest->addr[2] << 8 | dest->addr[3]);
899 ip6hdr->dest.addr[3] = lwip_htonl((dest->addr[4] << 24) | (dest->addr[5] << 16) | dest->addr[6] << 8 | dest->addr[7]);
900 }
901 }
902 }
903
904
905 /* Next Header Compression (NHC) decoding? */
906 if (lowpan6_buffer[0] & 0x04) {
907 if ((lowpan6_buffer[lowpan6_offset] & 0xf8) == 0xf0) {
908 struct udp_hdr *udphdr;
909
910 /* UDP compression */
911 IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_UDP);
912 udphdr = (struct udp_hdr *)((u8_t *)q->payload + ip6_offset);
913
914 if (lowpan6_buffer[lowpan6_offset] & 0x04) {
915 /* @todo support checksum decompress */
916 pbuf_free(p);
917 pbuf_free(q);
918 return NULL;
919 }
920
921 /* Decompress ports */
922 i = lowpan6_buffer[lowpan6_offset++] & 0x03;
923 if (i == 0) {
924 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
925 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 2] << 8 | lowpan6_buffer[lowpan6_offset + 3]);
926 lowpan6_offset += 4;
927 } else if (i == 0x01) {
928 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
929 udphdr->dest = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset + 2]);
930 lowpan6_offset += 3;
931 } else if (i == 0x02) {
932 udphdr->src = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset]);
933 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 1] << 8 | lowpan6_buffer[lowpan6_offset + 2]);
934 lowpan6_offset += 3;
935 } else if (i == 0x03) {
936 udphdr->src = lwip_htons(0xf0b0 | ((lowpan6_buffer[lowpan6_offset] >> 4) & 0x0f));
937 udphdr->dest = lwip_htons(0xf0b0 | (lowpan6_buffer[lowpan6_offset] & 0x0f));
938 lowpan6_offset += 1;
939 }
940
941 udphdr->chksum = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]);
942 lowpan6_offset += 2;
943 udphdr->len = lwip_htons(p->tot_len - lowpan6_offset + UDP_HLEN);
944
945 ip6_offset += UDP_HLEN;
946 } else {
947 /* @todo support NHC other than UDP */
948 pbuf_free(p);
949 pbuf_free(q);
950 return NULL;
951 }
952 }
953
954 /* Now we copy leftover contents from p to q, so we have all L2 and L3 headers (and L4?) in a single PBUF.
955 * Replace p with q, and free p */
956 pbuf_header(p, -lowpan6_offset);
957 MEMCPY((u8_t*)q->payload + ip6_offset, p->payload, p->len);
958 q->len = q->tot_len = ip6_offset + p->len;
959 if (p->next != NULL) {
960 pbuf_cat(q, p->next);
961 }
962 p->next = NULL;
963 pbuf_free(p);
964
965 /* Infer IPv6 payload length for header */
966 IP6H_PLEN_SET(ip6hdr, q->tot_len - IP6_HLEN);
967
968 /* all done */
969 return q;
970 }
971
972 err_t
lowpan6_input(struct pbuf * p,struct netif * netif)973 lowpan6_input(struct pbuf * p, struct netif *netif)
974 {
975 u8_t * puc;
976 s8_t i;
977 struct ieee_802154_addr src, dest;
978 u16_t datagram_size, datagram_offset, datagram_tag;
979 struct lowpan6_reass_helper *lrh, *lrh_temp;
980
981 MIB2_STATS_NETIF_ADD(netif, ifinoctets, p->tot_len);
982
983 /* Analyze header. @todo validate. */
984 puc = (u8_t*)p->payload;
985 datagram_offset = 5;
986 if ((puc[1] & 0x0c) == 0x0c) {
987 dest.addr_len = 8;
988 for (i = 0; i < 8; i++) {
989 dest.addr[i] = puc[datagram_offset + 7 - i];
990 }
991 datagram_offset += 8;
992 } else {
993 dest.addr_len = 2;
994 dest.addr[0] = puc[datagram_offset + 1];
995 dest.addr[1] = puc[datagram_offset];
996 datagram_offset += 2;
997 }
998
999 datagram_offset += 2; /* skip PAN ID. */
1000
1001 if ((puc[1] & 0xc0) == 0xc0) {
1002 src.addr_len = 8;
1003 for (i = 0; i < 8; i++) {
1004 src.addr[i] = puc[datagram_offset + 7 - i];
1005 }
1006 datagram_offset += 8;
1007 } else {
1008 src.addr_len = 2;
1009 src.addr[0] = puc[datagram_offset + 1];
1010 src.addr[1] = puc[datagram_offset];
1011 datagram_offset += 2;
1012 }
1013
1014 pbuf_header(p, -datagram_offset); /* hide IEEE802.15.4 header. */
1015
1016 /* Check dispatch. */
1017 puc = (u8_t*)p->payload;
1018
1019 if ((*puc & 0xf8) == 0xc0) {
1020 /* FRAG1 dispatch. add this packet to reassembly list. */
1021 datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1];
1022 datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3];
1023
1024 /* check for duplicate */
1025 lrh = reass_list;
1026 while (lrh != NULL) {
1027 if ((lrh->sender_addr.addr_len == src.addr_len) &&
1028 (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0)) {
1029 /* address match with packet in reassembly. */
1030 if ((datagram_tag == lrh->datagram_tag) && (datagram_size == lrh->datagram_size)) {
1031 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1032 /* duplicate fragment. */
1033 pbuf_free(p);
1034 return ERR_OK;
1035 } else {
1036 /* We are receiving the start of a new datagram. Discard old one (incomplete). */
1037 lrh_temp = lrh->next_packet;
1038 dequeue_datagram(lrh);
1039 pbuf_free(lrh->pbuf);
1040 mem_free(lrh);
1041
1042 /* Check next datagram in queue. */
1043 lrh = lrh_temp;
1044 }
1045 } else {
1046 /* Check next datagram in queue. */
1047 lrh = lrh->next_packet;
1048 }
1049 }
1050
1051 pbuf_header(p, -4); /* hide frag1 dispatch */
1052
1053 lrh = (struct lowpan6_reass_helper *) mem_malloc(sizeof(struct lowpan6_reass_helper));
1054 if (lrh == NULL) {
1055 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1056 pbuf_free(p);
1057 return ERR_MEM;
1058 }
1059
1060 lrh->sender_addr.addr_len = src.addr_len;
1061 for (i = 0; i < src.addr_len; i++) {
1062 lrh->sender_addr.addr[i] = src.addr[i];
1063 }
1064 lrh->datagram_size = datagram_size;
1065 lrh->datagram_tag = datagram_tag;
1066 lrh->pbuf = p;
1067 lrh->next_packet = reass_list;
1068 lrh->timer = 2;
1069 reass_list = lrh;
1070
1071 return ERR_OK;
1072 } else if ((*puc & 0xf8) == 0xe0) {
1073 /* FRAGN dispatch, find packet being reassembled. */
1074 datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1];
1075 datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3];
1076 datagram_offset = (u16_t)puc[4] << 3;
1077 pbuf_header(p, -5); /* hide frag1 dispatch */
1078
1079 for (lrh = reass_list; lrh != NULL; lrh = lrh->next_packet) {
1080 if ((lrh->sender_addr.addr_len == src.addr_len) &&
1081 (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0) &&
1082 (datagram_tag == lrh->datagram_tag) &&
1083 (datagram_size == lrh->datagram_size)) {
1084 break;
1085 }
1086 }
1087 if (lrh == NULL) {
1088 /* rogue fragment */
1089 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1090 pbuf_free(p);
1091 return ERR_OK;
1092 }
1093
1094 if (lrh->pbuf->tot_len < datagram_offset) {
1095 /* duplicate, ignore. */
1096 pbuf_free(p);
1097 return ERR_OK;
1098 } else if (lrh->pbuf->tot_len > datagram_offset) {
1099 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1100 /* We have missed a fragment. Delete whole reassembly. */
1101 dequeue_datagram(lrh);
1102 pbuf_free(lrh->pbuf);
1103 mem_free(lrh);
1104 pbuf_free(p);
1105 return ERR_OK;
1106 }
1107 pbuf_cat(lrh->pbuf, p);
1108 p = NULL;
1109
1110 /* is packet now complete?*/
1111 if (lrh->pbuf->tot_len >= lrh->datagram_size) {
1112 /* dequeue from reass list. */
1113 dequeue_datagram(lrh);
1114
1115 /* get pbuf */
1116 p = lrh->pbuf;
1117
1118 /* release helper */
1119 mem_free(lrh);
1120 } else {
1121 return ERR_OK;
1122 }
1123 }
1124
1125 if (p == NULL) {
1126 return ERR_OK;
1127 }
1128
1129 /* We have a complete packet, check dispatch for headers. */
1130 puc = (u8_t*)p->payload;
1131
1132 if (*puc == 0x41) {
1133 /* This is a complete IPv6 packet, just skip dispatch byte. */
1134 pbuf_header(p, -1); /* hide dispatch byte. */
1135 } else if ((*puc & 0xe0 )== 0x60) {
1136 /* IPv6 headers are compressed using IPHC. */
1137 p = lowpan6_decompress(p, &src, &dest);
1138 if (p == NULL) {
1139 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1140 return ERR_OK;
1141 }
1142 } else {
1143 MIB2_STATS_NETIF_INC(netif, ifindiscards);
1144 pbuf_free(p);
1145 return ERR_OK;
1146 }
1147
1148 /* @todo: distinguish unicast/multicast */
1149 MIB2_STATS_NETIF_INC(netif, ifinucastpkts);
1150
1151 return ip6_input(p, netif);
1152 }
1153
1154 err_t
lowpan6_if_init(struct netif * netif)1155 lowpan6_if_init(struct netif *netif)
1156 {
1157 netif->name[0] = 'L';
1158 netif->name[1] = '6';
1159 #if LWIP_IPV4
1160 netif->output = lowpan4_output;
1161 #endif /* LWIP_IPV4 */
1162 netif->output_ip6 = lowpan6_output;
1163
1164 MIB2_INIT_NETIF(netif, snmp_ifType_other, 0);
1165
1166 /* maximum transfer unit */
1167 netif->mtu = 1280;
1168
1169 /* broadcast capability */
1170 netif->flags = NETIF_FLAG_BROADCAST /* | NETIF_FLAG_LOWPAN6 */;
1171
1172 return ERR_OK;
1173 }
1174
1175 err_t
lowpan6_set_pan_id(u16_t pan_id)1176 lowpan6_set_pan_id(u16_t pan_id)
1177 {
1178 ieee_802154_pan_id = pan_id;
1179
1180 return ERR_OK;
1181 }
1182
1183 #if !NO_SYS
1184 /**
1185 * Pass a received packet to tcpip_thread for input processing
1186 *
1187 * @param p the received packet, p->payload pointing to the
1188 * IEEE 802.15.4 header.
1189 * @param inp the network interface on which the packet was received
1190 */
1191 err_t
tcpip_6lowpan_input(struct pbuf * p,struct netif * inp)1192 tcpip_6lowpan_input(struct pbuf *p, struct netif *inp)
1193 {
1194 return tcpip_inpkt(p, inp, lowpan6_input);
1195 }
1196 #endif /* !NO_SYS */
1197
1198 #endif /* LWIP_IPV6 && LWIP_6LOWPAN */
1199