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