Marco Zecchini
/
Example_RTOS
Rtos API example
Embed:
(wiki syntax)
Show/hide line numbers
lwip_lowpan6.c
Go to the documentation of this file.
00001 /** 00002 * @file 00003 * 00004 * 6LowPAN output for IPv6. Uses ND tables for link-layer addressing. Fragments packets to 6LowPAN units. 00005 */ 00006 00007 /* 00008 * Copyright (c) 2015 Inico Technologies Ltd. 00009 * All rights reserved. 00010 * 00011 * Redistribution and use in source and binary forms, with or without modification, 00012 * are permitted provided that the following conditions are met: 00013 * 00014 * 1. Redistributions of source code must retain the above copyright notice, 00015 * this list of conditions and the following disclaimer. 00016 * 2. Redistributions in binary form must reproduce the above copyright notice, 00017 * this list of conditions and the following disclaimer in the documentation 00018 * and/or other materials provided with the distribution. 00019 * 3. The name of the author may not be used to endorse or promote products 00020 * derived from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 00023 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 00024 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 00025 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 00026 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 00027 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00028 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00029 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 00030 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 00031 * OF SUCH DAMAGE. 00032 * 00033 * This file is part of the lwIP TCP/IP stack. 00034 * 00035 * Author: Ivan Delamer <delamer@inicotech.com> 00036 * 00037 * 00038 * Please coordinate changes and requests with Ivan Delamer 00039 * <delamer@inicotech.com> 00040 */ 00041 00042 /** 00043 * @defgroup sixlowpan 6LowPAN netif 00044 * @ingroup addons 00045 * 6LowPAN netif implementation 00046 */ 00047 00048 #include "netif/lowpan6.h" 00049 00050 #if LWIP_IPV6 && LWIP_6LOWPAN 00051 00052 #include "lwip/ip.h" 00053 #include "lwip/pbuf.h" 00054 #include "lwip/ip_addr.h" 00055 #include "lwip/netif.h" 00056 #include "lwip/nd6.h" 00057 #include "lwip/mem.h" 00058 #include "lwip/udp.h" 00059 #include "lwip/tcpip.h" 00060 #include "lwip/snmp.h" 00061 00062 #include <string.h> 00063 00064 struct ieee_802154_addr { 00065 u8_t addr_len; 00066 u8_t addr[8]; 00067 }; 00068 00069 /** This is a helper struct. 00070 */ 00071 struct lowpan6_reass_helper { 00072 struct pbuf *pbuf; 00073 struct lowpan6_reass_helper *next_packet; 00074 u8_t timer; 00075 struct ieee_802154_addr sender_addr; 00076 u16_t datagram_size; 00077 u16_t datagram_tag; 00078 }; 00079 00080 static struct lowpan6_reass_helper * reass_list; 00081 00082 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0 00083 static ip6_addr_t lowpan6_context[LWIP_6LOWPAN_NUM_CONTEXTS]; 00084 #endif 00085 00086 static u16_t ieee_802154_pan_id; 00087 00088 static const struct ieee_802154_addr ieee_802154_broadcast = {2, {0xff, 0xff}}; 00089 00090 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS 00091 static struct ieee_802154_addr short_mac_addr = {2, {0,0}}; 00092 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ 00093 00094 static err_t dequeue_datagram(struct lowpan6_reass_helper *lrh); 00095 00096 /** 00097 * Periodic timer for 6LowPAN functions: 00098 * 00099 * - Remove incomplete/old packets 00100 */ 00101 void 00102 lowpan6_tmr(void) 00103 { 00104 struct lowpan6_reass_helper *lrh, *lrh_temp; 00105 00106 lrh = reass_list; 00107 while (lrh != NULL) { 00108 lrh_temp = lrh->next_packet; 00109 if ((--lrh->timer) == 0) { 00110 dequeue_datagram(lrh); 00111 pbuf_free(lrh->pbuf); 00112 mem_free(lrh); 00113 } 00114 lrh = lrh_temp; 00115 } 00116 } 00117 00118 /** 00119 * Removes a datagram from the reassembly queue. 00120 **/ 00121 static err_t 00122 dequeue_datagram(struct lowpan6_reass_helper *lrh) 00123 { 00124 struct lowpan6_reass_helper *lrh_temp; 00125 00126 if (reass_list == lrh) { 00127 reass_list = reass_list->next_packet; 00128 } else { 00129 lrh_temp = reass_list; 00130 while (lrh_temp != NULL) { 00131 if (lrh_temp->next_packet == lrh) { 00132 lrh_temp->next_packet = lrh->next_packet; 00133 break; 00134 } 00135 lrh_temp = lrh_temp->next_packet; 00136 } 00137 } 00138 00139 return ERR_OK; 00140 } 00141 00142 static s8_t 00143 lowpan6_context_lookup(const ip6_addr_t *ip6addr) 00144 { 00145 s8_t i; 00146 00147 for (i = 0; i < LWIP_6LOWPAN_NUM_CONTEXTS; i++) { 00148 if (ip6_addr_netcmp(&lowpan6_context[i], ip6addr)) { 00149 return i; 00150 } 00151 } 00152 00153 return -1; 00154 } 00155 00156 /* Determine compression mode for unicast address. */ 00157 static s8_t 00158 lowpan6_get_address_mode(const ip6_addr_t *ip6addr, const struct ieee_802154_addr *mac_addr) 00159 { 00160 if (mac_addr->addr_len == 2) { 00161 if ((ip6addr->addr[2] == (u32_t)PP_HTONL(0x000000ff)) && 00162 ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000))) { 00163 if ((ip6addr->addr[3] & PP_HTONL(0x0000ffff)) == lwip_ntohl((mac_addr->addr[0] << 8) | mac_addr->addr[1])) { 00164 return 3; 00165 } 00166 } 00167 } else if (mac_addr->addr_len == 8) { 00168 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])) && 00169 (ip6addr->addr[3] == lwip_ntohl((mac_addr->addr[4] << 24) | (mac_addr->addr[5] << 16) | mac_addr->addr[6] << 8 | mac_addr->addr[7]))) { 00170 return 3; 00171 } 00172 } 00173 00174 if ((ip6addr->addr[2] == PP_HTONL(0x000000ffUL)) && 00175 ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000UL))) { 00176 return 2; 00177 } 00178 00179 return 1; 00180 } 00181 00182 /* Determine compression mode for multicast address. */ 00183 static s8_t 00184 lowpan6_get_address_mode_mc(const ip6_addr_t *ip6addr) 00185 { 00186 if ((ip6addr->addr[0] == PP_HTONL(0xff020000)) && 00187 (ip6addr->addr[1] == 0) && 00188 (ip6addr->addr[2] == 0) && 00189 ((ip6addr->addr[3] & PP_HTONL(0xffffff00)) == 0)) { 00190 return 3; 00191 } else if (((ip6addr->addr[0] & PP_HTONL(0xff00ffff)) == PP_HTONL(0xff000000)) && 00192 (ip6addr->addr[1] == 0)) { 00193 if ((ip6addr->addr[2] == 0) && 00194 ((ip6addr->addr[3] & PP_HTONL(0xff000000)) == 0)) { 00195 return 2; 00196 } else if ((ip6addr->addr[2] & PP_HTONL(0xffffff00)) == 0) { 00197 return 1; 00198 } 00199 } 00200 00201 return 0; 00202 } 00203 00204 /* 00205 * Encapsulates data into IEEE 802.15.4 frames. 00206 * Fragments an IPv6 datagram into 6LowPAN units, which fit into IEEE 802.15.4 frames. 00207 * If configured, will compress IPv6 and or UDP headers. 00208 * */ 00209 static err_t 00210 lowpan6_frag(struct netif *netif, struct pbuf *p, const struct ieee_802154_addr *src, const struct ieee_802154_addr *dst) 00211 { 00212 struct pbuf * p_frag; 00213 u16_t frag_len, remaining_len; 00214 u8_t * buffer; 00215 u8_t ieee_header_len; 00216 u8_t lowpan6_header_len; 00217 s8_t i; 00218 static u8_t frame_seq_num; 00219 static u16_t datagram_tag; 00220 u16_t datagram_offset; 00221 err_t err = ERR_IF; 00222 00223 /* We'll use a dedicated pbuf for building 6LowPAN fragments. */ 00224 p_frag = pbuf_alloc(PBUF_RAW, 127, PBUF_RAM); 00225 if (p_frag == NULL) { 00226 MIB2_STATS_NETIF_INC(netif, ifoutdiscards); 00227 return ERR_MEM; 00228 } 00229 00230 /* Write IEEE 802.15.4 header. */ 00231 buffer = (u8_t*)p_frag->payload; 00232 ieee_header_len = 0; 00233 if (dst == &ieee_802154_broadcast) { 00234 buffer[ieee_header_len++] = 0x01; /* data packet, no ack required. */ 00235 } else { 00236 buffer[ieee_header_len++] = 0x21; /* data packet, ack required. */ 00237 } 00238 buffer[ieee_header_len] = (0x00 << 4); /* 2003 frame version */ 00239 buffer[ieee_header_len] |= (dst->addr_len == 2) ? (0x02 << 2) : (0x03 << 2); /* destination addressing mode */ 00240 buffer[ieee_header_len] |= (src->addr_len == 2) ? (0x02 << 6) : (0x03 << 6); /* source addressing mode */ 00241 ieee_header_len++; 00242 buffer[ieee_header_len++] = frame_seq_num++; 00243 00244 buffer[ieee_header_len++] = ieee_802154_pan_id & 0xff; /* pan id */ 00245 buffer[ieee_header_len++] = (ieee_802154_pan_id >> 8) & 0xff; /* pan id */ 00246 i = dst->addr_len; 00247 while (i-- > 0) { 00248 buffer[ieee_header_len++] = dst->addr[i]; 00249 } 00250 00251 buffer[ieee_header_len++] = ieee_802154_pan_id & 0xff; /* pan id */ 00252 buffer[ieee_header_len++] = (ieee_802154_pan_id >> 8) & 0xff; /* pan id */ 00253 i = src->addr_len; 00254 while (i-- > 0) { 00255 buffer[ieee_header_len++] = src->addr[i]; 00256 } 00257 00258 #if LWIP_6LOWPAN_IPHC 00259 /* Perform 6LowPAN IPv6 header compression according to RFC 6282 */ 00260 { 00261 struct ip6_hdr *ip6hdr; 00262 00263 /* Point to ip6 header and align copies of src/dest addresses. */ 00264 ip6hdr = (struct ip6_hdr *)p->payload; 00265 ip_addr_copy_from_ip6(ip_data.current_iphdr_dest, ip6hdr->dest); 00266 ip_addr_copy_from_ip6(ip_data.current_iphdr_src, ip6hdr->src); 00267 00268 /* Basic length of 6LowPAN header, set dispatch and clear fields. */ 00269 lowpan6_header_len = 2; 00270 buffer[ieee_header_len] = 0x60; 00271 buffer[ieee_header_len + 1] = 0; 00272 00273 /* Determine whether there will be a Context Identifier Extension byte or not. 00274 * If so, set it already. */ 00275 #if LWIP_6LOWPAN_NUM_CONTEXTS > 0 00276 buffer[ieee_header_len + 2] = 0; 00277 00278 i = lowpan6_context_lookup(ip_2_ip6(&ip_data.current_iphdr_src)); 00279 if (i >= 0) { 00280 /* Stateful source address compression. */ 00281 buffer[ieee_header_len + 1] |= 0x40; 00282 buffer[ieee_header_len + 2] |= (i & 0x0f) << 4; 00283 } 00284 00285 i = lowpan6_context_lookup(ip_2_ip6(&ip_data.current_iphdr_dest)); 00286 if (i >= 0) { 00287 /* Stateful destination address compression. */ 00288 buffer[ieee_header_len + 1] |= 0x04; 00289 buffer[ieee_header_len + 2] |= i & 0x0f; 00290 } 00291 00292 if (buffer[ieee_header_len + 2] != 0x00) { 00293 /* Context identifier extension byte is appended. */ 00294 buffer[ieee_header_len + 1] |= 0x80; 00295 lowpan6_header_len++; 00296 } 00297 #endif /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */ 00298 00299 /* Determine TF field: Traffic Class, Flow Label */ 00300 if (IP6H_FL(ip6hdr) == 0) { 00301 /* Flow label is elided. */ 00302 buffer[ieee_header_len] |= 0x10; 00303 if (IP6H_TC(ip6hdr) == 0) { 00304 /* Traffic class (ECN+DSCP) elided too. */ 00305 buffer[ieee_header_len] |= 0x08; 00306 } else { 00307 /* Traffic class (ECN+DSCP) appended. */ 00308 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_TC(ip6hdr); 00309 } 00310 } else { 00311 if (((IP6H_TC(ip6hdr) & 0x3f) == 0)) { 00312 /* DSCP portion of Traffic Class is elided, ECN and FL are appended (3 bytes) */ 00313 buffer[ieee_header_len] |= 0x08; 00314 00315 buffer[ieee_header_len + lowpan6_header_len] = IP6H_TC(ip6hdr) & 0xc0; 00316 buffer[ieee_header_len + lowpan6_header_len++] |= (IP6H_FL(ip6hdr) >> 16) & 0x0f; 00317 buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff; 00318 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff; 00319 } else { 00320 /* Traffic class and flow label are appended (4 bytes) */ 00321 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_TC(ip6hdr); 00322 buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 16) & 0x0f; 00323 buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff; 00324 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff; 00325 } 00326 } 00327 00328 /* Compress NH? 00329 * Only if UDP for now. @todo support other NH compression. */ 00330 if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) { 00331 buffer[ieee_header_len] |= 0x04; 00332 } else { 00333 /* append nexth. */ 00334 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_NEXTH(ip6hdr); 00335 } 00336 00337 /* Compress hop limit? */ 00338 if (IP6H_HOPLIM(ip6hdr) == 255) { 00339 buffer[ieee_header_len] |= 0x03; 00340 } else if (IP6H_HOPLIM(ip6hdr) == 64) { 00341 buffer[ieee_header_len] |= 0x02; 00342 } else if (IP6H_HOPLIM(ip6hdr) == 1) { 00343 buffer[ieee_header_len] |= 0x01; 00344 } else { 00345 /* append hop limit */ 00346 buffer[ieee_header_len + lowpan6_header_len++] = IP6H_HOPLIM(ip6hdr); 00347 } 00348 00349 /* Compress source address */ 00350 if (((buffer[ieee_header_len + 1] & 0x40) != 0) || 00351 (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_src)))) { 00352 /* Context-based or link-local source address compression. */ 00353 i = lowpan6_get_address_mode(ip_2_ip6(&ip_data.current_iphdr_src), src); 00354 buffer[ieee_header_len + 1] |= (i & 0x03) << 4; 00355 if (i == 1) { 00356 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 16, 8); 00357 lowpan6_header_len += 8; 00358 } else if (i == 2) { 00359 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 22, 2); 00360 lowpan6_header_len += 2; 00361 } 00362 } else if (ip6_addr_isany(ip_2_ip6(&ip_data.current_iphdr_src))) { 00363 /* Special case: mark SAC and leave SAM=0 */ 00364 buffer[ieee_header_len + 1] |= 0x40; 00365 } else { 00366 /* Append full address. */ 00367 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 8, 16); 00368 lowpan6_header_len += 16; 00369 } 00370 00371 /* Compress destination address */ 00372 if (ip6_addr_ismulticast(ip_2_ip6(&ip_data.current_iphdr_dest))) { 00373 /* @todo support stateful multicast address compression */ 00374 00375 buffer[ieee_header_len + 1] |= 0x08; 00376 00377 i = lowpan6_get_address_mode_mc(ip_2_ip6(&ip_data.current_iphdr_dest)); 00378 buffer[ieee_header_len + 1] |= i & 0x03; 00379 if (i == 0) { 00380 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 24, 16); 00381 lowpan6_header_len += 16; 00382 } else if (i == 1) { 00383 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[25]; 00384 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 35, 5); 00385 lowpan6_header_len += 5; 00386 } else if (i == 2) { 00387 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[25]; 00388 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 37, 3); 00389 lowpan6_header_len += 3; 00390 } else if (i == 3) { 00391 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[39]; 00392 } 00393 } else if (((buffer[ieee_header_len + 1] & 0x04) != 0) || 00394 (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_dest)))) { 00395 /* Context-based or link-local destination address compression. */ 00396 i = lowpan6_get_address_mode(ip_2_ip6(&ip_data.current_iphdr_dest), dst); 00397 buffer[ieee_header_len + 1] |= i & 0x03; 00398 if (i == 1) { 00399 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 32, 8); 00400 lowpan6_header_len += 8; 00401 } else if (i == 2) { 00402 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 38, 2); 00403 lowpan6_header_len += 2; 00404 } 00405 } else { 00406 /* Append full address. */ 00407 MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 24, 16); 00408 lowpan6_header_len += 16; 00409 } 00410 00411 /* Move to payload. */ 00412 pbuf_header(p, -IP6_HLEN); 00413 00414 /* Compress UDP header? */ 00415 if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) { 00416 /* @todo support optional checksum compression */ 00417 00418 buffer[ieee_header_len + lowpan6_header_len] = 0xf0; 00419 00420 /* determine port compression mode. */ 00421 if ((((u8_t *)p->payload)[0] == 0xf0) && ((((u8_t *)p->payload)[1] & 0xf0) == 0xb0) && 00422 (((u8_t *)p->payload)[2] == 0xf0) && ((((u8_t *)p->payload)[3] & 0xf0) == 0xb0)) { 00423 /* Compress source and dest ports. */ 00424 buffer[ieee_header_len + lowpan6_header_len++] |= 0x03; 00425 buffer[ieee_header_len + lowpan6_header_len++] = ((((u8_t *)p->payload)[1] & 0x0f) << 4) | (((u8_t *)p->payload)[3] & 0x0f); 00426 } else if (((u8_t *)p->payload)[0] == 0xf0) { 00427 /* Compress source port. */ 00428 buffer[ieee_header_len + lowpan6_header_len++] |= 0x02; 00429 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1]; 00430 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2]; 00431 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3]; 00432 } else if (((u8_t *)p->payload)[2] == 0xf0) { 00433 /* Compress dest port. */ 00434 buffer[ieee_header_len + lowpan6_header_len++] |= 0x01; 00435 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0]; 00436 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1]; 00437 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3]; 00438 } else { 00439 /* append full ports. */ 00440 lowpan6_header_len++; 00441 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0]; 00442 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1]; 00443 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2]; 00444 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3]; 00445 } 00446 00447 /* elide length and copy checksum */ 00448 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[6]; 00449 buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[7]; 00450 00451 pbuf_header(p, -UDP_HLEN); 00452 } 00453 } 00454 00455 #else /* LWIP_6LOWPAN_HC */ 00456 /* Send uncompressed IPv6 header with appropriate dispatch byte. */ 00457 lowpan6_header_len = 1; 00458 buffer[ieee_header_len] = 0x41; /* IPv6 dispatch */ 00459 #endif /* LWIP_6LOWPAN_HC */ 00460 00461 /* Calculate remaining packet length */ 00462 remaining_len = p->tot_len; 00463 00464 if (remaining_len > 0x7FF) { 00465 MIB2_STATS_NETIF_INC(netif, ifoutdiscards); 00466 /* datagram_size must fit into 11 bit */ 00467 pbuf_free(p_frag); 00468 return ERR_VAL; 00469 } 00470 00471 /* Fragment, or 1 packet? */ 00472 if (remaining_len > (127 - ieee_header_len - lowpan6_header_len - 3)) { /* 127 - header - 1 byte dispatch - 2 bytes CRC */ 00473 /* We must move the 6LowPAN header to make room for the FRAG header. */ 00474 i = lowpan6_header_len; 00475 while (i-- != 0) { 00476 buffer[ieee_header_len + i + 4] = buffer[ieee_header_len + i]; 00477 } 00478 00479 /* Now we need to fragment the packet. FRAG1 header first */ 00480 buffer[ieee_header_len] = 0xc0 | (((p->tot_len + lowpan6_header_len) >> 8) & 0x7); 00481 buffer[ieee_header_len + 1] = (p->tot_len + lowpan6_header_len) & 0xff; 00482 00483 datagram_tag++; 00484 buffer[ieee_header_len + 2] = datagram_tag & 0xff; 00485 buffer[ieee_header_len + 3] = (datagram_tag >> 8) & 0xff; 00486 00487 /* Fragment follows. */ 00488 frag_len = (127 - ieee_header_len - 4 - 2) & 0xf8; 00489 00490 pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len + 4, frag_len - lowpan6_header_len, 0); 00491 remaining_len -= frag_len - lowpan6_header_len; 00492 datagram_offset = frag_len; 00493 00494 /* 2 bytes CRC */ 00495 #if LWIP_6LOWPAN_HW_CRC 00496 /* Leave blank, will be filled by HW. */ 00497 #else /* LWIP_6LOWPAN_HW_CRC */ 00498 /* @todo calculate CRC */ 00499 #endif /* LWIP_6LOWPAN_HW_CRC */ 00500 00501 /* Calculate frame length */ 00502 p_frag->len = p_frag->tot_len = ieee_header_len + 4 + frag_len + 2; /* add 2 dummy bytes for crc*/ 00503 00504 /* send the packet */ 00505 MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len); 00506 LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p)); 00507 err = netif->linkoutput(netif, p_frag); 00508 00509 while ((remaining_len > 0) && (err == ERR_OK)) { 00510 /* new frame, new seq num for ACK */ 00511 buffer[2] = frame_seq_num++; 00512 00513 buffer[ieee_header_len] |= 0x20; /* Change FRAG1 to FRAGN */ 00514 00515 buffer[ieee_header_len + 4] = (u8_t)(datagram_offset >> 3); /* datagram offset in FRAGN header (datagram_offset is max. 11 bit) */ 00516 00517 frag_len = (127 - ieee_header_len - 5 - 2) & 0xf8; 00518 if (frag_len > remaining_len) { 00519 frag_len = remaining_len; 00520 } 00521 00522 pbuf_copy_partial(p, buffer + ieee_header_len + 5, frag_len, p->tot_len - remaining_len); 00523 remaining_len -= frag_len; 00524 datagram_offset += frag_len; 00525 00526 /* 2 bytes CRC */ 00527 #if LWIP_6LOWPAN_HW_CRC 00528 /* Leave blank, will be filled by HW. */ 00529 #else /* LWIP_6LOWPAN_HW_CRC */ 00530 /* @todo calculate CRC */ 00531 #endif /* LWIP_6LOWPAN_HW_CRC */ 00532 00533 /* Calculate frame length */ 00534 p_frag->len = p_frag->tot_len = frag_len + 5 + ieee_header_len + 2; 00535 00536 /* send the packet */ 00537 MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len); 00538 LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p)); 00539 err = netif->linkoutput(netif, p_frag); 00540 } 00541 } else { 00542 /* It fits in one frame. */ 00543 frag_len = remaining_len; 00544 00545 /* Copy IPv6 packet */ 00546 pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len, frag_len, 0); 00547 remaining_len = 0; 00548 00549 /* 2 bytes CRC */ 00550 #if LWIP_6LOWPAN_HW_CRC 00551 /* Leave blank, will be filled by HW. */ 00552 #else /* LWIP_6LOWPAN_HW_CRC */ 00553 /* @todo calculate CRC */ 00554 #endif /* LWIP_6LOWPAN_HW_CRC */ 00555 00556 /* Calculate frame length */ 00557 p_frag->len = p_frag->tot_len = frag_len + lowpan6_header_len + ieee_header_len + 2; 00558 00559 /* send the packet */ 00560 MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len); 00561 LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p)); 00562 err = netif->linkoutput(netif, p_frag); 00563 } 00564 00565 pbuf_free(p_frag); 00566 00567 return err; 00568 } 00569 00570 err_t 00571 lowpan6_set_context(u8_t idx, const ip6_addr_t * context) 00572 { 00573 if (idx >= LWIP_6LOWPAN_NUM_CONTEXTS) { 00574 return ERR_ARG; 00575 } 00576 00577 ip6_addr_set(&lowpan6_context[idx], context); 00578 00579 return ERR_OK; 00580 } 00581 00582 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS 00583 err_t 00584 lowpan6_set_short_addr(u8_t addr_high, u8_t addr_low) 00585 { 00586 short_mac_addr.addr[0] = addr_high; 00587 short_mac_addr.addr[1] = addr_low; 00588 00589 return ERR_OK; 00590 } 00591 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ 00592 00593 #if LWIP_IPV4 00594 err_t 00595 lowpan4_output(struct netif *netif, struct pbuf *q, const ip4_addr_t *ipaddr) 00596 { 00597 (void)netif; 00598 (void)q; 00599 (void)ipaddr; 00600 00601 return ERR_IF; 00602 } 00603 #endif /* LWIP_IPV4 */ 00604 00605 /** 00606 * Resolve and fill-in IEEE 802.15.4 address header for outgoing IPv6 packet. 00607 * 00608 * Perform Header Compression and fragment if necessary. 00609 * 00610 * @param netif The lwIP network interface which the IP packet will be sent on. 00611 * @param q The pbuf(s) containing the IP packet to be sent. 00612 * @param ip6addr The IP address of the packet destination. 00613 * 00614 * @return err_t 00615 */ 00616 err_t 00617 lowpan6_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr) 00618 { 00619 err_t result; 00620 const u8_t *hwaddr; 00621 struct ieee_802154_addr src, dest; 00622 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS 00623 ip6_addr_t ip6_src; 00624 struct ip6_hdr * ip6_hdr; 00625 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ 00626 00627 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS 00628 /* Check if we can compress source address (use aligned copy) */ 00629 ip6_hdr = (struct ip6_hdr *)q->payload; 00630 ip6_addr_set(&ip6_src, &ip6_hdr->src); 00631 if (lowpan6_get_address_mode(&ip6_src, &short_mac_addr) == 3) { 00632 src.addr_len = 2; 00633 src.addr[0] = short_mac_addr.addr[0]; 00634 src.addr[1] = short_mac_addr.addr[1]; 00635 } else 00636 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ 00637 { 00638 src.addr_len = netif->hwaddr_len; 00639 SMEMCPY(src.addr, netif->hwaddr, netif->hwaddr_len); 00640 } 00641 00642 /* multicast destination IP address? */ 00643 if (ip6_addr_ismulticast(ip6addr)) { 00644 MIB2_STATS_NETIF_INC(netif, ifoutnucastpkts); 00645 /* We need to send to the broadcast address.*/ 00646 return lowpan6_frag(netif, q, &src, &ieee_802154_broadcast); 00647 } 00648 00649 /* We have a unicast destination IP address */ 00650 /* @todo anycast? */ 00651 00652 #if LWIP_6LOWPAN_INFER_SHORT_ADDRESS 00653 if (src.addr_len == 2) { 00654 /* If source address was compressable to short_mac_addr, and dest has same subnet and 00655 * is also compressable to 2-bytes, assume we can infer dest as a short address too. */ 00656 dest.addr_len = 2; 00657 dest.addr[0] = ((u8_t *)q->payload)[38]; 00658 dest.addr[1] = ((u8_t *)q->payload)[39]; 00659 if ((src.addr_len == 2) && (ip6_addr_netcmp(&ip6_hdr->src, &ip6_hdr->dest)) && 00660 (lowpan6_get_address_mode(ip6addr, &dest) == 3)) { 00661 MIB2_STATS_NETIF_INC(netif, ifoutucastpkts); 00662 return lowpan6_frag(netif, q, &src, &dest); 00663 } 00664 } 00665 #endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ 00666 00667 /* Ask ND6 what to do with the packet. */ 00668 result = nd6_get_next_hop_addr_or_queue(netif, q, ip6addr, &hwaddr); 00669 if (result != ERR_OK) { 00670 MIB2_STATS_NETIF_INC(netif, ifoutdiscards); 00671 return result; 00672 } 00673 00674 /* If no hardware address is returned, nd6 has queued the packet for later. */ 00675 if (hwaddr == NULL) { 00676 return ERR_OK; 00677 } 00678 00679 /* Send out the packet using the returned hardware address. */ 00680 dest.addr_len = netif->hwaddr_len; 00681 SMEMCPY(dest.addr, hwaddr, netif->hwaddr_len); 00682 MIB2_STATS_NETIF_INC(netif, ifoutucastpkts); 00683 return lowpan6_frag(netif, q, &src, &dest); 00684 } 00685 00686 static struct pbuf * 00687 lowpan6_decompress(struct pbuf * p, struct ieee_802154_addr * src, struct ieee_802154_addr * dest) 00688 { 00689 struct pbuf * q; 00690 u8_t * lowpan6_buffer; 00691 s8_t lowpan6_offset; 00692 struct ip6_hdr *ip6hdr; 00693 s8_t i; 00694 s8_t ip6_offset = IP6_HLEN; 00695 00696 00697 q = pbuf_alloc(PBUF_IP, p->len + IP6_HLEN + UDP_HLEN, PBUF_POOL); 00698 if (q == NULL) { 00699 pbuf_free(p); 00700 return NULL; 00701 } 00702 00703 lowpan6_buffer = (u8_t *)p->payload; 00704 ip6hdr = (struct ip6_hdr *)q->payload; 00705 00706 lowpan6_offset = 2; 00707 if (lowpan6_buffer[1] & 0x80) { 00708 lowpan6_offset++; 00709 } 00710 00711 /* Set IPv6 version, traffic class and flow label. */ 00712 if ((lowpan6_buffer[0] & 0x18) == 0x00) { 00713 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]); 00714 lowpan6_offset += 4; 00715 } else if ((lowpan6_buffer[0] & 0x18) == 0x08) { 00716 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]); 00717 lowpan6_offset += 3; 00718 } else if ((lowpan6_buffer[0] & 0x18) == 0x10) { 00719 IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset],0); 00720 lowpan6_offset += 1; 00721 } else if ((lowpan6_buffer[0] & 0x18) == 0x18) { 00722 IP6H_VTCFL_SET(ip6hdr, 6, 0, 0); 00723 } 00724 00725 /* Set Next Header */ 00726 if ((lowpan6_buffer[0] & 0x04) == 0x00) { 00727 IP6H_NEXTH_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]); 00728 } else { 00729 /* We should fill this later with NHC decoding */ 00730 IP6H_NEXTH_SET(ip6hdr, 0); 00731 } 00732 00733 /* Set Hop Limit */ 00734 if ((lowpan6_buffer[0] & 0x03) == 0x00) { 00735 IP6H_HOPLIM_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]); 00736 } else if ((lowpan6_buffer[0] & 0x03) == 0x01) { 00737 IP6H_HOPLIM_SET(ip6hdr, 1); 00738 } else if ((lowpan6_buffer[0] & 0x03) == 0x02) { 00739 IP6H_HOPLIM_SET(ip6hdr, 64); 00740 } else if ((lowpan6_buffer[0] & 0x03) == 0x03) { 00741 IP6H_HOPLIM_SET(ip6hdr, 255); 00742 } 00743 00744 /* Source address decoding. */ 00745 if ((lowpan6_buffer[1] & 0x40) == 0x00) { 00746 /* Stateless compression */ 00747 if ((lowpan6_buffer[1] & 0x30) == 0x00) { 00748 /* copy full address */ 00749 MEMCPY(&ip6hdr->src.addr[0], lowpan6_buffer + lowpan6_offset, 16); 00750 lowpan6_offset += 16; 00751 } else if ((lowpan6_buffer[1] & 0x30) == 0x10) { 00752 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); 00753 ip6hdr->src.addr[1] = 0; 00754 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8); 00755 lowpan6_offset += 8; 00756 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) { 00757 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); 00758 ip6hdr->src.addr[1] = 0; 00759 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); 00760 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | 00761 lowpan6_buffer[lowpan6_offset+1]); 00762 lowpan6_offset += 2; 00763 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) { 00764 ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); 00765 ip6hdr->src.addr[1] = 0; 00766 if (src->addr_len == 2) { 00767 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); 00768 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]); 00769 } else { 00770 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | 00771 (src->addr[2] << 8) | src->addr[3]); 00772 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) | 00773 (src->addr[6] << 8) | src->addr[7]); 00774 } 00775 } 00776 } else { 00777 /* Stateful compression */ 00778 if ((lowpan6_buffer[1] & 0x30) == 0x00) { 00779 /* ANY address */ 00780 ip6hdr->src.addr[0] = 0; 00781 ip6hdr->src.addr[1] = 0; 00782 ip6hdr->src.addr[2] = 0; 00783 ip6hdr->src.addr[3] = 0; 00784 } else { 00785 /* Set prefix from context info */ 00786 if (lowpan6_buffer[1] & 0x80) { 00787 i = (lowpan6_buffer[2] >> 4) & 0x0f; 00788 } else { 00789 i = 0; 00790 } 00791 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) { 00792 /* Error */ 00793 pbuf_free(p); 00794 pbuf_free(q); 00795 return NULL; 00796 } 00797 00798 ip6hdr->src.addr[0] = lowpan6_context[i].addr[0]; 00799 ip6hdr->src.addr[1] = lowpan6_context[i].addr[1]; 00800 } 00801 00802 if ((lowpan6_buffer[1] & 0x30) == 0x10) { 00803 MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8); 00804 lowpan6_offset += 8; 00805 } else if ((lowpan6_buffer[1] & 0x30) == 0x20) { 00806 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); 00807 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset+1]); 00808 lowpan6_offset += 2; 00809 } else if ((lowpan6_buffer[1] & 0x30) == 0x30) { 00810 if (src->addr_len == 2) { 00811 ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); 00812 ip6hdr->src.addr[3] = lwip_htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]); 00813 } else { 00814 ip6hdr->src.addr[2] = lwip_htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | (src->addr[2] << 8) | src->addr[3]); 00815 ip6hdr->src.addr[3] = lwip_htonl((src->addr[4] << 24) | (src->addr[5] << 16) | (src->addr[6] << 8) | src->addr[7]); 00816 } 00817 } 00818 } 00819 00820 /* Destination address decoding. */ 00821 if (lowpan6_buffer[1] & 0x08) { 00822 /* Multicast destination */ 00823 if (lowpan6_buffer[1] & 0x04) { 00824 /* @todo support stateful multicast addressing */ 00825 pbuf_free(p); 00826 pbuf_free(q); 00827 return NULL; 00828 } 00829 00830 if ((lowpan6_buffer[1] & 0x03) == 0x00) { 00831 /* copy full address */ 00832 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16); 00833 lowpan6_offset += 16; 00834 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) { 00835 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16)); 00836 ip6hdr->dest.addr[1] = 0; 00837 ip6hdr->dest.addr[2] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]); 00838 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]); 00839 lowpan6_offset += 4; 00840 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) { 00841 ip6hdr->dest.addr[0] = lwip_htonl(0xff000000UL | lowpan6_buffer[lowpan6_offset++]); 00842 ip6hdr->dest.addr[1] = 0; 00843 ip6hdr->dest.addr[2] = 0; 00844 ip6hdr->dest.addr[3] = lwip_htonl((lowpan6_buffer[lowpan6_offset] << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset + 2]); 00845 lowpan6_offset += 3; 00846 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) { 00847 ip6hdr->dest.addr[0] = PP_HTONL(0xff020000UL); 00848 ip6hdr->dest.addr[1] = 0; 00849 ip6hdr->dest.addr[2] = 0; 00850 ip6hdr->dest.addr[3] = lwip_htonl(lowpan6_buffer[lowpan6_offset++]); 00851 } 00852 00853 } else { 00854 if (lowpan6_buffer[1] & 0x04) { 00855 /* Stateful destination compression */ 00856 /* Set prefix from context info */ 00857 if (lowpan6_buffer[1] & 0x80) { 00858 i = lowpan6_buffer[2] & 0x0f; 00859 } else { 00860 i = 0; 00861 } 00862 if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) { 00863 /* Error */ 00864 pbuf_free(p); 00865 pbuf_free(q); 00866 return NULL; 00867 } 00868 00869 ip6hdr->dest.addr[0] = lowpan6_context[i].addr[0]; 00870 ip6hdr->dest.addr[1] = lowpan6_context[i].addr[1]; 00871 } else { 00872 /* Link local address compression */ 00873 ip6hdr->dest.addr[0] = PP_HTONL(0xfe800000UL); 00874 ip6hdr->dest.addr[1] = 0; 00875 } 00876 00877 if ((lowpan6_buffer[1] & 0x03) == 0x00) { 00878 /* copy full address */ 00879 MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16); 00880 lowpan6_offset += 16; 00881 } else if ((lowpan6_buffer[1] & 0x03) == 0x01) { 00882 MEMCPY(&ip6hdr->dest.addr[2], lowpan6_buffer + lowpan6_offset, 8); 00883 lowpan6_offset += 8; 00884 } else if ((lowpan6_buffer[1] & 0x03) == 0x02) { 00885 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL); 00886 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]); 00887 lowpan6_offset += 2; 00888 } else if ((lowpan6_buffer[1] & 0x03) == 0x03) { 00889 if (dest->addr_len == 2) { 00890 ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL); 00891 ip6hdr->dest.addr[3] = lwip_htonl(0xfe000000UL | (dest->addr[0] << 8) | dest->addr[1]); 00892 } else { 00893 ip6hdr->dest.addr[2] = lwip_htonl(((dest->addr[0] ^ 2) << 24) | (dest->addr[1] << 16) | dest->addr[2] << 8 | dest->addr[3]); 00894 ip6hdr->dest.addr[3] = lwip_htonl((dest->addr[4] << 24) | (dest->addr[5] << 16) | dest->addr[6] << 8 | dest->addr[7]); 00895 } 00896 } 00897 } 00898 00899 00900 /* Next Header Compression (NHC) decoding? */ 00901 if (lowpan6_buffer[0] & 0x04) { 00902 if ((lowpan6_buffer[lowpan6_offset] & 0xf8) == 0xf0) { 00903 struct udp_hdr *udphdr; 00904 00905 /* UDP compression */ 00906 IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_UDP); 00907 udphdr = (struct udp_hdr *)((u8_t *)q->payload + ip6_offset); 00908 00909 if (lowpan6_buffer[lowpan6_offset] & 0x04) { 00910 /* @todo support checksum decompress */ 00911 pbuf_free(p); 00912 pbuf_free(q); 00913 return NULL; 00914 } 00915 00916 /* Decompress ports */ 00917 i = lowpan6_buffer[lowpan6_offset++] & 0x03; 00918 if (i == 0) { 00919 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]); 00920 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 2] << 8 | lowpan6_buffer[lowpan6_offset + 3]); 00921 lowpan6_offset += 4; 00922 } else if (i == 0x01) { 00923 udphdr->src = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]); 00924 udphdr->dest = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset + 2]); 00925 lowpan6_offset += 3; 00926 } else if (i == 0x02) { 00927 udphdr->src = lwip_htons(0xf000 | lowpan6_buffer[lowpan6_offset]); 00928 udphdr->dest = lwip_htons(lowpan6_buffer[lowpan6_offset + 1] << 8 | lowpan6_buffer[lowpan6_offset + 2]); 00929 lowpan6_offset += 3; 00930 } else if (i == 0x03) { 00931 udphdr->src = lwip_htons(0xf0b0 | ((lowpan6_buffer[lowpan6_offset] >> 4) & 0x0f)); 00932 udphdr->dest = lwip_htons(0xf0b0 | (lowpan6_buffer[lowpan6_offset] & 0x0f)); 00933 lowpan6_offset += 1; 00934 } 00935 00936 udphdr->chksum = lwip_htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]); 00937 lowpan6_offset += 2; 00938 udphdr->len = lwip_htons(p->tot_len - lowpan6_offset + UDP_HLEN); 00939 00940 ip6_offset += UDP_HLEN; 00941 } else { 00942 /* @todo support NHC other than UDP */ 00943 pbuf_free(p); 00944 pbuf_free(q); 00945 return NULL; 00946 } 00947 } 00948 00949 /* Now we copy leftover contents from p to q, so we have all L2 and L3 headers (and L4?) in a single PBUF. 00950 * Replace p with q, and free p */ 00951 pbuf_header(p, -lowpan6_offset); 00952 MEMCPY((u8_t*)q->payload + ip6_offset, p->payload, p->len); 00953 q->len = q->tot_len = ip6_offset + p->len; 00954 if (p->next != NULL) { 00955 pbuf_cat(q, p->next); 00956 } 00957 p->next = NULL; 00958 pbuf_free(p); 00959 00960 /* Infer IPv6 payload length for header */ 00961 IP6H_PLEN_SET(ip6hdr, q->tot_len - IP6_HLEN); 00962 00963 /* all done */ 00964 return q; 00965 } 00966 00967 err_t 00968 lowpan6_input(struct pbuf * p, struct netif *netif) 00969 { 00970 u8_t * puc; 00971 s8_t i; 00972 struct ieee_802154_addr src, dest; 00973 u16_t datagram_size, datagram_offset, datagram_tag; 00974 struct lowpan6_reass_helper *lrh, *lrh_temp; 00975 00976 MIB2_STATS_NETIF_ADD(netif, ifinoctets, p->tot_len); 00977 00978 /* Analyze header. @todo validate. */ 00979 puc = (u8_t*)p->payload; 00980 datagram_offset = 5; 00981 if ((puc[1] & 0x0c) == 0x0c) { 00982 dest.addr_len = 8; 00983 for (i = 0; i < 8; i++) { 00984 dest.addr[i] = puc[datagram_offset + 7 - i]; 00985 } 00986 datagram_offset += 8; 00987 } else { 00988 dest.addr_len = 2; 00989 dest.addr[0] = puc[datagram_offset + 1]; 00990 dest.addr[1] = puc[datagram_offset]; 00991 datagram_offset += 2; 00992 } 00993 00994 datagram_offset += 2; /* skip PAN ID. */ 00995 00996 if ((puc[1] & 0xc0) == 0xc0) { 00997 src.addr_len = 8; 00998 for (i = 0; i < 8; i++) { 00999 src.addr[i] = puc[datagram_offset + 7 - i]; 01000 } 01001 datagram_offset += 8; 01002 } else { 01003 src.addr_len = 2; 01004 src.addr[0] = puc[datagram_offset + 1]; 01005 src.addr[1] = puc[datagram_offset]; 01006 datagram_offset += 2; 01007 } 01008 01009 pbuf_header(p, -datagram_offset); /* hide IEEE802.15.4 header. */ 01010 01011 /* Check dispatch. */ 01012 puc = (u8_t*)p->payload; 01013 01014 if ((*puc & 0xf8) == 0xc0) { 01015 /* FRAG1 dispatch. add this packet to reassembly list. */ 01016 datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1]; 01017 datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3]; 01018 01019 /* check for duplicate */ 01020 lrh = reass_list; 01021 while (lrh != NULL) { 01022 if ((lrh->sender_addr.addr_len == src.addr_len) && 01023 (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0)) { 01024 /* address match with packet in reassembly. */ 01025 if ((datagram_tag == lrh->datagram_tag) && (datagram_size == lrh->datagram_size)) { 01026 MIB2_STATS_NETIF_INC(netif, ifindiscards); 01027 /* duplicate fragment. */ 01028 pbuf_free(p); 01029 return ERR_OK; 01030 } else { 01031 /* We are receiving the start of a new datagram. Discard old one (incomplete). */ 01032 lrh_temp = lrh->next_packet; 01033 dequeue_datagram(lrh); 01034 pbuf_free(lrh->pbuf); 01035 mem_free(lrh); 01036 01037 /* Check next datagram in queue. */ 01038 lrh = lrh_temp; 01039 } 01040 } else { 01041 /* Check next datagram in queue. */ 01042 lrh = lrh->next_packet; 01043 } 01044 } 01045 01046 pbuf_header(p, -4); /* hide frag1 dispatch */ 01047 01048 lrh = (struct lowpan6_reass_helper *) mem_malloc(sizeof(struct lowpan6_reass_helper)); 01049 if (lrh == NULL) { 01050 MIB2_STATS_NETIF_INC(netif, ifindiscards); 01051 pbuf_free(p); 01052 return ERR_MEM; 01053 } 01054 01055 lrh->sender_addr.addr_len = src.addr_len; 01056 for (i = 0; i < src.addr_len; i++) { 01057 lrh->sender_addr.addr[i] = src.addr[i]; 01058 } 01059 lrh->datagram_size = datagram_size; 01060 lrh->datagram_tag = datagram_tag; 01061 lrh->pbuf = p; 01062 lrh->next_packet = reass_list; 01063 lrh->timer = 2; 01064 reass_list = lrh; 01065 01066 return ERR_OK; 01067 } else if ((*puc & 0xf8) == 0xe0) { 01068 /* FRAGN dispatch, find packet being reassembled. */ 01069 datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1]; 01070 datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3]; 01071 datagram_offset = (u16_t)puc[4] << 3; 01072 pbuf_header(p, -5); /* hide frag1 dispatch */ 01073 01074 for (lrh = reass_list; lrh != NULL; lrh = lrh->next_packet) { 01075 if ((lrh->sender_addr.addr_len == src.addr_len) && 01076 (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0) && 01077 (datagram_tag == lrh->datagram_tag) && 01078 (datagram_size == lrh->datagram_size)) { 01079 break; 01080 } 01081 } 01082 if (lrh == NULL) { 01083 /* rogue fragment */ 01084 MIB2_STATS_NETIF_INC(netif, ifindiscards); 01085 pbuf_free(p); 01086 return ERR_OK; 01087 } 01088 01089 if (lrh->pbuf->tot_len < datagram_offset) { 01090 /* duplicate, ignore. */ 01091 pbuf_free(p); 01092 return ERR_OK; 01093 } else if (lrh->pbuf->tot_len > datagram_offset) { 01094 MIB2_STATS_NETIF_INC(netif, ifindiscards); 01095 /* We have missed a fragment. Delete whole reassembly. */ 01096 dequeue_datagram(lrh); 01097 pbuf_free(lrh->pbuf); 01098 mem_free(lrh); 01099 pbuf_free(p); 01100 return ERR_OK; 01101 } 01102 pbuf_cat(lrh->pbuf, p); 01103 p = NULL; 01104 01105 /* is packet now complete?*/ 01106 if (lrh->pbuf->tot_len >= lrh->datagram_size) { 01107 /* dequeue from reass list. */ 01108 dequeue_datagram(lrh); 01109 01110 /* get pbuf */ 01111 p = lrh->pbuf; 01112 01113 /* release helper */ 01114 mem_free(lrh); 01115 } else { 01116 return ERR_OK; 01117 } 01118 } 01119 01120 if (p == NULL) { 01121 return ERR_OK; 01122 } 01123 01124 /* We have a complete packet, check dispatch for headers. */ 01125 puc = (u8_t*)p->payload; 01126 01127 if (*puc == 0x41) { 01128 /* This is a complete IPv6 packet, just skip dispatch byte. */ 01129 pbuf_header(p, -1); /* hide dispatch byte. */ 01130 } else if ((*puc & 0xe0 )== 0x60) { 01131 /* IPv6 headers are compressed using IPHC. */ 01132 p = lowpan6_decompress(p, &src, &dest); 01133 if (p == NULL) { 01134 MIB2_STATS_NETIF_INC(netif, ifindiscards); 01135 return ERR_OK; 01136 } 01137 } else { 01138 MIB2_STATS_NETIF_INC(netif, ifindiscards); 01139 pbuf_free(p); 01140 return ERR_OK; 01141 } 01142 01143 /* @todo: distinguish unicast/multicast */ 01144 MIB2_STATS_NETIF_INC(netif, ifinucastpkts); 01145 01146 return ip6_input(p, netif); 01147 } 01148 01149 err_t 01150 lowpan6_if_init(struct netif *netif) 01151 { 01152 netif->name[0] = 'L'; 01153 netif->name[1] = '6'; 01154 #if LWIP_IPV4 01155 netif->output = lowpan4_output; 01156 #endif /* LWIP_IPV4 */ 01157 netif->output_ip6 = lowpan6_output; 01158 01159 MIB2_INIT_NETIF(netif, snmp_ifType_other, 0); 01160 01161 /* maximum transfer unit */ 01162 netif->mtu = 1280; 01163 01164 /* broadcast capability */ 01165 netif->flags = NETIF_FLAG_BROADCAST /* | NETIF_FLAG_LOWPAN6 */; 01166 01167 return ERR_OK; 01168 } 01169 01170 err_t 01171 lowpan6_set_pan_id(u16_t pan_id) 01172 { 01173 ieee_802154_pan_id = pan_id; 01174 01175 return ERR_OK; 01176 } 01177 01178 #if !NO_SYS 01179 /** 01180 * Pass a received packet to tcpip_thread for input processing 01181 * 01182 * @param p the received packet, p->payload pointing to the 01183 * IEEE 802.15.4 header. 01184 * @param inp the network interface on which the packet was received 01185 */ 01186 err_t 01187 tcpip_6lowpan_input(struct pbuf *p, struct netif *inp) 01188 { 01189 return tcpip_inpkt(p, inp, lowpan6_input); 01190 } 01191 #endif /* !NO_SYS */ 01192 01193 #endif /* LWIP_IPV6 && LWIP_6LOWPAN */
Generated on Sun Jul 17 2022 08:25:24 by 1.7.2