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