aboutsummaryrefslogtreecommitdiffstats
path: root/net/ieee802154/6lowpan.c
diff options
context:
space:
mode:
Diffstat (limited to 'net/ieee802154/6lowpan.c')
-rw-r--r--net/ieee802154/6lowpan.c796
1 files changed, 40 insertions, 756 deletions
diff --git a/net/ieee802154/6lowpan.c b/net/ieee802154/6lowpan.c
index a2d2456a557a..48b25c0af4d0 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 */
66static const u8 lowpan_ttl_values[] = {0, 1, 64, 255};
67
68static LIST_HEAD(lowpan_devices); 65static LIST_HEAD(lowpan_devices);
69 66
70/* private device info */ 67/* private device info */
@@ -104,378 +101,14 @@ static inline void lowpan_address_flip(u8 *src, u8 *dest)
104 (dest)[IEEE802154_ADDR_LEN - i - 1] = (src)[i]; 101 (dest)[IEEE802154_ADDR_LEN - i - 1] = (src)[i];
105} 102}
106 103
107/* list of all 6lowpan devices, uses for package delivering */
108/* print data in line */
109static inline void lowpan_raw_dump_inline(const char *caller, char *msg,
110 unsigned char *buf, int len)
111{
112#ifdef DEBUG
113 if (msg)
114 pr_debug("(%s) %s: ", caller, msg);
115 print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_NONE,
116 16, 1, buf, len, false);
117#endif /* DEBUG */
118}
119
120/*
121 * print data in a table format:
122 *
123 * addr: xx xx xx xx xx xx
124 * addr: xx xx xx xx xx xx
125 * ...
126 */
127static inline void lowpan_raw_dump_table(const char *caller, char *msg,
128 unsigned char *buf, int len)
129{
130#ifdef DEBUG
131 if (msg)
132 pr_debug("(%s) %s:\n", caller, msg);
133 print_hex_dump(KERN_DEBUG, "\t", DUMP_PREFIX_OFFSET,
134 16, 1, buf, len, false);
135#endif /* DEBUG */
136}
137
138static u8
139lowpan_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 */
167static int
168lowpan_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 */
248static int
249lowpan_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 */
282static int
283lowpan_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
336static void
337lowpan_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
381static 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
392static 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
403static int
404lowpan_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;
465err:
466 return -EINVAL;
467}
468
469static int lowpan_header_create(struct sk_buff *skb, 104static int lowpan_header_create(struct sk_buff *skb,
470 struct net_device *dev, 105 struct net_device *dev,
471 unsigned short type, const void *_daddr, 106 unsigned short type, const void *_daddr,
472 const void *_saddr, unsigned int len) 107 const void *_saddr, unsigned int len)
473{ 108{
474 u8 tmp, iphc0, iphc1, *hc06_ptr;
475 struct ipv6hdr *hdr; 109 struct ipv6hdr *hdr;
476 const u8 *saddr = _saddr; 110 const u8 *saddr = _saddr;
477 const u8 *daddr = _daddr; 111 const u8 *daddr = _daddr;
478 u8 head[100];
479 struct ieee802154_addr sa, da; 112 struct ieee802154_addr sa, da;
480 113
481 /* TODO: 114 /* TODO:
@@ -485,181 +118,14 @@ static int lowpan_header_create(struct sk_buff *skb,
485 return 0; 118 return 0;
486 119
487 hdr = ipv6_hdr(skb); 120 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 121
497 if (!saddr) 122 if (!saddr)
498 saddr = dev->dev_addr; 123 saddr = dev->dev_addr;
499 124
500 lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8); 125 raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
501 126 raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
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 127
513 lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8); 128 lowpan_header_compress(skb, dev, type, daddr, saddr, len);
514
515 /*
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 129
664 /* 130 /*
665 * NOTE1: I'm still unsure about the fact that compression and WPAN 131 * NOTE1: I'm still unsure about the fact that compression and WPAN
@@ -671,39 +137,38 @@ static int lowpan_header_create(struct sk_buff *skb,
671 * from MAC subif of the 'dev' and 'real_dev' network devices, but 137 * 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 138 * this isn't implemented in mainline yet, so currently we assign 0xff
673 */ 139 */
674 { 140 mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA;
675 mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA; 141 mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
676 mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
677 142
678 /* prepare wpan address data */ 143 /* prepare wpan address data */
679 sa.addr_type = IEEE802154_ADDR_LONG; 144 sa.addr_type = IEEE802154_ADDR_LONG;
680 sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev); 145 sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
681 146
682 memcpy(&(sa.hwaddr), saddr, 8); 147 memcpy(&(sa.hwaddr), saddr, 8);
683 /* intra-PAN communications */ 148 /* intra-PAN communications */
684 da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev); 149 da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
685 150
686 /* 151 /*
687 * if the destination address is the broadcast address, use the 152 * if the destination address is the broadcast address, use the
688 * corresponding short address 153 * corresponding short address
689 */ 154 */
690 if (lowpan_is_addr_broadcast(daddr)) { 155 if (lowpan_is_addr_broadcast(daddr)) {
691 da.addr_type = IEEE802154_ADDR_SHORT; 156 da.addr_type = IEEE802154_ADDR_SHORT;
692 da.short_addr = IEEE802154_ADDR_BROADCAST; 157 da.short_addr = IEEE802154_ADDR_BROADCAST;
693 } else { 158 } else {
694 da.addr_type = IEEE802154_ADDR_LONG; 159 da.addr_type = IEEE802154_ADDR_LONG;
695 memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN); 160 memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN);
696
697 /* request acknowledgment */
698 mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
699 }
700 161
701 return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev, 162 /* request acknowledgment */
702 type, (void *)&da, (void *)&sa, skb->len); 163 mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
703 } 164 }
165
166 return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
167 type, (void *)&da, (void *)&sa, skb->len);
704} 168}
705 169
706static int lowpan_give_skb_to_devices(struct sk_buff *skb) 170static int lowpan_give_skb_to_devices(struct sk_buff *skb,
171 struct net_device *dev)
707{ 172{
708 struct lowpan_dev_record *entry; 173 struct lowpan_dev_record *entry;
709 struct sk_buff *skb_cp; 174 struct sk_buff *skb_cp;
@@ -726,31 +191,6 @@ static int lowpan_give_skb_to_devices(struct sk_buff *skb)
726 return stat; 191 return stat;
727} 192}
728 193
729static 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
754static void lowpan_fragment_timer_expired(unsigned long entry_addr) 194static void lowpan_fragment_timer_expired(unsigned long entry_addr)
755{ 195{
756 struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr; 196 struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr;
@@ -814,16 +254,12 @@ frame_err:
814 return NULL; 254 return NULL;
815} 255}
816 256
817static int 257static int process_data(struct sk_buff *skb)
818lowpan_process_data(struct sk_buff *skb)
819{ 258{
820 struct ipv6hdr hdr = {}; 259 u8 iphc0, iphc1;
821 u8 tmp, iphc0, iphc1, num_context = 0;
822 const struct ieee802154_addr *_saddr, *_daddr; 260 const struct ieee802154_addr *_saddr, *_daddr;
823 int err;
824 261
825 lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data, 262 raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
826 skb->len);
827 /* at least two bytes will be used for the encoding */ 263 /* at least two bytes will be used for the encoding */
828 if (skb->len < 2) 264 if (skb->len < 2)
829 goto drop; 265 goto drop;
@@ -925,162 +361,11 @@ lowpan_process_data(struct sk_buff *skb)
925 _saddr = &mac_cb(skb)->sa; 361 _saddr = &mac_cb(skb)->sa;
926 _daddr = &mac_cb(skb)->da; 362 _daddr = &mac_cb(skb)->da;
927 363
928 pr_debug("iphc0 = %02x, iphc1 = %02x\n", iphc0, iphc1); 364 return lowpan_process_data(skb, skb->dev, (u8 *)_saddr->hwaddr,
929 365 _saddr->addr_type, IEEE802154_ADDR_LEN,
930 /* another if the CID flag is set */ 366 (u8 *)_daddr->hwaddr, _daddr->addr_type,
931 if (iphc1 & LOWPAN_IPHC_CID) { 367 IEEE802154_ADDR_LEN, iphc0, iphc1,
932 pr_debug("CID flag is set, increase header with one\n"); 368 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 369
1085unlock_and_drop: 370unlock_and_drop:
1086 spin_unlock_bh(&flist_lock); 371 spin_unlock_bh(&flist_lock);
@@ -1112,7 +397,7 @@ lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
1112 hlen = (type == LOWPAN_DISPATCH_FRAG1) ? 397 hlen = (type == LOWPAN_DISPATCH_FRAG1) ?
1113 LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE; 398 LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE;
1114 399
1115 lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen); 400 raw_dump_inline(__func__, "6lowpan fragment header", head, hlen);
1116 401
1117 frag = netdev_alloc_skb(skb->dev, 402 frag = netdev_alloc_skb(skb->dev,
1118 hlen + mlen + plen + IEEE802154_MFR_SIZE); 403 hlen + mlen + plen + IEEE802154_MFR_SIZE);
@@ -1132,8 +417,7 @@ lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
1132 skb_copy_to_linear_data_offset(frag, mlen + hlen, 417 skb_copy_to_linear_data_offset(frag, mlen + hlen,
1133 skb_network_header(skb) + offset, plen); 418 skb_network_header(skb) + offset, plen);
1134 419
1135 lowpan_raw_dump_table(__func__, " raw fragment dump", frag->data, 420 raw_dump_table(__func__, " raw fragment dump", frag->data, frag->len);
1136 frag->len);
1137 421
1138 return dev_queue_xmit(frag); 422 return dev_queue_xmit(frag);
1139} 423}
@@ -1316,7 +600,7 @@ static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
1316 /* Pull off the 1-byte of 6lowpan header. */ 600 /* Pull off the 1-byte of 6lowpan header. */
1317 skb_pull(local_skb, 1); 601 skb_pull(local_skb, 1);
1318 602
1319 lowpan_give_skb_to_devices(local_skb); 603 lowpan_give_skb_to_devices(local_skb, NULL);
1320 604
1321 kfree_skb(local_skb); 605 kfree_skb(local_skb);
1322 kfree_skb(skb); 606 kfree_skb(skb);
@@ -1328,7 +612,7 @@ static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
1328 local_skb = skb_clone(skb, GFP_ATOMIC); 612 local_skb = skb_clone(skb, GFP_ATOMIC);
1329 if (!local_skb) 613 if (!local_skb)
1330 goto drop; 614 goto drop;
1331 lowpan_process_data(local_skb); 615 process_data(local_skb);
1332 616
1333 kfree_skb(skb); 617 kfree_skb(skb);
1334 break; 618 break;
generated by cgit v1.2.2 (git 2.25.0) at 2024-10-13 12:24:22 -0400