diff options
author | Jukka Rissanen <jukka.rissanen@linux.intel.com> | 2013-12-11 10:05:34 -0500 |
---|---|---|
committer | Marcel Holtmann <marcel@holtmann.org> | 2013-12-11 15:57:54 -0500 |
commit | 8df8c56a5abc70af5862aa7cac2875aeeb17a42b (patch) | |
tree | fa21c03bc3bd3bc9778894aa39c234e291f1d850 /net/ieee802154 | |
parent | 71fb419724fadab4efdf98210aa3fe053bd81d29 (diff) |
6lowpan: Moving generic compression code into 6lowpan_iphc.c
Because the IEEE 802154 and Bluetooth share the IP header compression
and uncompression code, the common code is moved to 6lowpan_iphc.c
file.
Signed-off-by: Jukka Rissanen <jukka.rissanen@linux.intel.com>
Acked-by: Alexander Aring <alex.aring@gmail.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>
Diffstat (limited to 'net/ieee802154')
-rw-r--r-- | net/ieee802154/6lowpan.c | 753 | ||||
-rw-r--r-- | net/ieee802154/6lowpan.h | 32 | ||||
-rw-r--r-- | net/ieee802154/6lowpan_iphc.c | 807 | ||||
-rw-r--r-- | net/ieee802154/Makefile | 2 |
4 files changed, 875 insertions, 719 deletions
diff --git a/net/ieee802154/6lowpan.c b/net/ieee802154/6lowpan.c index 459e200c08a4..53d0bd58ed8d 100644 --- a/net/ieee802154/6lowpan.c +++ b/net/ieee802154/6lowpan.c | |||
@@ -62,9 +62,6 @@ | |||
62 | 62 | ||
63 | #include "6lowpan.h" | 63 | #include "6lowpan.h" |
64 | 64 | ||
65 | /* TTL uncompression values */ | ||
66 | static const u8 lowpan_ttl_values[] = {0, 1, 64, 255}; | ||
67 | |||
68 | static LIST_HEAD(lowpan_devices); | 65 | static LIST_HEAD(lowpan_devices); |
69 | 66 | ||
70 | /* private device info */ | 67 | /* private device info */ |
@@ -135,347 +132,14 @@ static inline void lowpan_raw_dump_table(const char *caller, char *msg, | |||
135 | #endif /* DEBUG */ | 132 | #endif /* DEBUG */ |
136 | } | 133 | } |
137 | 134 | ||
138 | static u8 | ||
139 | lowpan_compress_addr_64(u8 **hc06_ptr, u8 shift, const struct in6_addr *ipaddr, | ||
140 | const unsigned char *lladdr) | ||
141 | { | ||
142 | u8 val = 0; | ||
143 | |||
144 | if (is_addr_mac_addr_based(ipaddr, lladdr)) | ||
145 | val = 3; /* 0-bits */ | ||
146 | else if (lowpan_is_iid_16_bit_compressable(ipaddr)) { | ||
147 | /* compress IID to 16 bits xxxx::XXXX */ | ||
148 | memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2); | ||
149 | *hc06_ptr += 2; | ||
150 | val = 2; /* 16-bits */ | ||
151 | } else { | ||
152 | /* do not compress IID => xxxx::IID */ | ||
153 | memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8); | ||
154 | *hc06_ptr += 8; | ||
155 | val = 1; /* 64-bits */ | ||
156 | } | ||
157 | |||
158 | return rol8(val, shift); | ||
159 | } | ||
160 | |||
161 | /* | ||
162 | * Uncompress address function for source and | ||
163 | * destination address(non-multicast). | ||
164 | * | ||
165 | * address_mode is sam value or dam value. | ||
166 | */ | ||
167 | static int | ||
168 | lowpan_uncompress_addr(struct sk_buff *skb, | ||
169 | struct in6_addr *ipaddr, | ||
170 | const u8 address_mode, | ||
171 | const struct ieee802154_addr *lladdr) | ||
172 | { | ||
173 | bool fail; | ||
174 | |||
175 | switch (address_mode) { | ||
176 | case LOWPAN_IPHC_ADDR_00: | ||
177 | /* for global link addresses */ | ||
178 | fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); | ||
179 | break; | ||
180 | case LOWPAN_IPHC_ADDR_01: | ||
181 | /* fe:80::XXXX:XXXX:XXXX:XXXX */ | ||
182 | ipaddr->s6_addr[0] = 0xFE; | ||
183 | ipaddr->s6_addr[1] = 0x80; | ||
184 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); | ||
185 | break; | ||
186 | case LOWPAN_IPHC_ADDR_02: | ||
187 | /* fe:80::ff:fe00:XXXX */ | ||
188 | ipaddr->s6_addr[0] = 0xFE; | ||
189 | ipaddr->s6_addr[1] = 0x80; | ||
190 | ipaddr->s6_addr[11] = 0xFF; | ||
191 | ipaddr->s6_addr[12] = 0xFE; | ||
192 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); | ||
193 | break; | ||
194 | case LOWPAN_IPHC_ADDR_03: | ||
195 | fail = false; | ||
196 | switch (lladdr->addr_type) { | ||
197 | case IEEE802154_ADDR_LONG: | ||
198 | /* fe:80::XXXX:XXXX:XXXX:XXXX | ||
199 | * \_________________/ | ||
200 | * hwaddr | ||
201 | */ | ||
202 | ipaddr->s6_addr[0] = 0xFE; | ||
203 | ipaddr->s6_addr[1] = 0x80; | ||
204 | memcpy(&ipaddr->s6_addr[8], lladdr->hwaddr, | ||
205 | IEEE802154_ADDR_LEN); | ||
206 | /* second bit-flip (Universe/Local) | ||
207 | * is done according RFC2464 | ||
208 | */ | ||
209 | ipaddr->s6_addr[8] ^= 0x02; | ||
210 | break; | ||
211 | case IEEE802154_ADDR_SHORT: | ||
212 | /* fe:80::ff:fe00:XXXX | ||
213 | * \__/ | ||
214 | * short_addr | ||
215 | * | ||
216 | * Universe/Local bit is zero. | ||
217 | */ | ||
218 | ipaddr->s6_addr[0] = 0xFE; | ||
219 | ipaddr->s6_addr[1] = 0x80; | ||
220 | ipaddr->s6_addr[11] = 0xFF; | ||
221 | ipaddr->s6_addr[12] = 0xFE; | ||
222 | ipaddr->s6_addr16[7] = htons(lladdr->short_addr); | ||
223 | break; | ||
224 | default: | ||
225 | pr_debug("Invalid addr_type set\n"); | ||
226 | return -EINVAL; | ||
227 | } | ||
228 | break; | ||
229 | default: | ||
230 | pr_debug("Invalid address mode value: 0x%x\n", address_mode); | ||
231 | return -EINVAL; | ||
232 | } | ||
233 | |||
234 | if (fail) { | ||
235 | pr_debug("Failed to fetch skb data\n"); | ||
236 | return -EIO; | ||
237 | } | ||
238 | |||
239 | lowpan_raw_dump_inline(NULL, "Reconstructed ipv6 addr is:\n", | ||
240 | ipaddr->s6_addr, 16); | ||
241 | |||
242 | return 0; | ||
243 | } | ||
244 | |||
245 | /* Uncompress address function for source context | ||
246 | * based address(non-multicast). | ||
247 | */ | ||
248 | static int | ||
249 | lowpan_uncompress_context_based_src_addr(struct sk_buff *skb, | ||
250 | struct in6_addr *ipaddr, | ||
251 | const u8 sam) | ||
252 | { | ||
253 | switch (sam) { | ||
254 | case LOWPAN_IPHC_ADDR_00: | ||
255 | /* unspec address :: | ||
256 | * Do nothing, address is already :: | ||
257 | */ | ||
258 | break; | ||
259 | case LOWPAN_IPHC_ADDR_01: | ||
260 | /* TODO */ | ||
261 | case LOWPAN_IPHC_ADDR_02: | ||
262 | /* TODO */ | ||
263 | case LOWPAN_IPHC_ADDR_03: | ||
264 | /* TODO */ | ||
265 | netdev_warn(skb->dev, "SAM value 0x%x not supported\n", sam); | ||
266 | return -EINVAL; | ||
267 | default: | ||
268 | pr_debug("Invalid sam value: 0x%x\n", sam); | ||
269 | return -EINVAL; | ||
270 | } | ||
271 | |||
272 | lowpan_raw_dump_inline(NULL, | ||
273 | "Reconstructed context based ipv6 src addr is:\n", | ||
274 | ipaddr->s6_addr, 16); | ||
275 | |||
276 | return 0; | ||
277 | } | ||
278 | |||
279 | /* Uncompress function for multicast destination address, | ||
280 | * when M bit is set. | ||
281 | */ | ||
282 | static int | ||
283 | lowpan_uncompress_multicast_daddr(struct sk_buff *skb, | ||
284 | struct in6_addr *ipaddr, | ||
285 | const u8 dam) | ||
286 | { | ||
287 | bool fail; | ||
288 | |||
289 | switch (dam) { | ||
290 | case LOWPAN_IPHC_DAM_00: | ||
291 | /* 00: 128 bits. The full address | ||
292 | * is carried in-line. | ||
293 | */ | ||
294 | fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); | ||
295 | break; | ||
296 | case LOWPAN_IPHC_DAM_01: | ||
297 | /* 01: 48 bits. The address takes | ||
298 | * the form ffXX::00XX:XXXX:XXXX. | ||
299 | */ | ||
300 | ipaddr->s6_addr[0] = 0xFF; | ||
301 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); | ||
302 | fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); | ||
303 | break; | ||
304 | case LOWPAN_IPHC_DAM_10: | ||
305 | /* 10: 32 bits. The address takes | ||
306 | * the form ffXX::00XX:XXXX. | ||
307 | */ | ||
308 | ipaddr->s6_addr[0] = 0xFF; | ||
309 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); | ||
310 | fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); | ||
311 | break; | ||
312 | case LOWPAN_IPHC_DAM_11: | ||
313 | /* 11: 8 bits. The address takes | ||
314 | * the form ff02::00XX. | ||
315 | */ | ||
316 | ipaddr->s6_addr[0] = 0xFF; | ||
317 | ipaddr->s6_addr[1] = 0x02; | ||
318 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); | ||
319 | break; | ||
320 | default: | ||
321 | pr_debug("DAM value has a wrong value: 0x%x\n", dam); | ||
322 | return -EINVAL; | ||
323 | } | ||
324 | |||
325 | if (fail) { | ||
326 | pr_debug("Failed to fetch skb data\n"); | ||
327 | return -EIO; | ||
328 | } | ||
329 | |||
330 | lowpan_raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is:\n", | ||
331 | ipaddr->s6_addr, 16); | ||
332 | |||
333 | return 0; | ||
334 | } | ||
335 | |||
336 | static void | ||
337 | lowpan_compress_udp_header(u8 **hc06_ptr, struct sk_buff *skb) | ||
338 | { | ||
339 | struct udphdr *uh = udp_hdr(skb); | ||
340 | |||
341 | if (((uh->source & LOWPAN_NHC_UDP_4BIT_MASK) == | ||
342 | LOWPAN_NHC_UDP_4BIT_PORT) && | ||
343 | ((uh->dest & LOWPAN_NHC_UDP_4BIT_MASK) == | ||
344 | LOWPAN_NHC_UDP_4BIT_PORT)) { | ||
345 | pr_debug("UDP header: both ports compression to 4 bits\n"); | ||
346 | **hc06_ptr = LOWPAN_NHC_UDP_CS_P_11; | ||
347 | **(hc06_ptr + 1) = /* subtraction is faster */ | ||
348 | (u8)((uh->dest - LOWPAN_NHC_UDP_4BIT_PORT) + | ||
349 | ((uh->source & LOWPAN_NHC_UDP_4BIT_PORT) << 4)); | ||
350 | *hc06_ptr += 2; | ||
351 | } else if ((uh->dest & LOWPAN_NHC_UDP_8BIT_MASK) == | ||
352 | LOWPAN_NHC_UDP_8BIT_PORT) { | ||
353 | pr_debug("UDP header: remove 8 bits of dest\n"); | ||
354 | **hc06_ptr = LOWPAN_NHC_UDP_CS_P_01; | ||
355 | memcpy(*hc06_ptr + 1, &uh->source, 2); | ||
356 | **(hc06_ptr + 3) = (u8)(uh->dest - LOWPAN_NHC_UDP_8BIT_PORT); | ||
357 | *hc06_ptr += 4; | ||
358 | } else if ((uh->source & LOWPAN_NHC_UDP_8BIT_MASK) == | ||
359 | LOWPAN_NHC_UDP_8BIT_PORT) { | ||
360 | pr_debug("UDP header: remove 8 bits of source\n"); | ||
361 | **hc06_ptr = LOWPAN_NHC_UDP_CS_P_10; | ||
362 | memcpy(*hc06_ptr + 1, &uh->dest, 2); | ||
363 | **(hc06_ptr + 3) = (u8)(uh->source - LOWPAN_NHC_UDP_8BIT_PORT); | ||
364 | *hc06_ptr += 4; | ||
365 | } else { | ||
366 | pr_debug("UDP header: can't compress\n"); | ||
367 | **hc06_ptr = LOWPAN_NHC_UDP_CS_P_00; | ||
368 | memcpy(*hc06_ptr + 1, &uh->source, 2); | ||
369 | memcpy(*hc06_ptr + 3, &uh->dest, 2); | ||
370 | *hc06_ptr += 5; | ||
371 | } | ||
372 | |||
373 | /* checksum is always inline */ | ||
374 | memcpy(*hc06_ptr, &uh->check, 2); | ||
375 | *hc06_ptr += 2; | ||
376 | |||
377 | /* skip the UDP header */ | ||
378 | skb_pull(skb, sizeof(struct udphdr)); | ||
379 | } | ||
380 | |||
381 | static inline int lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val) | ||
382 | { | ||
383 | if (unlikely(!pskb_may_pull(skb, 1))) | ||
384 | return -EINVAL; | ||
385 | |||
386 | *val = skb->data[0]; | ||
387 | skb_pull(skb, 1); | ||
388 | |||
389 | return 0; | ||
390 | } | ||
391 | |||
392 | static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val) | ||
393 | { | ||
394 | if (unlikely(!pskb_may_pull(skb, 2))) | ||
395 | return -EINVAL; | ||
396 | |||
397 | *val = (skb->data[0] << 8) | skb->data[1]; | ||
398 | skb_pull(skb, 2); | ||
399 | |||
400 | return 0; | ||
401 | } | ||
402 | |||
403 | static int | ||
404 | lowpan_uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh) | ||
405 | { | ||
406 | u8 tmp; | ||
407 | |||
408 | if (!uh) | ||
409 | goto err; | ||
410 | |||
411 | if (lowpan_fetch_skb_u8(skb, &tmp)) | ||
412 | goto err; | ||
413 | |||
414 | if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) { | ||
415 | pr_debug("UDP header uncompression\n"); | ||
416 | switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { | ||
417 | case LOWPAN_NHC_UDP_CS_P_00: | ||
418 | memcpy(&uh->source, &skb->data[0], 2); | ||
419 | memcpy(&uh->dest, &skb->data[2], 2); | ||
420 | skb_pull(skb, 4); | ||
421 | break; | ||
422 | case LOWPAN_NHC_UDP_CS_P_01: | ||
423 | memcpy(&uh->source, &skb->data[0], 2); | ||
424 | uh->dest = | ||
425 | skb->data[2] + LOWPAN_NHC_UDP_8BIT_PORT; | ||
426 | skb_pull(skb, 3); | ||
427 | break; | ||
428 | case LOWPAN_NHC_UDP_CS_P_10: | ||
429 | uh->source = skb->data[0] + LOWPAN_NHC_UDP_8BIT_PORT; | ||
430 | memcpy(&uh->dest, &skb->data[1], 2); | ||
431 | skb_pull(skb, 3); | ||
432 | break; | ||
433 | case LOWPAN_NHC_UDP_CS_P_11: | ||
434 | uh->source = | ||
435 | LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] >> 4); | ||
436 | uh->dest = | ||
437 | LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] & 0x0f); | ||
438 | skb_pull(skb, 1); | ||
439 | break; | ||
440 | default: | ||
441 | pr_debug("ERROR: unknown UDP format\n"); | ||
442 | goto err; | ||
443 | } | ||
444 | |||
445 | pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", | ||
446 | uh->source, uh->dest); | ||
447 | |||
448 | /* copy checksum */ | ||
449 | memcpy(&uh->check, &skb->data[0], 2); | ||
450 | skb_pull(skb, 2); | ||
451 | |||
452 | /* | ||
453 | * UDP lenght needs to be infered from the lower layers | ||
454 | * here, we obtain the hint from the remaining size of the | ||
455 | * frame | ||
456 | */ | ||
457 | uh->len = htons(skb->len + sizeof(struct udphdr)); | ||
458 | pr_debug("uncompressed UDP length: src = %d", uh->len); | ||
459 | } else { | ||
460 | pr_debug("ERROR: unsupported NH format\n"); | ||
461 | goto err; | ||
462 | } | ||
463 | |||
464 | return 0; | ||
465 | err: | ||
466 | return -EINVAL; | ||
467 | } | ||
468 | |||
469 | static int lowpan_header_create(struct sk_buff *skb, | 135 | static int lowpan_header_create(struct sk_buff *skb, |
470 | struct net_device *dev, | 136 | struct net_device *dev, |
471 | unsigned short type, const void *_daddr, | 137 | unsigned short type, const void *_daddr, |
472 | const void *_saddr, unsigned int len) | 138 | const void *_saddr, unsigned int len) |
473 | { | 139 | { |
474 | u8 tmp, iphc0, iphc1, *hc06_ptr; | ||
475 | struct ipv6hdr *hdr; | 140 | struct ipv6hdr *hdr; |
476 | const u8 *saddr = _saddr; | 141 | const u8 *saddr = _saddr; |
477 | const u8 *daddr = _daddr; | 142 | const u8 *daddr = _daddr; |
478 | u8 head[100]; | ||
479 | struct ieee802154_addr sa, da; | 143 | struct ieee802154_addr sa, da; |
480 | 144 | ||
481 | /* TODO: | 145 | /* TODO: |
@@ -485,181 +149,14 @@ static int lowpan_header_create(struct sk_buff *skb, | |||
485 | return 0; | 149 | return 0; |
486 | 150 | ||
487 | hdr = ipv6_hdr(skb); | 151 | hdr = ipv6_hdr(skb); |
488 | hc06_ptr = head + 2; | ||
489 | |||
490 | pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" | ||
491 | "\tnexthdr = 0x%02x\n\thop_lim = %d\n", hdr->version, | ||
492 | ntohs(hdr->payload_len), hdr->nexthdr, hdr->hop_limit); | ||
493 | |||
494 | lowpan_raw_dump_table(__func__, "raw skb network header dump", | ||
495 | skb_network_header(skb), sizeof(struct ipv6hdr)); | ||
496 | 152 | ||
497 | if (!saddr) | 153 | if (!saddr) |
498 | saddr = dev->dev_addr; | 154 | saddr = dev->dev_addr; |
499 | 155 | ||
500 | lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8); | 156 | lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8); |
501 | |||
502 | /* | ||
503 | * As we copy some bit-length fields, in the IPHC encoding bytes, | ||
504 | * we sometimes use |= | ||
505 | * If the field is 0, and the current bit value in memory is 1, | ||
506 | * this does not work. We therefore reset the IPHC encoding here | ||
507 | */ | ||
508 | iphc0 = LOWPAN_DISPATCH_IPHC; | ||
509 | iphc1 = 0; | ||
510 | |||
511 | /* TODO: context lookup */ | ||
512 | |||
513 | lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8); | 157 | lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8); |
514 | 158 | ||
515 | /* | 159 | lowpan_header_compress(skb, dev, type, daddr, saddr, len); |
516 | * Traffic class, flow label | ||
517 | * If flow label is 0, compress it. If traffic class is 0, compress it | ||
518 | * We have to process both in the same time as the offset of traffic | ||
519 | * class depends on the presence of version and flow label | ||
520 | */ | ||
521 | |||
522 | /* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */ | ||
523 | tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4); | ||
524 | tmp = ((tmp & 0x03) << 6) | (tmp >> 2); | ||
525 | |||
526 | if (((hdr->flow_lbl[0] & 0x0F) == 0) && | ||
527 | (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) { | ||
528 | /* flow label can be compressed */ | ||
529 | iphc0 |= LOWPAN_IPHC_FL_C; | ||
530 | if ((hdr->priority == 0) && | ||
531 | ((hdr->flow_lbl[0] & 0xF0) == 0)) { | ||
532 | /* compress (elide) all */ | ||
533 | iphc0 |= LOWPAN_IPHC_TC_C; | ||
534 | } else { | ||
535 | /* compress only the flow label */ | ||
536 | *hc06_ptr = tmp; | ||
537 | hc06_ptr += 1; | ||
538 | } | ||
539 | } else { | ||
540 | /* Flow label cannot be compressed */ | ||
541 | if ((hdr->priority == 0) && | ||
542 | ((hdr->flow_lbl[0] & 0xF0) == 0)) { | ||
543 | /* compress only traffic class */ | ||
544 | iphc0 |= LOWPAN_IPHC_TC_C; | ||
545 | *hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F); | ||
546 | memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2); | ||
547 | hc06_ptr += 3; | ||
548 | } else { | ||
549 | /* compress nothing */ | ||
550 | memcpy(hc06_ptr, &hdr, 4); | ||
551 | /* replace the top byte with new ECN | DSCP format */ | ||
552 | *hc06_ptr = tmp; | ||
553 | hc06_ptr += 4; | ||
554 | } | ||
555 | } | ||
556 | |||
557 | /* NOTE: payload length is always compressed */ | ||
558 | |||
559 | /* Next Header is compress if UDP */ | ||
560 | if (hdr->nexthdr == UIP_PROTO_UDP) | ||
561 | iphc0 |= LOWPAN_IPHC_NH_C; | ||
562 | |||
563 | if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { | ||
564 | *hc06_ptr = hdr->nexthdr; | ||
565 | hc06_ptr += 1; | ||
566 | } | ||
567 | |||
568 | /* | ||
569 | * Hop limit | ||
570 | * if 1: compress, encoding is 01 | ||
571 | * if 64: compress, encoding is 10 | ||
572 | * if 255: compress, encoding is 11 | ||
573 | * else do not compress | ||
574 | */ | ||
575 | switch (hdr->hop_limit) { | ||
576 | case 1: | ||
577 | iphc0 |= LOWPAN_IPHC_TTL_1; | ||
578 | break; | ||
579 | case 64: | ||
580 | iphc0 |= LOWPAN_IPHC_TTL_64; | ||
581 | break; | ||
582 | case 255: | ||
583 | iphc0 |= LOWPAN_IPHC_TTL_255; | ||
584 | break; | ||
585 | default: | ||
586 | *hc06_ptr = hdr->hop_limit; | ||
587 | hc06_ptr += 1; | ||
588 | break; | ||
589 | } | ||
590 | |||
591 | /* source address compression */ | ||
592 | if (is_addr_unspecified(&hdr->saddr)) { | ||
593 | pr_debug("source address is unspecified, setting SAC\n"); | ||
594 | iphc1 |= LOWPAN_IPHC_SAC; | ||
595 | /* TODO: context lookup */ | ||
596 | } else if (is_addr_link_local(&hdr->saddr)) { | ||
597 | pr_debug("source address is link-local\n"); | ||
598 | iphc1 |= lowpan_compress_addr_64(&hc06_ptr, | ||
599 | LOWPAN_IPHC_SAM_BIT, &hdr->saddr, saddr); | ||
600 | } else { | ||
601 | pr_debug("send the full source address\n"); | ||
602 | memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16); | ||
603 | hc06_ptr += 16; | ||
604 | } | ||
605 | |||
606 | /* destination address compression */ | ||
607 | if (is_addr_mcast(&hdr->daddr)) { | ||
608 | pr_debug("destination address is multicast: "); | ||
609 | iphc1 |= LOWPAN_IPHC_M; | ||
610 | if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) { | ||
611 | pr_debug("compressed to 1 octet\n"); | ||
612 | iphc1 |= LOWPAN_IPHC_DAM_11; | ||
613 | /* use last byte */ | ||
614 | *hc06_ptr = hdr->daddr.s6_addr[15]; | ||
615 | hc06_ptr += 1; | ||
616 | } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) { | ||
617 | pr_debug("compressed to 4 octets\n"); | ||
618 | iphc1 |= LOWPAN_IPHC_DAM_10; | ||
619 | /* second byte + the last three */ | ||
620 | *hc06_ptr = hdr->daddr.s6_addr[1]; | ||
621 | memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3); | ||
622 | hc06_ptr += 4; | ||
623 | } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) { | ||
624 | pr_debug("compressed to 6 octets\n"); | ||
625 | iphc1 |= LOWPAN_IPHC_DAM_01; | ||
626 | /* second byte + the last five */ | ||
627 | *hc06_ptr = hdr->daddr.s6_addr[1]; | ||
628 | memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5); | ||
629 | hc06_ptr += 6; | ||
630 | } else { | ||
631 | pr_debug("using full address\n"); | ||
632 | iphc1 |= LOWPAN_IPHC_DAM_00; | ||
633 | memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16); | ||
634 | hc06_ptr += 16; | ||
635 | } | ||
636 | } else { | ||
637 | /* TODO: context lookup */ | ||
638 | if (is_addr_link_local(&hdr->daddr)) { | ||
639 | pr_debug("dest address is unicast and link-local\n"); | ||
640 | iphc1 |= lowpan_compress_addr_64(&hc06_ptr, | ||
641 | LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr); | ||
642 | } else { | ||
643 | pr_debug("dest address is unicast: using full one\n"); | ||
644 | memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16); | ||
645 | hc06_ptr += 16; | ||
646 | } | ||
647 | } | ||
648 | |||
649 | /* UDP header compression */ | ||
650 | if (hdr->nexthdr == UIP_PROTO_UDP) | ||
651 | lowpan_compress_udp_header(&hc06_ptr, skb); | ||
652 | |||
653 | head[0] = iphc0; | ||
654 | head[1] = iphc1; | ||
655 | |||
656 | skb_pull(skb, sizeof(struct ipv6hdr)); | ||
657 | skb_reset_transport_header(skb); | ||
658 | memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head); | ||
659 | skb_reset_network_header(skb); | ||
660 | |||
661 | lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data, | ||
662 | skb->len); | ||
663 | 160 | ||
664 | /* | 161 | /* |
665 | * NOTE1: I'm still unsure about the fact that compression and WPAN | 162 | * NOTE1: I'm still unsure about the fact that compression and WPAN |
@@ -671,39 +168,38 @@ static int lowpan_header_create(struct sk_buff *skb, | |||
671 | * from MAC subif of the 'dev' and 'real_dev' network devices, but | 168 | * from MAC subif of the 'dev' and 'real_dev' network devices, but |
672 | * this isn't implemented in mainline yet, so currently we assign 0xff | 169 | * this isn't implemented in mainline yet, so currently we assign 0xff |
673 | */ | 170 | */ |
674 | { | 171 | mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA; |
675 | mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA; | 172 | mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev); |
676 | mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev); | ||
677 | 173 | ||
678 | /* prepare wpan address data */ | 174 | /* prepare wpan address data */ |
679 | sa.addr_type = IEEE802154_ADDR_LONG; | 175 | sa.addr_type = IEEE802154_ADDR_LONG; |
680 | sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev); | 176 | sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev); |
681 | 177 | ||
682 | memcpy(&(sa.hwaddr), saddr, 8); | 178 | memcpy(&(sa.hwaddr), saddr, 8); |
683 | /* intra-PAN communications */ | 179 | /* intra-PAN communications */ |
684 | da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev); | 180 | da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev); |
685 | 181 | ||
686 | /* | 182 | /* |
687 | * if the destination address is the broadcast address, use the | 183 | * if the destination address is the broadcast address, use the |
688 | * corresponding short address | 184 | * corresponding short address |
689 | */ | 185 | */ |
690 | if (lowpan_is_addr_broadcast(daddr)) { | 186 | if (lowpan_is_addr_broadcast(daddr)) { |
691 | da.addr_type = IEEE802154_ADDR_SHORT; | 187 | da.addr_type = IEEE802154_ADDR_SHORT; |
692 | da.short_addr = IEEE802154_ADDR_BROADCAST; | 188 | da.short_addr = IEEE802154_ADDR_BROADCAST; |
693 | } else { | 189 | } else { |
694 | da.addr_type = IEEE802154_ADDR_LONG; | 190 | da.addr_type = IEEE802154_ADDR_LONG; |
695 | memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN); | 191 | memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN); |
696 | |||
697 | /* request acknowledgment */ | ||
698 | mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ; | ||
699 | } | ||
700 | 192 | ||
701 | return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev, | 193 | /* request acknowledgment */ |
702 | type, (void *)&da, (void *)&sa, skb->len); | 194 | mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ; |
703 | } | 195 | } |
196 | |||
197 | return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev, | ||
198 | type, (void *)&da, (void *)&sa, skb->len); | ||
704 | } | 199 | } |
705 | 200 | ||
706 | static int lowpan_give_skb_to_devices(struct sk_buff *skb) | 201 | static int lowpan_give_skb_to_devices(struct sk_buff *skb, |
202 | struct net_device *dev) | ||
707 | { | 203 | { |
708 | struct lowpan_dev_record *entry; | 204 | struct lowpan_dev_record *entry; |
709 | struct sk_buff *skb_cp; | 205 | struct sk_buff *skb_cp; |
@@ -726,31 +222,6 @@ static int lowpan_give_skb_to_devices(struct sk_buff *skb) | |||
726 | return stat; | 222 | return stat; |
727 | } | 223 | } |
728 | 224 | ||
729 | static int lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr) | ||
730 | { | ||
731 | struct sk_buff *new; | ||
732 | int stat = NET_RX_SUCCESS; | ||
733 | |||
734 | new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb), | ||
735 | GFP_ATOMIC); | ||
736 | kfree_skb(skb); | ||
737 | |||
738 | if (!new) | ||
739 | return -ENOMEM; | ||
740 | |||
741 | skb_push(new, sizeof(struct ipv6hdr)); | ||
742 | skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr)); | ||
743 | |||
744 | new->protocol = htons(ETH_P_IPV6); | ||
745 | new->pkt_type = PACKET_HOST; | ||
746 | |||
747 | stat = lowpan_give_skb_to_devices(new); | ||
748 | |||
749 | kfree_skb(new); | ||
750 | |||
751 | return stat; | ||
752 | } | ||
753 | |||
754 | static void lowpan_fragment_timer_expired(unsigned long entry_addr) | 225 | static void lowpan_fragment_timer_expired(unsigned long entry_addr) |
755 | { | 226 | { |
756 | struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr; | 227 | struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr; |
@@ -814,13 +285,10 @@ frame_err: | |||
814 | return NULL; | 285 | return NULL; |
815 | } | 286 | } |
816 | 287 | ||
817 | static int | 288 | static int process_data(struct sk_buff *skb) |
818 | lowpan_process_data(struct sk_buff *skb) | ||
819 | { | 289 | { |
820 | struct ipv6hdr hdr = {}; | 290 | u8 iphc0, iphc1; |
821 | u8 tmp, iphc0, iphc1, num_context = 0; | ||
822 | const struct ieee802154_addr *_saddr, *_daddr; | 291 | const struct ieee802154_addr *_saddr, *_daddr; |
823 | int err; | ||
824 | 292 | ||
825 | lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data, | 293 | lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data, |
826 | skb->len); | 294 | skb->len); |
@@ -925,162 +393,11 @@ lowpan_process_data(struct sk_buff *skb) | |||
925 | _saddr = &mac_cb(skb)->sa; | 393 | _saddr = &mac_cb(skb)->sa; |
926 | _daddr = &mac_cb(skb)->da; | 394 | _daddr = &mac_cb(skb)->da; |
927 | 395 | ||
928 | pr_debug("iphc0 = %02x, iphc1 = %02x\n", iphc0, iphc1); | 396 | return lowpan_process_data(skb, skb->dev, (u8 *)_saddr->hwaddr, |
929 | 397 | _saddr->addr_type, IEEE802154_ADDR_LEN, | |
930 | /* another if the CID flag is set */ | 398 | (u8 *)_daddr->hwaddr, _daddr->addr_type, |
931 | if (iphc1 & LOWPAN_IPHC_CID) { | 399 | IEEE802154_ADDR_LEN, iphc0, iphc1, |
932 | pr_debug("CID flag is set, increase header with one\n"); | 400 | lowpan_give_skb_to_devices); |
933 | if (lowpan_fetch_skb_u8(skb, &num_context)) | ||
934 | goto drop; | ||
935 | } | ||
936 | |||
937 | hdr.version = 6; | ||
938 | |||
939 | /* Traffic Class and Flow Label */ | ||
940 | switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) { | ||
941 | /* | ||
942 | * Traffic Class and FLow Label carried in-line | ||
943 | * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) | ||
944 | */ | ||
945 | case 0: /* 00b */ | ||
946 | if (lowpan_fetch_skb_u8(skb, &tmp)) | ||
947 | goto drop; | ||
948 | |||
949 | memcpy(&hdr.flow_lbl, &skb->data[0], 3); | ||
950 | skb_pull(skb, 3); | ||
951 | hdr.priority = ((tmp >> 2) & 0x0f); | ||
952 | hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) | | ||
953 | (hdr.flow_lbl[0] & 0x0f); | ||
954 | break; | ||
955 | /* | ||
956 | * Traffic class carried in-line | ||
957 | * ECN + DSCP (1 byte), Flow Label is elided | ||
958 | */ | ||
959 | case 2: /* 10b */ | ||
960 | if (lowpan_fetch_skb_u8(skb, &tmp)) | ||
961 | goto drop; | ||
962 | |||
963 | hdr.priority = ((tmp >> 2) & 0x0f); | ||
964 | hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30); | ||
965 | break; | ||
966 | /* | ||
967 | * Flow Label carried in-line | ||
968 | * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided | ||
969 | */ | ||
970 | case 1: /* 01b */ | ||
971 | if (lowpan_fetch_skb_u8(skb, &tmp)) | ||
972 | goto drop; | ||
973 | |||
974 | hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30); | ||
975 | memcpy(&hdr.flow_lbl[1], &skb->data[0], 2); | ||
976 | skb_pull(skb, 2); | ||
977 | break; | ||
978 | /* Traffic Class and Flow Label are elided */ | ||
979 | case 3: /* 11b */ | ||
980 | break; | ||
981 | default: | ||
982 | break; | ||
983 | } | ||
984 | |||
985 | /* Next Header */ | ||
986 | if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { | ||
987 | /* Next header is carried inline */ | ||
988 | if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr))) | ||
989 | goto drop; | ||
990 | |||
991 | pr_debug("NH flag is set, next header carried inline: %02x\n", | ||
992 | hdr.nexthdr); | ||
993 | } | ||
994 | |||
995 | /* Hop Limit */ | ||
996 | if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I) | ||
997 | hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03]; | ||
998 | else { | ||
999 | if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit))) | ||
1000 | goto drop; | ||
1001 | } | ||
1002 | |||
1003 | /* Extract SAM to the tmp variable */ | ||
1004 | tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03; | ||
1005 | |||
1006 | if (iphc1 & LOWPAN_IPHC_SAC) { | ||
1007 | /* Source address context based uncompression */ | ||
1008 | pr_debug("SAC bit is set. Handle context based source address.\n"); | ||
1009 | err = lowpan_uncompress_context_based_src_addr( | ||
1010 | skb, &hdr.saddr, tmp); | ||
1011 | } else { | ||
1012 | /* Source address uncompression */ | ||
1013 | pr_debug("source address stateless compression\n"); | ||
1014 | err = lowpan_uncompress_addr(skb, &hdr.saddr, tmp, _saddr); | ||
1015 | } | ||
1016 | |||
1017 | /* Check on error of previous branch */ | ||
1018 | if (err) | ||
1019 | goto drop; | ||
1020 | |||
1021 | /* Extract DAM to the tmp variable */ | ||
1022 | tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03; | ||
1023 | |||
1024 | /* check for Multicast Compression */ | ||
1025 | if (iphc1 & LOWPAN_IPHC_M) { | ||
1026 | if (iphc1 & LOWPAN_IPHC_DAC) { | ||
1027 | pr_debug("dest: context-based mcast compression\n"); | ||
1028 | /* TODO: implement this */ | ||
1029 | } else { | ||
1030 | err = lowpan_uncompress_multicast_daddr( | ||
1031 | skb, &hdr.daddr, tmp); | ||
1032 | if (err) | ||
1033 | goto drop; | ||
1034 | } | ||
1035 | } else { | ||
1036 | pr_debug("dest: stateless compression\n"); | ||
1037 | err = lowpan_uncompress_addr(skb, &hdr.daddr, tmp, _daddr); | ||
1038 | if (err) | ||
1039 | goto drop; | ||
1040 | } | ||
1041 | |||
1042 | /* UDP data uncompression */ | ||
1043 | if (iphc0 & LOWPAN_IPHC_NH_C) { | ||
1044 | struct udphdr uh; | ||
1045 | struct sk_buff *new; | ||
1046 | if (lowpan_uncompress_udp_header(skb, &uh)) | ||
1047 | goto drop; | ||
1048 | |||
1049 | /* | ||
1050 | * replace the compressed UDP head by the uncompressed UDP | ||
1051 | * header | ||
1052 | */ | ||
1053 | new = skb_copy_expand(skb, sizeof(struct udphdr), | ||
1054 | skb_tailroom(skb), GFP_ATOMIC); | ||
1055 | kfree_skb(skb); | ||
1056 | |||
1057 | if (!new) | ||
1058 | return -ENOMEM; | ||
1059 | |||
1060 | skb = new; | ||
1061 | |||
1062 | skb_push(skb, sizeof(struct udphdr)); | ||
1063 | skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); | ||
1064 | |||
1065 | lowpan_raw_dump_table(__func__, "raw UDP header dump", | ||
1066 | (u8 *)&uh, sizeof(uh)); | ||
1067 | |||
1068 | hdr.nexthdr = UIP_PROTO_UDP; | ||
1069 | } | ||
1070 | |||
1071 | /* Not fragmented package */ | ||
1072 | hdr.payload_len = htons(skb->len); | ||
1073 | |||
1074 | pr_debug("skb headroom size = %d, data length = %d\n", | ||
1075 | skb_headroom(skb), skb->len); | ||
1076 | |||
1077 | pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t" | ||
1078 | "nexthdr = 0x%02x\n\thop_lim = %d\n", hdr.version, | ||
1079 | ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit); | ||
1080 | |||
1081 | lowpan_raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, | ||
1082 | sizeof(hdr)); | ||
1083 | return lowpan_skb_deliver(skb, &hdr); | ||
1084 | 401 | ||
1085 | unlock_and_drop: | 402 | unlock_and_drop: |
1086 | spin_unlock_bh(&flist_lock); | 403 | spin_unlock_bh(&flist_lock); |
@@ -1316,7 +633,7 @@ static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev, | |||
1316 | /* Pull off the 1-byte of 6lowpan header. */ | 633 | /* Pull off the 1-byte of 6lowpan header. */ |
1317 | skb_pull(local_skb, 1); | 634 | skb_pull(local_skb, 1); |
1318 | 635 | ||
1319 | lowpan_give_skb_to_devices(local_skb); | 636 | lowpan_give_skb_to_devices(local_skb, NULL); |
1320 | 637 | ||
1321 | kfree_skb(local_skb); | 638 | kfree_skb(local_skb); |
1322 | kfree_skb(skb); | 639 | kfree_skb(skb); |
@@ -1328,7 +645,7 @@ static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev, | |||
1328 | local_skb = skb_clone(skb, GFP_ATOMIC); | 645 | local_skb = skb_clone(skb, GFP_ATOMIC); |
1329 | if (!local_skb) | 646 | if (!local_skb) |
1330 | goto drop; | 647 | goto drop; |
1331 | lowpan_process_data(local_skb); | 648 | process_data(local_skb); |
1332 | 649 | ||
1333 | kfree_skb(skb); | 650 | kfree_skb(skb); |
1334 | break; | 651 | break; |
diff --git a/net/ieee802154/6lowpan.h b/net/ieee802154/6lowpan.h index 2869c0526dad..535606d45c39 100644 --- a/net/ieee802154/6lowpan.h +++ b/net/ieee802154/6lowpan.h | |||
@@ -232,6 +232,28 @@ | |||
232 | dest = 16 bit inline */ | 232 | dest = 16 bit inline */ |
233 | #define LOWPAN_NHC_UDP_CS_P_11 0xF3 /* source & dest = 0xF0B + 4bit inline */ | 233 | #define LOWPAN_NHC_UDP_CS_P_11 0xF3 /* source & dest = 0xF0B + 4bit inline */ |
234 | 234 | ||
235 | static inline int lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val) | ||
236 | { | ||
237 | if (unlikely(!pskb_may_pull(skb, 1))) | ||
238 | return -EINVAL; | ||
239 | |||
240 | *val = skb->data[0]; | ||
241 | skb_pull(skb, 1); | ||
242 | |||
243 | return 0; | ||
244 | } | ||
245 | |||
246 | static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val) | ||
247 | { | ||
248 | if (unlikely(!pskb_may_pull(skb, 2))) | ||
249 | return -EINVAL; | ||
250 | |||
251 | *val = (skb->data[0] << 8) | skb->data[1]; | ||
252 | skb_pull(skb, 2); | ||
253 | |||
254 | return 0; | ||
255 | } | ||
256 | |||
235 | static inline bool lowpan_fetch_skb(struct sk_buff *skb, | 257 | static inline bool lowpan_fetch_skb(struct sk_buff *skb, |
236 | void *data, const unsigned int len) | 258 | void *data, const unsigned int len) |
237 | { | 259 | { |
@@ -244,4 +266,14 @@ static inline bool lowpan_fetch_skb(struct sk_buff *skb, | |||
244 | return false; | 266 | return false; |
245 | } | 267 | } |
246 | 268 | ||
269 | typedef int (*skb_delivery_cb)(struct sk_buff *skb, struct net_device *dev); | ||
270 | |||
271 | int lowpan_process_data(struct sk_buff *skb, struct net_device *dev, | ||
272 | const u8 *saddr, const u8 saddr_type, const u8 saddr_len, | ||
273 | const u8 *daddr, const u8 daddr_type, const u8 daddr_len, | ||
274 | u8 iphc0, u8 iphc1, skb_delivery_cb skb_deliver); | ||
275 | int lowpan_header_compress(struct sk_buff *skb, struct net_device *dev, | ||
276 | unsigned short type, const void *_daddr, | ||
277 | const void *_saddr, unsigned int len); | ||
278 | |||
247 | #endif /* __6LOWPAN_H__ */ | 279 | #endif /* __6LOWPAN_H__ */ |
diff --git a/net/ieee802154/6lowpan_iphc.c b/net/ieee802154/6lowpan_iphc.c new file mode 100644 index 000000000000..57c0b7ad6b6c --- /dev/null +++ b/net/ieee802154/6lowpan_iphc.c | |||
@@ -0,0 +1,807 @@ | |||
1 | /* | ||
2 | * Copyright 2011, Siemens AG | ||
3 | * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com> | ||
4 | */ | ||
5 | |||
6 | /* | ||
7 | * Based on patches from Jon Smirl <jonsmirl@gmail.com> | ||
8 | * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com> | ||
9 | * | ||
10 | * This program is free software; you can redistribute it and/or modify | ||
11 | * it under the terms of the GNU General Public License version 2 | ||
12 | * as published by the Free Software Foundation. | ||
13 | * | ||
14 | * This program is distributed in the hope that it will be useful, | ||
15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
17 | * GNU General Public License for more details. | ||
18 | * | ||
19 | * You should have received a copy of the GNU General Public License along | ||
20 | * with this program; if not, write to the Free Software Foundation, Inc., | ||
21 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. | ||
22 | */ | ||
23 | |||
24 | /* Jon's code is based on 6lowpan implementation for Contiki which is: | ||
25 | * Copyright (c) 2008, Swedish Institute of Computer Science. | ||
26 | * All rights reserved. | ||
27 | * | ||
28 | * Redistribution and use in source and binary forms, with or without | ||
29 | * modification, are permitted provided that the following conditions | ||
30 | * are met: | ||
31 | * 1. Redistributions of source code must retain the above copyright | ||
32 | * notice, this list of conditions and the following disclaimer. | ||
33 | * 2. Redistributions in binary form must reproduce the above copyright | ||
34 | * notice, this list of conditions and the following disclaimer in the | ||
35 | * documentation and/or other materials provided with the distribution. | ||
36 | * 3. Neither the name of the Institute nor the names of its contributors | ||
37 | * may be used to endorse or promote products derived from this software | ||
38 | * without specific prior written permission. | ||
39 | * | ||
40 | * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND | ||
41 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | ||
42 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | ||
43 | * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE | ||
44 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL | ||
45 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS | ||
46 | * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) | ||
47 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
48 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY | ||
49 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF | ||
50 | * SUCH DAMAGE. | ||
51 | */ | ||
52 | |||
53 | #include <linux/bitops.h> | ||
54 | #include <linux/if_arp.h> | ||
55 | #include <linux/netdevice.h> | ||
56 | #include <net/ipv6.h> | ||
57 | #include <net/af_ieee802154.h> | ||
58 | |||
59 | #include "6lowpan.h" | ||
60 | |||
61 | /* print data in line */ | ||
62 | static inline void raw_dump_inline(const char *caller, char *msg, | ||
63 | unsigned char *buf, int len) | ||
64 | { | ||
65 | if (msg) | ||
66 | pr_debug("%s():%s: ", caller, msg); | ||
67 | print_hex_dump_debug("", DUMP_PREFIX_NONE, | ||
68 | 16, 1, buf, len, false); | ||
69 | } | ||
70 | |||
71 | /* | ||
72 | * print data in a table format: | ||
73 | * | ||
74 | * addr: xx xx xx xx xx xx | ||
75 | * addr: xx xx xx xx xx xx | ||
76 | * ... | ||
77 | */ | ||
78 | static inline void raw_dump_table(const char *caller, char *msg, | ||
79 | unsigned char *buf, int len) | ||
80 | { | ||
81 | if (msg) | ||
82 | pr_debug("%s():%s:\n", caller, msg); | ||
83 | print_hex_dump_debug("\t", DUMP_PREFIX_OFFSET, | ||
84 | 16, 1, buf, len, false); | ||
85 | } | ||
86 | |||
87 | /* | ||
88 | * Uncompress address function for source and | ||
89 | * destination address(non-multicast). | ||
90 | * | ||
91 | * address_mode is sam value or dam value. | ||
92 | */ | ||
93 | static int uncompress_addr(struct sk_buff *skb, | ||
94 | struct in6_addr *ipaddr, const u8 address_mode, | ||
95 | const u8 *lladdr, const u8 addr_type, | ||
96 | const u8 addr_len) | ||
97 | { | ||
98 | bool fail; | ||
99 | |||
100 | switch (address_mode) { | ||
101 | case LOWPAN_IPHC_ADDR_00: | ||
102 | /* for global link addresses */ | ||
103 | fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); | ||
104 | break; | ||
105 | case LOWPAN_IPHC_ADDR_01: | ||
106 | /* fe:80::XXXX:XXXX:XXXX:XXXX */ | ||
107 | ipaddr->s6_addr[0] = 0xFE; | ||
108 | ipaddr->s6_addr[1] = 0x80; | ||
109 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); | ||
110 | break; | ||
111 | case LOWPAN_IPHC_ADDR_02: | ||
112 | /* fe:80::ff:fe00:XXXX */ | ||
113 | ipaddr->s6_addr[0] = 0xFE; | ||
114 | ipaddr->s6_addr[1] = 0x80; | ||
115 | ipaddr->s6_addr[11] = 0xFF; | ||
116 | ipaddr->s6_addr[12] = 0xFE; | ||
117 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); | ||
118 | break; | ||
119 | case LOWPAN_IPHC_ADDR_03: | ||
120 | fail = false; | ||
121 | switch (addr_type) { | ||
122 | case IEEE802154_ADDR_LONG: | ||
123 | /* fe:80::XXXX:XXXX:XXXX:XXXX | ||
124 | * \_________________/ | ||
125 | * hwaddr | ||
126 | */ | ||
127 | ipaddr->s6_addr[0] = 0xFE; | ||
128 | ipaddr->s6_addr[1] = 0x80; | ||
129 | memcpy(&ipaddr->s6_addr[8], lladdr, addr_len); | ||
130 | /* second bit-flip (Universe/Local) | ||
131 | * is done according RFC2464 | ||
132 | */ | ||
133 | ipaddr->s6_addr[8] ^= 0x02; | ||
134 | break; | ||
135 | case IEEE802154_ADDR_SHORT: | ||
136 | /* fe:80::ff:fe00:XXXX | ||
137 | * \__/ | ||
138 | * short_addr | ||
139 | * | ||
140 | * Universe/Local bit is zero. | ||
141 | */ | ||
142 | ipaddr->s6_addr[0] = 0xFE; | ||
143 | ipaddr->s6_addr[1] = 0x80; | ||
144 | ipaddr->s6_addr[11] = 0xFF; | ||
145 | ipaddr->s6_addr[12] = 0xFE; | ||
146 | ipaddr->s6_addr16[7] = htons(*((u16 *)lladdr)); | ||
147 | break; | ||
148 | default: | ||
149 | pr_debug("Invalid addr_type set\n"); | ||
150 | return -EINVAL; | ||
151 | } | ||
152 | break; | ||
153 | default: | ||
154 | pr_debug("Invalid address mode value: 0x%x\n", address_mode); | ||
155 | return -EINVAL; | ||
156 | } | ||
157 | |||
158 | if (fail) { | ||
159 | pr_debug("Failed to fetch skb data\n"); | ||
160 | return -EIO; | ||
161 | } | ||
162 | |||
163 | raw_dump_inline(NULL, "Reconstructed ipv6 addr is", | ||
164 | ipaddr->s6_addr, 16); | ||
165 | |||
166 | return 0; | ||
167 | } | ||
168 | |||
169 | /* | ||
170 | * Uncompress address function for source context | ||
171 | * based address(non-multicast). | ||
172 | */ | ||
173 | static int uncompress_context_based_src_addr(struct sk_buff *skb, | ||
174 | struct in6_addr *ipaddr, | ||
175 | const u8 sam) | ||
176 | { | ||
177 | switch (sam) { | ||
178 | case LOWPAN_IPHC_ADDR_00: | ||
179 | /* unspec address :: | ||
180 | * Do nothing, address is already :: | ||
181 | */ | ||
182 | break; | ||
183 | case LOWPAN_IPHC_ADDR_01: | ||
184 | /* TODO */ | ||
185 | case LOWPAN_IPHC_ADDR_02: | ||
186 | /* TODO */ | ||
187 | case LOWPAN_IPHC_ADDR_03: | ||
188 | /* TODO */ | ||
189 | netdev_warn(skb->dev, "SAM value 0x%x not supported\n", sam); | ||
190 | return -EINVAL; | ||
191 | default: | ||
192 | pr_debug("Invalid sam value: 0x%x\n", sam); | ||
193 | return -EINVAL; | ||
194 | } | ||
195 | |||
196 | raw_dump_inline(NULL, | ||
197 | "Reconstructed context based ipv6 src addr is", | ||
198 | ipaddr->s6_addr, 16); | ||
199 | |||
200 | return 0; | ||
201 | } | ||
202 | |||
203 | static int skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr, | ||
204 | struct net_device *dev, skb_delivery_cb deliver_skb) | ||
205 | { | ||
206 | struct sk_buff *new; | ||
207 | int stat; | ||
208 | |||
209 | new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb), | ||
210 | GFP_ATOMIC); | ||
211 | kfree_skb(skb); | ||
212 | |||
213 | if (!new) | ||
214 | return -ENOMEM; | ||
215 | |||
216 | skb_push(new, sizeof(struct ipv6hdr)); | ||
217 | skb_reset_network_header(new); | ||
218 | skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr)); | ||
219 | |||
220 | new->protocol = htons(ETH_P_IPV6); | ||
221 | new->pkt_type = PACKET_HOST; | ||
222 | new->dev = dev; | ||
223 | |||
224 | raw_dump_table(__func__, "raw skb data dump before receiving", | ||
225 | new->data, new->len); | ||
226 | |||
227 | stat = deliver_skb(new, dev); | ||
228 | |||
229 | kfree_skb(new); | ||
230 | |||
231 | return stat; | ||
232 | } | ||
233 | |||
234 | /* Uncompress function for multicast destination address, | ||
235 | * when M bit is set. | ||
236 | */ | ||
237 | static int | ||
238 | lowpan_uncompress_multicast_daddr(struct sk_buff *skb, | ||
239 | struct in6_addr *ipaddr, | ||
240 | const u8 dam) | ||
241 | { | ||
242 | bool fail; | ||
243 | |||
244 | switch (dam) { | ||
245 | case LOWPAN_IPHC_DAM_00: | ||
246 | /* 00: 128 bits. The full address | ||
247 | * is carried in-line. | ||
248 | */ | ||
249 | fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); | ||
250 | break; | ||
251 | case LOWPAN_IPHC_DAM_01: | ||
252 | /* 01: 48 bits. The address takes | ||
253 | * the form ffXX::00XX:XXXX:XXXX. | ||
254 | */ | ||
255 | ipaddr->s6_addr[0] = 0xFF; | ||
256 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); | ||
257 | fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); | ||
258 | break; | ||
259 | case LOWPAN_IPHC_DAM_10: | ||
260 | /* 10: 32 bits. The address takes | ||
261 | * the form ffXX::00XX:XXXX. | ||
262 | */ | ||
263 | ipaddr->s6_addr[0] = 0xFF; | ||
264 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); | ||
265 | fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); | ||
266 | break; | ||
267 | case LOWPAN_IPHC_DAM_11: | ||
268 | /* 11: 8 bits. The address takes | ||
269 | * the form ff02::00XX. | ||
270 | */ | ||
271 | ipaddr->s6_addr[0] = 0xFF; | ||
272 | ipaddr->s6_addr[1] = 0x02; | ||
273 | fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); | ||
274 | break; | ||
275 | default: | ||
276 | pr_debug("DAM value has a wrong value: 0x%x\n", dam); | ||
277 | return -EINVAL; | ||
278 | } | ||
279 | |||
280 | if (fail) { | ||
281 | pr_debug("Failed to fetch skb data\n"); | ||
282 | return -EIO; | ||
283 | } | ||
284 | |||
285 | raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is", | ||
286 | ipaddr->s6_addr, 16); | ||
287 | |||
288 | return 0; | ||
289 | } | ||
290 | |||
291 | static int | ||
292 | uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh) | ||
293 | { | ||
294 | u8 tmp; | ||
295 | |||
296 | if (!uh) | ||
297 | goto err; | ||
298 | |||
299 | if (lowpan_fetch_skb_u8(skb, &tmp)) | ||
300 | goto err; | ||
301 | |||
302 | if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) { | ||
303 | pr_debug("UDP header uncompression\n"); | ||
304 | switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { | ||
305 | case LOWPAN_NHC_UDP_CS_P_00: | ||
306 | memcpy(&uh->source, &skb->data[0], 2); | ||
307 | memcpy(&uh->dest, &skb->data[2], 2); | ||
308 | skb_pull(skb, 4); | ||
309 | break; | ||
310 | case LOWPAN_NHC_UDP_CS_P_01: | ||
311 | memcpy(&uh->source, &skb->data[0], 2); | ||
312 | uh->dest = | ||
313 | skb->data[2] + LOWPAN_NHC_UDP_8BIT_PORT; | ||
314 | skb_pull(skb, 3); | ||
315 | break; | ||
316 | case LOWPAN_NHC_UDP_CS_P_10: | ||
317 | uh->source = skb->data[0] + LOWPAN_NHC_UDP_8BIT_PORT; | ||
318 | memcpy(&uh->dest, &skb->data[1], 2); | ||
319 | skb_pull(skb, 3); | ||
320 | break; | ||
321 | case LOWPAN_NHC_UDP_CS_P_11: | ||
322 | uh->source = | ||
323 | LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] >> 4); | ||
324 | uh->dest = | ||
325 | LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] & 0x0f); | ||
326 | skb_pull(skb, 1); | ||
327 | break; | ||
328 | default: | ||
329 | pr_debug("ERROR: unknown UDP format\n"); | ||
330 | goto err; | ||
331 | break; | ||
332 | } | ||
333 | |||
334 | pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", | ||
335 | uh->source, uh->dest); | ||
336 | |||
337 | /* copy checksum */ | ||
338 | memcpy(&uh->check, &skb->data[0], 2); | ||
339 | skb_pull(skb, 2); | ||
340 | |||
341 | /* | ||
342 | * UDP lenght needs to be infered from the lower layers | ||
343 | * here, we obtain the hint from the remaining size of the | ||
344 | * frame | ||
345 | */ | ||
346 | uh->len = htons(skb->len + sizeof(struct udphdr)); | ||
347 | pr_debug("uncompressed UDP length: src = %d", uh->len); | ||
348 | } else { | ||
349 | pr_debug("ERROR: unsupported NH format\n"); | ||
350 | goto err; | ||
351 | } | ||
352 | |||
353 | return 0; | ||
354 | err: | ||
355 | return -EINVAL; | ||
356 | } | ||
357 | |||
358 | /* TTL uncompression values */ | ||
359 | static const u8 lowpan_ttl_values[] = { 0, 1, 64, 255 }; | ||
360 | |||
361 | int lowpan_process_data(struct sk_buff *skb, struct net_device *dev, | ||
362 | const u8 *saddr, const u8 saddr_type, const u8 saddr_len, | ||
363 | const u8 *daddr, const u8 daddr_type, const u8 daddr_len, | ||
364 | u8 iphc0, u8 iphc1, skb_delivery_cb deliver_skb) | ||
365 | { | ||
366 | struct ipv6hdr hdr = {}; | ||
367 | u8 tmp, num_context = 0; | ||
368 | int err; | ||
369 | |||
370 | raw_dump_table(__func__, "raw skb data dump uncompressed", | ||
371 | skb->data, skb->len); | ||
372 | |||
373 | /* another if the CID flag is set */ | ||
374 | if (iphc1 & LOWPAN_IPHC_CID) { | ||
375 | pr_debug("CID flag is set, increase header with one\n"); | ||
376 | if (lowpan_fetch_skb_u8(skb, &num_context)) | ||
377 | goto drop; | ||
378 | } | ||
379 | |||
380 | hdr.version = 6; | ||
381 | |||
382 | /* Traffic Class and Flow Label */ | ||
383 | switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) { | ||
384 | /* | ||
385 | * Traffic Class and FLow Label carried in-line | ||
386 | * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) | ||
387 | */ | ||
388 | case 0: /* 00b */ | ||
389 | if (lowpan_fetch_skb_u8(skb, &tmp)) | ||
390 | goto drop; | ||
391 | |||
392 | memcpy(&hdr.flow_lbl, &skb->data[0], 3); | ||
393 | skb_pull(skb, 3); | ||
394 | hdr.priority = ((tmp >> 2) & 0x0f); | ||
395 | hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) | | ||
396 | (hdr.flow_lbl[0] & 0x0f); | ||
397 | break; | ||
398 | /* | ||
399 | * Traffic class carried in-line | ||
400 | * ECN + DSCP (1 byte), Flow Label is elided | ||
401 | */ | ||
402 | case 2: /* 10b */ | ||
403 | if (lowpan_fetch_skb_u8(skb, &tmp)) | ||
404 | goto drop; | ||
405 | |||
406 | hdr.priority = ((tmp >> 2) & 0x0f); | ||
407 | hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30); | ||
408 | break; | ||
409 | /* | ||
410 | * Flow Label carried in-line | ||
411 | * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided | ||
412 | */ | ||
413 | case 1: /* 01b */ | ||
414 | if (lowpan_fetch_skb_u8(skb, &tmp)) | ||
415 | goto drop; | ||
416 | |||
417 | hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30); | ||
418 | memcpy(&hdr.flow_lbl[1], &skb->data[0], 2); | ||
419 | skb_pull(skb, 2); | ||
420 | break; | ||
421 | /* Traffic Class and Flow Label are elided */ | ||
422 | case 3: /* 11b */ | ||
423 | break; | ||
424 | default: | ||
425 | break; | ||
426 | } | ||
427 | |||
428 | /* Next Header */ | ||
429 | if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { | ||
430 | /* Next header is carried inline */ | ||
431 | if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr))) | ||
432 | goto drop; | ||
433 | |||
434 | pr_debug("NH flag is set, next header carried inline: %02x\n", | ||
435 | hdr.nexthdr); | ||
436 | } | ||
437 | |||
438 | /* Hop Limit */ | ||
439 | if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I) | ||
440 | hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03]; | ||
441 | else { | ||
442 | if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit))) | ||
443 | goto drop; | ||
444 | } | ||
445 | |||
446 | /* Extract SAM to the tmp variable */ | ||
447 | tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03; | ||
448 | |||
449 | if (iphc1 & LOWPAN_IPHC_SAC) { | ||
450 | /* Source address context based uncompression */ | ||
451 | pr_debug("SAC bit is set. Handle context based source address.\n"); | ||
452 | err = uncompress_context_based_src_addr( | ||
453 | skb, &hdr.saddr, tmp); | ||
454 | } else { | ||
455 | /* Source address uncompression */ | ||
456 | pr_debug("source address stateless compression\n"); | ||
457 | err = uncompress_addr(skb, &hdr.saddr, tmp, saddr, | ||
458 | saddr_type, saddr_len); | ||
459 | } | ||
460 | |||
461 | /* Check on error of previous branch */ | ||
462 | if (err) | ||
463 | goto drop; | ||
464 | |||
465 | /* Extract DAM to the tmp variable */ | ||
466 | tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03; | ||
467 | |||
468 | /* check for Multicast Compression */ | ||
469 | if (iphc1 & LOWPAN_IPHC_M) { | ||
470 | if (iphc1 & LOWPAN_IPHC_DAC) { | ||
471 | pr_debug("dest: context-based mcast compression\n"); | ||
472 | /* TODO: implement this */ | ||
473 | } else { | ||
474 | err = lowpan_uncompress_multicast_daddr( | ||
475 | skb, &hdr.daddr, tmp); | ||
476 | if (err) | ||
477 | goto drop; | ||
478 | } | ||
479 | } else { | ||
480 | err = uncompress_addr(skb, &hdr.daddr, tmp, daddr, | ||
481 | daddr_type, daddr_len); | ||
482 | pr_debug("dest: stateless compression mode %d dest %pI6c\n", | ||
483 | tmp, &hdr.daddr); | ||
484 | if (err) | ||
485 | goto drop; | ||
486 | } | ||
487 | |||
488 | /* UDP data uncompression */ | ||
489 | if (iphc0 & LOWPAN_IPHC_NH_C) { | ||
490 | struct udphdr uh; | ||
491 | struct sk_buff *new; | ||
492 | if (uncompress_udp_header(skb, &uh)) | ||
493 | goto drop; | ||
494 | |||
495 | /* | ||
496 | * replace the compressed UDP head by the uncompressed UDP | ||
497 | * header | ||
498 | */ | ||
499 | new = skb_copy_expand(skb, sizeof(struct udphdr), | ||
500 | skb_tailroom(skb), GFP_ATOMIC); | ||
501 | kfree_skb(skb); | ||
502 | |||
503 | if (!new) | ||
504 | return -ENOMEM; | ||
505 | |||
506 | skb = new; | ||
507 | |||
508 | skb_push(skb, sizeof(struct udphdr)); | ||
509 | skb_reset_transport_header(skb); | ||
510 | skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); | ||
511 | |||
512 | raw_dump_table(__func__, "raw UDP header dump", | ||
513 | (u8 *)&uh, sizeof(uh)); | ||
514 | |||
515 | hdr.nexthdr = UIP_PROTO_UDP; | ||
516 | } | ||
517 | |||
518 | hdr.payload_len = htons(skb->len); | ||
519 | |||
520 | pr_debug("skb headroom size = %d, data length = %d\n", | ||
521 | skb_headroom(skb), skb->len); | ||
522 | |||
523 | pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t" | ||
524 | "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", | ||
525 | hdr.version, ntohs(hdr.payload_len), hdr.nexthdr, | ||
526 | hdr.hop_limit, &hdr.daddr); | ||
527 | |||
528 | raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, | ||
529 | sizeof(hdr)); | ||
530 | |||
531 | return skb_deliver(skb, &hdr, dev, deliver_skb); | ||
532 | |||
533 | drop: | ||
534 | kfree_skb(skb); | ||
535 | return -EINVAL; | ||
536 | } | ||
537 | EXPORT_SYMBOL_GPL(lowpan_process_data); | ||
538 | |||
539 | static u8 lowpan_compress_addr_64(u8 **hc06_ptr, u8 shift, | ||
540 | const struct in6_addr *ipaddr, | ||
541 | const unsigned char *lladdr) | ||
542 | { | ||
543 | u8 val = 0; | ||
544 | |||
545 | if (is_addr_mac_addr_based(ipaddr, lladdr)) { | ||
546 | val = 3; /* 0-bits */ | ||
547 | pr_debug("address compression 0 bits\n"); | ||
548 | } else if (lowpan_is_iid_16_bit_compressable(ipaddr)) { | ||
549 | /* compress IID to 16 bits xxxx::XXXX */ | ||
550 | memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2); | ||
551 | *hc06_ptr += 2; | ||
552 | val = 2; /* 16-bits */ | ||
553 | raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)", | ||
554 | *hc06_ptr - 2, 2); | ||
555 | } else { | ||
556 | /* do not compress IID => xxxx::IID */ | ||
557 | memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8); | ||
558 | *hc06_ptr += 8; | ||
559 | val = 1; /* 64-bits */ | ||
560 | raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)", | ||
561 | *hc06_ptr - 8, 8); | ||
562 | } | ||
563 | |||
564 | return rol8(val, shift); | ||
565 | } | ||
566 | |||
567 | static void compress_udp_header(u8 **hc06_ptr, struct sk_buff *skb) | ||
568 | { | ||
569 | struct udphdr *uh = udp_hdr(skb); | ||
570 | |||
571 | if (((uh->source & LOWPAN_NHC_UDP_4BIT_MASK) == | ||
572 | LOWPAN_NHC_UDP_4BIT_PORT) && | ||
573 | ((uh->dest & LOWPAN_NHC_UDP_4BIT_MASK) == | ||
574 | LOWPAN_NHC_UDP_4BIT_PORT)) { | ||
575 | pr_debug("UDP header: both ports compression to 4 bits\n"); | ||
576 | **hc06_ptr = LOWPAN_NHC_UDP_CS_P_11; | ||
577 | **(hc06_ptr + 1) = /* subtraction is faster */ | ||
578 | (u8)((uh->dest - LOWPAN_NHC_UDP_4BIT_PORT) + | ||
579 | ((uh->source & LOWPAN_NHC_UDP_4BIT_PORT) << 4)); | ||
580 | *hc06_ptr += 2; | ||
581 | } else if ((uh->dest & LOWPAN_NHC_UDP_8BIT_MASK) == | ||
582 | LOWPAN_NHC_UDP_8BIT_PORT) { | ||
583 | pr_debug("UDP header: remove 8 bits of dest\n"); | ||
584 | **hc06_ptr = LOWPAN_NHC_UDP_CS_P_01; | ||
585 | memcpy(*hc06_ptr + 1, &uh->source, 2); | ||
586 | **(hc06_ptr + 3) = (u8)(uh->dest - LOWPAN_NHC_UDP_8BIT_PORT); | ||
587 | *hc06_ptr += 4; | ||
588 | } else if ((uh->source & LOWPAN_NHC_UDP_8BIT_MASK) == | ||
589 | LOWPAN_NHC_UDP_8BIT_PORT) { | ||
590 | pr_debug("UDP header: remove 8 bits of source\n"); | ||
591 | **hc06_ptr = LOWPAN_NHC_UDP_CS_P_10; | ||
592 | memcpy(*hc06_ptr + 1, &uh->dest, 2); | ||
593 | **(hc06_ptr + 3) = (u8)(uh->source - LOWPAN_NHC_UDP_8BIT_PORT); | ||
594 | *hc06_ptr += 4; | ||
595 | } else { | ||
596 | pr_debug("UDP header: can't compress\n"); | ||
597 | **hc06_ptr = LOWPAN_NHC_UDP_CS_P_00; | ||
598 | memcpy(*hc06_ptr + 1, &uh->source, 2); | ||
599 | memcpy(*hc06_ptr + 3, &uh->dest, 2); | ||
600 | *hc06_ptr += 5; | ||
601 | } | ||
602 | |||
603 | /* checksum is always inline */ | ||
604 | memcpy(*hc06_ptr, &uh->check, 2); | ||
605 | *hc06_ptr += 2; | ||
606 | |||
607 | /* skip the UDP header */ | ||
608 | skb_pull(skb, sizeof(struct udphdr)); | ||
609 | } | ||
610 | |||
611 | int lowpan_header_compress(struct sk_buff *skb, struct net_device *dev, | ||
612 | unsigned short type, const void *_daddr, | ||
613 | const void *_saddr, unsigned int len) | ||
614 | { | ||
615 | u8 tmp, iphc0, iphc1, *hc06_ptr; | ||
616 | struct ipv6hdr *hdr; | ||
617 | u8 head[100] = {}; | ||
618 | |||
619 | if (type != ETH_P_IPV6) | ||
620 | return -EINVAL; | ||
621 | |||
622 | hdr = ipv6_hdr(skb); | ||
623 | hc06_ptr = head + 2; | ||
624 | |||
625 | pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" | ||
626 | "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", | ||
627 | hdr->version, ntohs(hdr->payload_len), hdr->nexthdr, | ||
628 | hdr->hop_limit, &hdr->daddr); | ||
629 | |||
630 | raw_dump_table(__func__, "raw skb network header dump", | ||
631 | skb_network_header(skb), sizeof(struct ipv6hdr)); | ||
632 | |||
633 | /* | ||
634 | * As we copy some bit-length fields, in the IPHC encoding bytes, | ||
635 | * we sometimes use |= | ||
636 | * If the field is 0, and the current bit value in memory is 1, | ||
637 | * this does not work. We therefore reset the IPHC encoding here | ||
638 | */ | ||
639 | iphc0 = LOWPAN_DISPATCH_IPHC; | ||
640 | iphc1 = 0; | ||
641 | |||
642 | /* TODO: context lookup */ | ||
643 | |||
644 | raw_dump_inline(__func__, "saddr", | ||
645 | (unsigned char *)_saddr, IEEE802154_ADDR_LEN); | ||
646 | raw_dump_inline(__func__, "daddr", | ||
647 | (unsigned char *)_daddr, IEEE802154_ADDR_LEN); | ||
648 | |||
649 | raw_dump_table(__func__, | ||
650 | "sending raw skb network uncompressed packet", | ||
651 | skb->data, skb->len); | ||
652 | |||
653 | /* | ||
654 | * Traffic class, flow label | ||
655 | * If flow label is 0, compress it. If traffic class is 0, compress it | ||
656 | * We have to process both in the same time as the offset of traffic | ||
657 | * class depends on the presence of version and flow label | ||
658 | */ | ||
659 | |||
660 | /* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */ | ||
661 | tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4); | ||
662 | tmp = ((tmp & 0x03) << 6) | (tmp >> 2); | ||
663 | |||
664 | if (((hdr->flow_lbl[0] & 0x0F) == 0) && | ||
665 | (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) { | ||
666 | /* flow label can be compressed */ | ||
667 | iphc0 |= LOWPAN_IPHC_FL_C; | ||
668 | if ((hdr->priority == 0) && | ||
669 | ((hdr->flow_lbl[0] & 0xF0) == 0)) { | ||
670 | /* compress (elide) all */ | ||
671 | iphc0 |= LOWPAN_IPHC_TC_C; | ||
672 | } else { | ||
673 | /* compress only the flow label */ | ||
674 | *hc06_ptr = tmp; | ||
675 | hc06_ptr += 1; | ||
676 | } | ||
677 | } else { | ||
678 | /* Flow label cannot be compressed */ | ||
679 | if ((hdr->priority == 0) && | ||
680 | ((hdr->flow_lbl[0] & 0xF0) == 0)) { | ||
681 | /* compress only traffic class */ | ||
682 | iphc0 |= LOWPAN_IPHC_TC_C; | ||
683 | *hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F); | ||
684 | memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2); | ||
685 | hc06_ptr += 3; | ||
686 | } else { | ||
687 | /* compress nothing */ | ||
688 | memcpy(hc06_ptr, &hdr, 4); | ||
689 | /* replace the top byte with new ECN | DSCP format */ | ||
690 | *hc06_ptr = tmp; | ||
691 | hc06_ptr += 4; | ||
692 | } | ||
693 | } | ||
694 | |||
695 | /* NOTE: payload length is always compressed */ | ||
696 | |||
697 | /* Next Header is compress if UDP */ | ||
698 | if (hdr->nexthdr == UIP_PROTO_UDP) | ||
699 | iphc0 |= LOWPAN_IPHC_NH_C; | ||
700 | |||
701 | if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { | ||
702 | *hc06_ptr = hdr->nexthdr; | ||
703 | hc06_ptr += 1; | ||
704 | } | ||
705 | |||
706 | /* | ||
707 | * Hop limit | ||
708 | * if 1: compress, encoding is 01 | ||
709 | * if 64: compress, encoding is 10 | ||
710 | * if 255: compress, encoding is 11 | ||
711 | * else do not compress | ||
712 | */ | ||
713 | switch (hdr->hop_limit) { | ||
714 | case 1: | ||
715 | iphc0 |= LOWPAN_IPHC_TTL_1; | ||
716 | break; | ||
717 | case 64: | ||
718 | iphc0 |= LOWPAN_IPHC_TTL_64; | ||
719 | break; | ||
720 | case 255: | ||
721 | iphc0 |= LOWPAN_IPHC_TTL_255; | ||
722 | break; | ||
723 | default: | ||
724 | *hc06_ptr = hdr->hop_limit; | ||
725 | hc06_ptr += 1; | ||
726 | break; | ||
727 | } | ||
728 | |||
729 | /* source address compression */ | ||
730 | if (is_addr_unspecified(&hdr->saddr)) { | ||
731 | pr_debug("source address is unspecified, setting SAC\n"); | ||
732 | iphc1 |= LOWPAN_IPHC_SAC; | ||
733 | /* TODO: context lookup */ | ||
734 | } else if (is_addr_link_local(&hdr->saddr)) { | ||
735 | iphc1 |= lowpan_compress_addr_64(&hc06_ptr, | ||
736 | LOWPAN_IPHC_SAM_BIT, &hdr->saddr, _saddr); | ||
737 | pr_debug("source address unicast link-local %pI6c " | ||
738 | "iphc1 0x%02x\n", &hdr->saddr, iphc1); | ||
739 | } else { | ||
740 | pr_debug("send the full source address\n"); | ||
741 | memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16); | ||
742 | hc06_ptr += 16; | ||
743 | } | ||
744 | |||
745 | /* destination address compression */ | ||
746 | if (is_addr_mcast(&hdr->daddr)) { | ||
747 | pr_debug("destination address is multicast: "); | ||
748 | iphc1 |= LOWPAN_IPHC_M; | ||
749 | if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) { | ||
750 | pr_debug("compressed to 1 octet\n"); | ||
751 | iphc1 |= LOWPAN_IPHC_DAM_11; | ||
752 | /* use last byte */ | ||
753 | *hc06_ptr = hdr->daddr.s6_addr[15]; | ||
754 | hc06_ptr += 1; | ||
755 | } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) { | ||
756 | pr_debug("compressed to 4 octets\n"); | ||
757 | iphc1 |= LOWPAN_IPHC_DAM_10; | ||
758 | /* second byte + the last three */ | ||
759 | *hc06_ptr = hdr->daddr.s6_addr[1]; | ||
760 | memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3); | ||
761 | hc06_ptr += 4; | ||
762 | } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) { | ||
763 | pr_debug("compressed to 6 octets\n"); | ||
764 | iphc1 |= LOWPAN_IPHC_DAM_01; | ||
765 | /* second byte + the last five */ | ||
766 | *hc06_ptr = hdr->daddr.s6_addr[1]; | ||
767 | memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5); | ||
768 | hc06_ptr += 6; | ||
769 | } else { | ||
770 | pr_debug("using full address\n"); | ||
771 | iphc1 |= LOWPAN_IPHC_DAM_00; | ||
772 | memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16); | ||
773 | hc06_ptr += 16; | ||
774 | } | ||
775 | } else { | ||
776 | /* TODO: context lookup */ | ||
777 | if (is_addr_link_local(&hdr->daddr)) { | ||
778 | iphc1 |= lowpan_compress_addr_64(&hc06_ptr, | ||
779 | LOWPAN_IPHC_DAM_BIT, &hdr->daddr, _daddr); | ||
780 | pr_debug("dest address unicast link-local %pI6c " | ||
781 | "iphc1 0x%02x\n", &hdr->daddr, iphc1); | ||
782 | } else { | ||
783 | pr_debug("dest address unicast %pI6c\n", &hdr->daddr); | ||
784 | memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16); | ||
785 | hc06_ptr += 16; | ||
786 | } | ||
787 | } | ||
788 | |||
789 | /* UDP header compression */ | ||
790 | if (hdr->nexthdr == UIP_PROTO_UDP) | ||
791 | compress_udp_header(&hc06_ptr, skb); | ||
792 | |||
793 | head[0] = iphc0; | ||
794 | head[1] = iphc1; | ||
795 | |||
796 | skb_pull(skb, sizeof(struct ipv6hdr)); | ||
797 | skb_reset_transport_header(skb); | ||
798 | memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head); | ||
799 | skb_reset_network_header(skb); | ||
800 | |||
801 | pr_debug("header len %d skb %u\n", (int)(hc06_ptr - head), skb->len); | ||
802 | |||
803 | raw_dump_table(__func__, "raw skb data dump compressed", | ||
804 | skb->data, skb->len); | ||
805 | return 0; | ||
806 | } | ||
807 | EXPORT_SYMBOL_GPL(lowpan_header_compress); | ||
diff --git a/net/ieee802154/Makefile b/net/ieee802154/Makefile index d7716d64c6bb..951a83ee8af4 100644 --- a/net/ieee802154/Makefile +++ b/net/ieee802154/Makefile | |||
@@ -1,5 +1,5 @@ | |||
1 | obj-$(CONFIG_IEEE802154) += ieee802154.o af_802154.o | 1 | obj-$(CONFIG_IEEE802154) += ieee802154.o af_802154.o |
2 | obj-$(CONFIG_IEEE802154_6LOWPAN) += 6lowpan.o | 2 | obj-$(CONFIG_IEEE802154_6LOWPAN) += 6lowpan.o 6lowpan_iphc.o |
3 | 3 | ||
4 | ieee802154-y := netlink.o nl-mac.o nl-phy.o nl_policy.o wpan-class.o | 4 | ieee802154-y := netlink.o nl-mac.o nl-phy.o nl_policy.o wpan-class.o |
5 | af_802154-y := af_ieee802154.o raw.o dgram.o | 5 | af_802154-y := af_ieee802154.o raw.o dgram.o |