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 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_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 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 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 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 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 * 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 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 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 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 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