third_party/lwip/repo/lwip/src/netif/lowpan6.c

   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(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
 570 err_t
 571 lowpan6_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
 583 err_t
 584 lowpan6_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
 594 err_t
 595 lowpan4_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  */
 616 err_t
 617 lowpan6_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
 686 static struct pbuf *
 687 lowpan6_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
 967 err_t
 968 lowpan6_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
1149 err_t
1150 lowpan6_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
1170 err_t
1171 lowpan6_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  */
1186 err_t
1187 tcpip_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 */