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/module.h> 56#include <linux/moduleparam.h> 57#include <linux/netdevice.h> 58#include <net/af_ieee802154.h> 59#include <net/ieee802154.h> 60#include <net/ieee802154_netdev.h> 61#include <net/ipv6.h> 62 63#include "6lowpan.h" 64 65/* TTL uncompression values */ 66static const u8 lowpan_ttl_values[] = {0, 1, 64, 255}; 67 68static LIST_HEAD(lowpan_devices); 69 70/* 71 * Uncompression of linklocal: 72 * 0 -> 16 bytes from packet 73 * 1 -> 2 bytes from prefix - bunch of zeroes and 8 from packet 74 * 2 -> 2 bytes from prefix - zeroes + 2 from packet 75 * 3 -> 2 bytes from prefix - infer 8 bytes from lladdr 76 * 77 * NOTE: => the uncompress function does change 0xf to 0x10 78 * NOTE: 0x00 => no-autoconfig => unspecified 79 */ 80static const u8 lowpan_unc_llconf[] = {0x0f, 0x28, 0x22, 0x20}; 81 82/* 83 * Uncompression of ctx-based: 84 * 0 -> 0 bits from packet [unspecified / reserved] 85 * 1 -> 8 bytes from prefix - bunch of zeroes and 8 from packet 86 * 2 -> 8 bytes from prefix - zeroes + 2 from packet 87 * 3 -> 8 bytes from prefix - infer 8 bytes from lladdr 88 */ 89static const u8 lowpan_unc_ctxconf[] = {0x00, 0x88, 0x82, 0x80}; 90 91/* 92 * Uncompression of ctx-base 93 * 0 -> 0 bits from packet 94 * 1 -> 2 bytes from prefix - bunch of zeroes 5 from packet 95 * 2 -> 2 bytes from prefix - zeroes + 3 from packet 96 * 3 -> 2 bytes from prefix - infer 1 bytes from lladdr 97 */ 98static const u8 lowpan_unc_mxconf[] = {0x0f, 0x25, 0x23, 0x21}; 99 100/* Link local prefix */ 101static const u8 lowpan_llprefix[] = {0xfe, 0x80}; 102 103/* private device info */ 104struct lowpan_dev_info { 105 struct net_device *real_dev; /* real WPAN device ptr */ 106 struct mutex dev_list_mtx; /* mutex for list ops */ 107 unsigned short fragment_tag; 108}; 109 110struct lowpan_dev_record { 111 struct net_device *ldev; 112 struct list_head list; 113}; 114 115struct lowpan_fragment { 116 struct sk_buff *skb; /* skb to be assembled */ 117 u16 length; /* length to be assemled */ 118 u32 bytes_rcv; /* bytes received */ 119 u16 tag; /* current fragment tag */ 120 struct timer_list timer; /* assembling timer */ 121 struct list_head list; /* fragments list */ 122}; 123 124static LIST_HEAD(lowpan_fragments); 125static DEFINE_SPINLOCK(flist_lock); 126 127static inline struct 128lowpan_dev_info *lowpan_dev_info(const struct net_device *dev) 129{ 130 return netdev_priv(dev); 131} 132 133static inline void lowpan_address_flip(u8 *src, u8 *dest) 134{ 135 int i; 136 for (i = 0; i < IEEE802154_ADDR_LEN; i++) 137 (dest)[IEEE802154_ADDR_LEN - i - 1] = (src)[i]; 138} 139 140/* list of all 6lowpan devices, uses for package delivering */ 141/* print data in line */ 142static inline void lowpan_raw_dump_inline(const char *caller, char *msg, 143 unsigned char *buf, int len) 144{ 145#ifdef DEBUG 146 if (msg) 147 pr_debug("(%s) %s: ", caller, msg); 148 print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_NONE, 149 16, 1, buf, len, false); 150#endif /* DEBUG */ 151} 152 153/* 154 * print data in a table format: 155 * 156 * addr: xx xx xx xx xx xx 157 * addr: xx xx xx xx xx xx 158 * ... 159 */ 160static inline void lowpan_raw_dump_table(const char *caller, char *msg, 161 unsigned char *buf, int len) 162{ 163#ifdef DEBUG 164 if (msg) 165 pr_debug("(%s) %s:\n", caller, msg); 166 print_hex_dump(KERN_DEBUG, "\t", DUMP_PREFIX_OFFSET, 167 16, 1, buf, len, false); 168#endif /* DEBUG */ 169} 170 171static u8 172lowpan_compress_addr_64(u8 **hc06_ptr, u8 shift, const struct in6_addr *ipaddr, 173 const unsigned char *lladdr) 174{ 175 u8 val = 0; 176 177 if (is_addr_mac_addr_based(ipaddr, lladdr)) 178 val = 3; /* 0-bits */ 179 else if (lowpan_is_iid_16_bit_compressable(ipaddr)) { 180 /* compress IID to 16 bits xxxx::XXXX */ 181 memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2); 182 *hc06_ptr += 2; 183 val = 2; /* 16-bits */ 184 } else { 185 /* do not compress IID => xxxx::IID */ 186 memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8); 187 *hc06_ptr += 8; 188 val = 1; /* 64-bits */ 189 } 190 191 return rol8(val, shift); 192} 193 194static void 195lowpan_uip_ds6_set_addr_iid(struct in6_addr *ipaddr, unsigned char *lladdr) 196{ 197 memcpy(&ipaddr->s6_addr[8], lladdr, IEEE802154_ADDR_LEN); 198 /* second bit-flip (Universe/Local) is done according RFC2464 */ 199 ipaddr->s6_addr[8] ^= 0x02; 200} 201 202/* 203 * Uncompress addresses based on a prefix and a postfix with zeroes in 204 * between. If the postfix is zero in length it will use the link address 205 * to configure the IP address (autoconf style). 206 * pref_post_count takes a byte where the first nibble specify prefix count 207 * and the second postfix count (NOTE: 15/0xf => 16 bytes copy). 208 */ 209static int 210lowpan_uncompress_addr(struct sk_buff *skb, struct in6_addr *ipaddr, 211 u8 const *prefix, u8 pref_post_count, unsigned char *lladdr) 212{ 213 u8 prefcount = pref_post_count >> 4; 214 u8 postcount = pref_post_count & 0x0f; 215 216 /* full nibble 15 => 16 */ 217 prefcount = (prefcount == 15 ? 16 : prefcount); 218 postcount = (postcount == 15 ? 16 : postcount); 219 220 if (lladdr) 221 lowpan_raw_dump_inline(__func__, "linklocal address", 222 lladdr, IEEE802154_ADDR_LEN); 223 if (prefcount > 0) 224 memcpy(ipaddr, prefix, prefcount); 225 226 if (prefcount + postcount < 16) 227 memset(&ipaddr->s6_addr[prefcount], 0, 228 16 - (prefcount + postcount)); 229 230 if (postcount > 0) { 231 memcpy(&ipaddr->s6_addr[16 - postcount], skb->data, postcount); 232 skb_pull(skb, postcount); 233 } else if (prefcount > 0) { 234 if (lladdr == NULL) 235 return -EINVAL; 236 237 /* no IID based configuration if no prefix and no data */ 238 lowpan_uip_ds6_set_addr_iid(ipaddr, lladdr); 239 } 240 241 pr_debug("uncompressing %d + %d => ", prefcount, postcount); 242 lowpan_raw_dump_inline(NULL, NULL, ipaddr->s6_addr, 16); 243 244 return 0; 245} 246 247static void 248lowpan_compress_udp_header(u8 **hc06_ptr, struct sk_buff *skb) 249{ 250 struct udphdr *uh = udp_hdr(skb); 251 252 if (((uh->source & LOWPAN_NHC_UDP_4BIT_MASK) == 253 LOWPAN_NHC_UDP_4BIT_PORT) && 254 ((uh->dest & LOWPAN_NHC_UDP_4BIT_MASK) == 255 LOWPAN_NHC_UDP_4BIT_PORT)) { 256 pr_debug("UDP header: both ports compression to 4 bits\n"); 257 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_11; 258 **(hc06_ptr + 1) = /* subtraction is faster */ 259 (u8)((uh->dest - LOWPAN_NHC_UDP_4BIT_PORT) + 260 ((uh->source & LOWPAN_NHC_UDP_4BIT_PORT) << 4)); 261 *hc06_ptr += 2; 262 } else if ((uh->dest & LOWPAN_NHC_UDP_8BIT_MASK) == 263 LOWPAN_NHC_UDP_8BIT_PORT) { 264 pr_debug("UDP header: remove 8 bits of dest\n"); 265 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_01; 266 memcpy(*hc06_ptr + 1, &uh->source, 2); 267 **(hc06_ptr + 3) = (u8)(uh->dest - LOWPAN_NHC_UDP_8BIT_PORT); 268 *hc06_ptr += 4; 269 } else if ((uh->source & LOWPAN_NHC_UDP_8BIT_MASK) == 270 LOWPAN_NHC_UDP_8BIT_PORT) { 271 pr_debug("UDP header: remove 8 bits of source\n"); 272 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_10; 273 memcpy(*hc06_ptr + 1, &uh->dest, 2); 274 **(hc06_ptr + 3) = (u8)(uh->source - LOWPAN_NHC_UDP_8BIT_PORT); 275 *hc06_ptr += 4; 276 } else { 277 pr_debug("UDP header: can't compress\n"); 278 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_00; 279 memcpy(*hc06_ptr + 1, &uh->source, 2); 280 memcpy(*hc06_ptr + 3, &uh->dest, 2); 281 *hc06_ptr += 5; 282 } 283 284 /* checksum is always inline */ 285 memcpy(*hc06_ptr, &uh->check, 2); 286 *hc06_ptr += 2; 287 288 /* skip the UDP header */ 289 skb_pull(skb, sizeof(struct udphdr)); 290} 291 292static inline int lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val) 293{ 294 if (unlikely(!pskb_may_pull(skb, 1))) 295 return -EINVAL; 296 297 *val = skb->data[0]; 298 skb_pull(skb, 1); 299 300 return 0; 301} 302 303static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val) 304{ 305 if (unlikely(!pskb_may_pull(skb, 2))) 306 return -EINVAL; 307 308 *val = (skb->data[0] << 8) | skb->data[1]; 309 skb_pull(skb, 2); 310 311 return 0; 312} 313 314static int 315lowpan_uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh) 316{ 317 u8 tmp; 318 319 if (!uh) 320 goto err; 321 322 if (lowpan_fetch_skb_u8(skb, &tmp)) 323 goto err; 324 325 if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) { 326 pr_debug("UDP header uncompression\n"); 327 switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { 328 case LOWPAN_NHC_UDP_CS_P_00: 329 memcpy(&uh->source, &skb->data[0], 2); 330 memcpy(&uh->dest, &skb->data[2], 2); 331 skb_pull(skb, 4); 332 break; 333 case LOWPAN_NHC_UDP_CS_P_01: 334 memcpy(&uh->source, &skb->data[0], 2); 335 uh->dest = 336 skb->data[2] + LOWPAN_NHC_UDP_8BIT_PORT; 337 skb_pull(skb, 3); 338 break; 339 case LOWPAN_NHC_UDP_CS_P_10: 340 uh->source = skb->data[0] + LOWPAN_NHC_UDP_8BIT_PORT; 341 memcpy(&uh->dest, &skb->data[1], 2); 342 skb_pull(skb, 3); 343 break; 344 case LOWPAN_NHC_UDP_CS_P_11: 345 uh->source = 346 LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] >> 4); 347 uh->dest = 348 LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] & 0x0f); 349 skb_pull(skb, 1); 350 break; 351 default: 352 pr_debug("ERROR: unknown UDP format\n"); 353 goto err; 354 break; 355 } 356 357 pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", 358 uh->source, uh->dest); 359 360 /* copy checksum */ 361 memcpy(&uh->check, &skb->data[0], 2); 362 skb_pull(skb, 2); 363 364 /* 365 * UDP lenght needs to be infered from the lower layers 366 * here, we obtain the hint from the remaining size of the 367 * frame 368 */ 369 uh->len = htons(skb->len + sizeof(struct udphdr)); 370 pr_debug("uncompressed UDP length: src = %d", uh->len); 371 } else { 372 pr_debug("ERROR: unsupported NH format\n"); 373 goto err; 374 } 375 376 return 0; 377err: 378 return -EINVAL; 379} 380 381static int lowpan_header_create(struct sk_buff *skb, 382 struct net_device *dev, 383 unsigned short type, const void *_daddr, 384 const void *_saddr, unsigned int len) 385{ 386 u8 tmp, iphc0, iphc1, *hc06_ptr; 387 struct ipv6hdr *hdr; 388 const u8 *saddr = _saddr; 389 const u8 *daddr = _daddr; 390 u8 head[100]; 391 struct ieee802154_addr sa, da; 392 393 /* TODO: 394 * if this package isn't ipv6 one, where should it be routed? 395 */ 396 if (type != ETH_P_IPV6) 397 return 0; 398 399 hdr = ipv6_hdr(skb); 400 hc06_ptr = head + 2; 401 402 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" 403 "\tnexthdr = 0x%02x\n\thop_lim = %d\n", hdr->version, 404 ntohs(hdr->payload_len), hdr->nexthdr, hdr->hop_limit); 405 406 lowpan_raw_dump_table(__func__, "raw skb network header dump", 407 skb_network_header(skb), sizeof(struct ipv6hdr)); 408 409 if (!saddr) 410 saddr = dev->dev_addr; 411 412 lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8); 413 414 /* 415 * As we copy some bit-length fields, in the IPHC encoding bytes, 416 * we sometimes use |= 417 * If the field is 0, and the current bit value in memory is 1, 418 * this does not work. We therefore reset the IPHC encoding here 419 */ 420 iphc0 = LOWPAN_DISPATCH_IPHC; 421 iphc1 = 0; 422 423 /* TODO: context lookup */ 424 425 lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8); 426 427 /* 428 * Traffic class, flow label 429 * If flow label is 0, compress it. If traffic class is 0, compress it 430 * We have to process both in the same time as the offset of traffic 431 * class depends on the presence of version and flow label 432 */ 433 434 /* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */ 435 tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4); 436 tmp = ((tmp & 0x03) << 6) | (tmp >> 2); 437 438 if (((hdr->flow_lbl[0] & 0x0F) == 0) && 439 (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) { 440 /* flow label can be compressed */ 441 iphc0 |= LOWPAN_IPHC_FL_C; 442 if ((hdr->priority == 0) && 443 ((hdr->flow_lbl[0] & 0xF0) == 0)) { 444 /* compress (elide) all */ 445 iphc0 |= LOWPAN_IPHC_TC_C; 446 } else { 447 /* compress only the flow label */ 448 *hc06_ptr = tmp; 449 hc06_ptr += 1; 450 } 451 } else { 452 /* Flow label cannot be compressed */ 453 if ((hdr->priority == 0) && 454 ((hdr->flow_lbl[0] & 0xF0) == 0)) { 455 /* compress only traffic class */ 456 iphc0 |= LOWPAN_IPHC_TC_C; 457 *hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F); 458 memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2); 459 hc06_ptr += 3; 460 } else { 461 /* compress nothing */ 462 memcpy(hc06_ptr, &hdr, 4); 463 /* replace the top byte with new ECN | DSCP format */ 464 *hc06_ptr = tmp; 465 hc06_ptr += 4; 466 } 467 } 468 469 /* NOTE: payload length is always compressed */ 470 471 /* Next Header is compress if UDP */ 472 if (hdr->nexthdr == UIP_PROTO_UDP) 473 iphc0 |= LOWPAN_IPHC_NH_C; 474 475 if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { 476 *hc06_ptr = hdr->nexthdr; 477 hc06_ptr += 1; 478 } 479 480 /* 481 * Hop limit 482 * if 1: compress, encoding is 01 483 * if 64: compress, encoding is 10 484 * if 255: compress, encoding is 11 485 * else do not compress 486 */ 487 switch (hdr->hop_limit) { 488 case 1: 489 iphc0 |= LOWPAN_IPHC_TTL_1; 490 break; 491 case 64: 492 iphc0 |= LOWPAN_IPHC_TTL_64; 493 break; 494 case 255: 495 iphc0 |= LOWPAN_IPHC_TTL_255; 496 break; 497 default: 498 *hc06_ptr = hdr->hop_limit; 499 hc06_ptr += 1; 500 break; 501 } 502 503 /* source address compression */ 504 if (is_addr_unspecified(&hdr->saddr)) { 505 pr_debug("source address is unspecified, setting SAC\n"); 506 iphc1 |= LOWPAN_IPHC_SAC; 507 /* TODO: context lookup */ 508 } else if (is_addr_link_local(&hdr->saddr)) { 509 pr_debug("source address is link-local\n"); 510 iphc1 |= lowpan_compress_addr_64(&hc06_ptr, 511 LOWPAN_IPHC_SAM_BIT, &hdr->saddr, saddr); 512 } else { 513 pr_debug("send the full source address\n"); 514 memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16); 515 hc06_ptr += 16; 516 } 517 518 /* destination address compression */ 519 if (is_addr_mcast(&hdr->daddr)) { 520 pr_debug("destination address is multicast: "); 521 iphc1 |= LOWPAN_IPHC_M; 522 if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) { 523 pr_debug("compressed to 1 octet\n"); 524 iphc1 |= LOWPAN_IPHC_DAM_11; 525 /* use last byte */ 526 *hc06_ptr = hdr->daddr.s6_addr[15]; 527 hc06_ptr += 1; 528 } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) { 529 pr_debug("compressed to 4 octets\n"); 530 iphc1 |= LOWPAN_IPHC_DAM_10; 531 /* second byte + the last three */ 532 *hc06_ptr = hdr->daddr.s6_addr[1]; 533 memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3); 534 hc06_ptr += 4; 535 } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) { 536 pr_debug("compressed to 6 octets\n"); 537 iphc1 |= LOWPAN_IPHC_DAM_01; 538 /* second byte + the last five */ 539 *hc06_ptr = hdr->daddr.s6_addr[1]; 540 memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5); 541 hc06_ptr += 6; 542 } else { 543 pr_debug("using full address\n"); 544 iphc1 |= LOWPAN_IPHC_DAM_00; 545 memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16); 546 hc06_ptr += 16; 547 } 548 } else { 549 /* TODO: context lookup */ 550 if (is_addr_link_local(&hdr->daddr)) { 551 pr_debug("dest address is unicast and link-local\n"); 552 iphc1 |= lowpan_compress_addr_64(&hc06_ptr, 553 LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr); 554 } else { 555 pr_debug("dest address is unicast: using full one\n"); 556 memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16); 557 hc06_ptr += 16; 558 } 559 } 560 561 /* UDP header compression */ 562 if (hdr->nexthdr == UIP_PROTO_UDP) 563 lowpan_compress_udp_header(&hc06_ptr, skb); 564 565 head[0] = iphc0; 566 head[1] = iphc1; 567 568 skb_pull(skb, sizeof(struct ipv6hdr)); 569 memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head); 570 571 lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data, 572 skb->len); 573 574 /* 575 * NOTE1: I'm still unsure about the fact that compression and WPAN 576 * header are created here and not later in the xmit. So wait for 577 * an opinion of net maintainers. 578 */ 579 /* 580 * NOTE2: to be absolutely correct, we must derive PANid information 581 * from MAC subif of the 'dev' and 'real_dev' network devices, but 582 * this isn't implemented in mainline yet, so currently we assign 0xff 583 */ 584 { 585 mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA; 586 mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev); 587 588 /* prepare wpan address data */ 589 sa.addr_type = IEEE802154_ADDR_LONG; 590 sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev); 591 592 memcpy(&(sa.hwaddr), saddr, 8); 593 /* intra-PAN communications */ 594 da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev); 595 596 /* 597 * if the destination address is the broadcast address, use the 598 * corresponding short address 599 */ 600 if (lowpan_is_addr_broadcast(daddr)) { 601 da.addr_type = IEEE802154_ADDR_SHORT; 602 da.short_addr = IEEE802154_ADDR_BROADCAST; 603 } else { 604 da.addr_type = IEEE802154_ADDR_LONG; 605 memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN); 606 607 /* request acknowledgment */ 608 mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ; 609 } 610 611 return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev, 612 type, (void *)&da, (void *)&sa, skb->len); 613 } 614} 615 616static int lowpan_give_skb_to_devices(struct sk_buff *skb) 617{ 618 struct lowpan_dev_record *entry; 619 struct sk_buff *skb_cp; 620 int stat = NET_RX_SUCCESS; 621 622 rcu_read_lock(); 623 list_for_each_entry_rcu(entry, &lowpan_devices, list) 624 if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) { 625 skb_cp = skb_copy(skb, GFP_ATOMIC); 626 if (!skb_cp) { 627 stat = -ENOMEM; 628 break; 629 } 630 631 skb_cp->dev = entry->ldev; 632 stat = netif_rx(skb_cp); 633 } 634 rcu_read_unlock(); 635 636 return stat; 637} 638 639static int lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr) 640{ 641 struct sk_buff *new; 642 int stat = NET_RX_SUCCESS; 643 644 new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb), 645 GFP_ATOMIC); 646 kfree_skb(skb); 647 648 if (!new) 649 return -ENOMEM; 650 651 skb_push(new, sizeof(struct ipv6hdr)); 652 skb_reset_network_header(new); 653 skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr)); 654 655 new->protocol = htons(ETH_P_IPV6); 656 new->pkt_type = PACKET_HOST; 657 658 stat = lowpan_give_skb_to_devices(new); 659 660 kfree_skb(new); 661 662 return stat; 663} 664 665static void lowpan_fragment_timer_expired(unsigned long entry_addr) 666{ 667 struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr; 668 669 pr_debug("timer expired for frame with tag %d\n", entry->tag); 670 671 list_del(&entry->list); 672 dev_kfree_skb(entry->skb); 673 kfree(entry); 674} 675 676static struct lowpan_fragment * 677lowpan_alloc_new_frame(struct sk_buff *skb, u16 len, u16 tag) 678{ 679 struct lowpan_fragment *frame; 680 681 frame = kzalloc(sizeof(struct lowpan_fragment), 682 GFP_ATOMIC); 683 if (!frame) 684 goto frame_err; 685 686 INIT_LIST_HEAD(&frame->list); 687 688 frame->length = len; 689 frame->tag = tag; 690 691 /* allocate buffer for frame assembling */ 692 frame->skb = netdev_alloc_skb_ip_align(skb->dev, frame->length + 693 sizeof(struct ipv6hdr)); 694 695 if (!frame->skb) 696 goto skb_err; 697 698 frame->skb->priority = skb->priority; 699 frame->skb->dev = skb->dev; 700 701 /* reserve headroom for uncompressed ipv6 header */ 702 skb_reserve(frame->skb, sizeof(struct ipv6hdr)); 703 skb_put(frame->skb, frame->length); 704 705 init_timer(&frame->timer); 706 /* time out is the same as for ipv6 - 60 sec */ 707 frame->timer.expires = jiffies + LOWPAN_FRAG_TIMEOUT; 708 frame->timer.data = (unsigned long)frame; 709 frame->timer.function = lowpan_fragment_timer_expired; 710 711 add_timer(&frame->timer); 712 713 list_add_tail(&frame->list, &lowpan_fragments); 714 715 return frame; 716 717skb_err: 718 kfree(frame); 719frame_err: 720 return NULL; 721} 722 723static int 724lowpan_process_data(struct sk_buff *skb) 725{ 726 struct ipv6hdr hdr; 727 u8 tmp, iphc0, iphc1, num_context = 0; 728 u8 *_saddr, *_daddr; 729 int err; 730 731 lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data, 732 skb->len); 733 /* at least two bytes will be used for the encoding */ 734 if (skb->len < 2) 735 goto drop; 736 737 if (lowpan_fetch_skb_u8(skb, &iphc0)) 738 goto drop; 739 740 /* fragments assembling */ 741 switch (iphc0 & LOWPAN_DISPATCH_MASK) { 742 case LOWPAN_DISPATCH_FRAG1: 743 case LOWPAN_DISPATCH_FRAGN: 744 { 745 struct lowpan_fragment *frame; 746 /* slen stores the rightmost 8 bits of the 11 bits length */ 747 u8 slen, offset = 0; 748 u16 len, tag; 749 bool found = false; 750 751 if (lowpan_fetch_skb_u8(skb, &slen) || /* frame length */ 752 lowpan_fetch_skb_u16(skb, &tag)) /* fragment tag */ 753 goto drop; 754 755 /* adds the 3 MSB to the 8 LSB to retrieve the 11 bits length */ 756 len = ((iphc0 & 7) << 8) | slen; 757 758 if ((iphc0 & LOWPAN_DISPATCH_MASK) == LOWPAN_DISPATCH_FRAG1) { 759 pr_debug("%s received a FRAG1 packet (tag: %d, " 760 "size of the entire IP packet: %d)", 761 __func__, tag, len); 762 } else { /* FRAGN */ 763 if (lowpan_fetch_skb_u8(skb, &offset)) 764 goto unlock_and_drop; 765 pr_debug("%s received a FRAGN packet (tag: %d, " 766 "size of the entire IP packet: %d, " 767 "offset: %d)", __func__, tag, len, offset * 8); 768 } 769 770 /* 771 * check if frame assembling with the same tag is 772 * already in progress 773 */ 774 spin_lock_bh(&flist_lock); 775 776 list_for_each_entry(frame, &lowpan_fragments, list) 777 if (frame->tag == tag) { 778 found = true; 779 break; 780 } 781 782 /* alloc new frame structure */ 783 if (!found) { 784 pr_debug("%s first fragment received for tag %d, " 785 "begin packet reassembly", __func__, tag); 786 frame = lowpan_alloc_new_frame(skb, len, tag); 787 if (!frame) 788 goto unlock_and_drop; 789 } 790 791 /* if payload fits buffer, copy it */ 792 if (likely((offset * 8 + skb->len) <= frame->length)) 793 skb_copy_to_linear_data_offset(frame->skb, offset * 8, 794 skb->data, skb->len); 795 else 796 goto unlock_and_drop; 797 798 frame->bytes_rcv += skb->len; 799 800 /* frame assembling complete */ 801 if ((frame->bytes_rcv == frame->length) && 802 frame->timer.expires > jiffies) { 803 /* if timer haven't expired - first of all delete it */ 804 del_timer_sync(&frame->timer); 805 list_del(&frame->list); 806 spin_unlock_bh(&flist_lock); 807 808 pr_debug("%s successfully reassembled fragment " 809 "(tag %d)", __func__, tag); 810 811 dev_kfree_skb(skb); 812 skb = frame->skb; 813 kfree(frame); 814 815 if (lowpan_fetch_skb_u8(skb, &iphc0)) 816 goto drop; 817 818 break; 819 } 820 spin_unlock_bh(&flist_lock); 821 822 return kfree_skb(skb), 0; 823 } 824 default: 825 break; 826 } 827 828 if (lowpan_fetch_skb_u8(skb, &iphc1)) 829 goto drop; 830 831 _saddr = mac_cb(skb)->sa.hwaddr; 832 _daddr = mac_cb(skb)->da.hwaddr; 833 834 pr_debug("iphc0 = %02x, iphc1 = %02x\n", iphc0, iphc1); 835 836 /* another if the CID flag is set */ 837 if (iphc1 & LOWPAN_IPHC_CID) { 838 pr_debug("CID flag is set, increase header with one\n"); 839 if (lowpan_fetch_skb_u8(skb, &num_context)) 840 goto drop; 841 } 842 843 hdr.version = 6; 844 845 /* Traffic Class and Flow Label */ 846 switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) { 847 /* 848 * Traffic Class and FLow Label carried in-line 849 * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) 850 */ 851 case 0: /* 00b */ 852 if (lowpan_fetch_skb_u8(skb, &tmp)) 853 goto drop; 854 855 memcpy(&hdr.flow_lbl, &skb->data[0], 3); 856 skb_pull(skb, 3); 857 hdr.priority = ((tmp >> 2) & 0x0f); 858 hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) | 859 (hdr.flow_lbl[0] & 0x0f); 860 break; 861 /* 862 * Traffic class carried in-line 863 * ECN + DSCP (1 byte), Flow Label is elided 864 */ 865 case 1: /* 10b */ 866 if (lowpan_fetch_skb_u8(skb, &tmp)) 867 goto drop; 868 869 hdr.priority = ((tmp >> 2) & 0x0f); 870 hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30); 871 hdr.flow_lbl[1] = 0; 872 hdr.flow_lbl[2] = 0; 873 break; 874 /* 875 * Flow Label carried in-line 876 * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided 877 */ 878 case 2: /* 01b */ 879 if (lowpan_fetch_skb_u8(skb, &tmp)) 880 goto drop; 881 882 hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30); 883 memcpy(&hdr.flow_lbl[1], &skb->data[0], 2); 884 skb_pull(skb, 2); 885 break; 886 /* Traffic Class and Flow Label are elided */ 887 case 3: /* 11b */ 888 hdr.priority = 0; 889 hdr.flow_lbl[0] = 0; 890 hdr.flow_lbl[1] = 0; 891 hdr.flow_lbl[2] = 0; 892 break; 893 default: 894 break; 895 } 896 897 /* Next Header */ 898 if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { 899 /* Next header is carried inline */ 900 if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr))) 901 goto drop; 902 903 pr_debug("NH flag is set, next header carried inline: %02x\n", 904 hdr.nexthdr); 905 } 906 907 /* Hop Limit */ 908 if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I) 909 hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03]; 910 else { 911 if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit))) 912 goto drop; 913 } 914 915 /* Extract SAM to the tmp variable */ 916 tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03; 917 918 /* Source address uncompression */ 919 pr_debug("source address stateless compression\n"); 920 err = lowpan_uncompress_addr(skb, &hdr.saddr, lowpan_llprefix, 921 lowpan_unc_llconf[tmp], skb->data); 922 if (err) 923 goto drop; 924 925 /* Extract DAM to the tmp variable */ 926 tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03; 927 928 /* check for Multicast Compression */ 929 if (iphc1 & LOWPAN_IPHC_M) { 930 if (iphc1 & LOWPAN_IPHC_DAC) { 931 pr_debug("dest: context-based mcast compression\n"); 932 /* TODO: implement this */ 933 } else { 934 u8 prefix[] = {0xff, 0x02}; 935 936 pr_debug("dest: non context-based mcast compression\n"); 937 if (0 < tmp && tmp < 3) { 938 if (lowpan_fetch_skb_u8(skb, &prefix[1])) 939 goto drop; 940 } 941 942 err = lowpan_uncompress_addr(skb, &hdr.daddr, prefix, 943 lowpan_unc_mxconf[tmp], NULL); 944 if (err) 945 goto drop; 946 } 947 } else { 948 pr_debug("dest: stateless compression\n"); 949 err = lowpan_uncompress_addr(skb, &hdr.daddr, lowpan_llprefix, 950 lowpan_unc_llconf[tmp], skb->data); 951 if (err) 952 goto drop; 953 } 954 955 /* UDP data uncompression */ 956 if (iphc0 & LOWPAN_IPHC_NH_C) { 957 struct udphdr uh; 958 struct sk_buff *new; 959 if (lowpan_uncompress_udp_header(skb, &uh)) 960 goto drop; 961 962 /* 963 * replace the compressed UDP head by the uncompressed UDP 964 * header 965 */ 966 new = skb_copy_expand(skb, sizeof(struct udphdr), 967 skb_tailroom(skb), GFP_ATOMIC); 968 kfree_skb(skb); 969 970 if (!new) 971 return -ENOMEM; 972 973 skb = new; 974 975 skb_push(skb, sizeof(struct udphdr)); 976 skb_reset_transport_header(skb); 977 skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); 978 979 lowpan_raw_dump_table(__func__, "raw UDP header dump", 980 (u8 *)&uh, sizeof(uh)); 981 982 hdr.nexthdr = UIP_PROTO_UDP; 983 } 984 985 /* Not fragmented package */ 986 hdr.payload_len = htons(skb->len); 987 988 pr_debug("skb headroom size = %d, data length = %d\n", 989 skb_headroom(skb), skb->len); 990 991 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t" 992 "nexthdr = 0x%02x\n\thop_lim = %d\n", hdr.version, 993 ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit); 994 995 lowpan_raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, 996 sizeof(hdr)); 997 return lowpan_skb_deliver(skb, &hdr); 998 999unlock_and_drop: 1000 spin_unlock_bh(&flist_lock); 1001drop: 1002 kfree_skb(skb); 1003 return -EINVAL; 1004} 1005 1006static int lowpan_set_address(struct net_device *dev, void *p) 1007{ 1008 struct sockaddr *sa = p; 1009 1010 if (netif_running(dev)) 1011 return -EBUSY; 1012 1013 /* TODO: validate addr */ 1014 memcpy(dev->dev_addr, sa->sa_data, dev->addr_len); 1015 1016 return 0; 1017} 1018 1019static int lowpan_get_mac_header_length(struct sk_buff *skb) 1020{ 1021 /* 1022 * Currently long addressing mode is supported only, so the overall 1023 * header size is 21: 1024 * FC SeqNum DPAN DA SA Sec 1025 * 2 + 1 + 2 + 8 + 8 + 0 = 21 1026 */ 1027 return 21; 1028} 1029 1030static int 1031lowpan_fragment_xmit(struct sk_buff *skb, u8 *head, 1032 int mlen, int plen, int offset, int type) 1033{ 1034 struct sk_buff *frag; 1035 int hlen, ret; 1036 1037 hlen = (type == LOWPAN_DISPATCH_FRAG1) ? 1038 LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE; 1039 1040 lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen); 1041 1042 frag = dev_alloc_skb(hlen + mlen + plen + IEEE802154_MFR_SIZE); 1043 if (!frag) 1044 return -ENOMEM; 1045 1046 frag->priority = skb->priority; 1047 frag->dev = skb->dev; 1048 1049 /* copy header, MFR and payload */ 1050 memcpy(skb_put(frag, mlen), skb->data, mlen); 1051 memcpy(skb_put(frag, hlen), head, hlen); 1052 1053 if (plen) 1054 skb_copy_from_linear_data_offset(skb, offset + mlen, 1055 skb_put(frag, plen), plen); 1056 1057 lowpan_raw_dump_table(__func__, " raw fragment dump", frag->data, 1058 frag->len); 1059 1060 ret = dev_queue_xmit(frag); 1061 1062 return ret; 1063} 1064 1065static int 1066lowpan_skb_fragmentation(struct sk_buff *skb, struct net_device *dev) 1067{ 1068 int err, header_length, payload_length, tag, offset = 0; 1069 u8 head[5]; 1070 1071 header_length = lowpan_get_mac_header_length(skb); 1072 payload_length = skb->len - header_length; 1073 tag = lowpan_dev_info(dev)->fragment_tag++; 1074 1075 /* first fragment header */ 1076 head[0] = LOWPAN_DISPATCH_FRAG1 | ((payload_length >> 8) & 0x7); 1077 head[1] = payload_length & 0xff; 1078 head[2] = tag >> 8; 1079 head[3] = tag & 0xff; 1080 1081 err = lowpan_fragment_xmit(skb, head, header_length, LOWPAN_FRAG_SIZE, 1082 0, LOWPAN_DISPATCH_FRAG1); 1083 1084 if (err) { 1085 pr_debug("%s unable to send FRAG1 packet (tag: %d)", 1086 __func__, tag); 1087 goto exit; 1088 } 1089 1090 offset = LOWPAN_FRAG_SIZE; 1091 1092 /* next fragment header */ 1093 head[0] &= ~LOWPAN_DISPATCH_FRAG1; 1094 head[0] |= LOWPAN_DISPATCH_FRAGN; 1095 1096 while ((payload_length - offset > 0) && (err >= 0)) { 1097 int len = LOWPAN_FRAG_SIZE; 1098 1099 head[4] = offset / 8; 1100 1101 if (payload_length - offset < len) 1102 len = payload_length - offset; 1103 1104 err = lowpan_fragment_xmit(skb, head, header_length, 1105 len, offset, LOWPAN_DISPATCH_FRAGN); 1106 if (err) { 1107 pr_debug("%s unable to send a subsequent FRAGN packet " 1108 "(tag: %d, offset: %d", __func__, tag, offset); 1109 goto exit; 1110 } 1111 1112 offset += len; 1113 } 1114 1115exit: 1116 return err; 1117} 1118 1119static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev) 1120{ 1121 int err = -1; 1122 1123 pr_debug("package xmit\n"); 1124 1125 skb->dev = lowpan_dev_info(dev)->real_dev; 1126 if (skb->dev == NULL) { 1127 pr_debug("ERROR: no real wpan device found\n"); 1128 goto error; 1129 } 1130 1131 /* Send directly if less than the MTU minus the 2 checksum bytes. */ 1132 if (skb->len <= IEEE802154_MTU - IEEE802154_MFR_SIZE) { 1133 err = dev_queue_xmit(skb); 1134 goto out; 1135 } 1136 1137 pr_debug("frame is too big, fragmentation is needed\n"); 1138 err = lowpan_skb_fragmentation(skb, dev); 1139error: 1140 dev_kfree_skb(skb); 1141out: 1142 if (err) 1143 pr_debug("ERROR: xmit failed\n"); 1144 1145 return (err < 0) ? NET_XMIT_DROP : err; 1146} 1147 1148static struct wpan_phy *lowpan_get_phy(const struct net_device *dev) 1149{ 1150 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev; 1151 return ieee802154_mlme_ops(real_dev)->get_phy(real_dev); 1152} 1153 1154static u16 lowpan_get_pan_id(const struct net_device *dev) 1155{ 1156 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev; 1157 return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev); 1158} 1159 1160static u16 lowpan_get_short_addr(const struct net_device *dev) 1161{ 1162 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev; 1163 return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev); 1164} 1165 1166static u8 lowpan_get_dsn(const struct net_device *dev) 1167{ 1168 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev; 1169 return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev); 1170} 1171 1172static struct header_ops lowpan_header_ops = { 1173 .create = lowpan_header_create, 1174}; 1175 1176static const struct net_device_ops lowpan_netdev_ops = { 1177 .ndo_start_xmit = lowpan_xmit, 1178 .ndo_set_mac_address = lowpan_set_address, 1179}; 1180 1181static struct ieee802154_mlme_ops lowpan_mlme = { 1182 .get_pan_id = lowpan_get_pan_id, 1183 .get_phy = lowpan_get_phy, 1184 .get_short_addr = lowpan_get_short_addr, 1185 .get_dsn = lowpan_get_dsn, 1186}; 1187 1188static void lowpan_setup(struct net_device *dev) 1189{ 1190 dev->addr_len = IEEE802154_ADDR_LEN; 1191 memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN); 1192 dev->type = ARPHRD_IEEE802154; 1193 /* Frame Control + Sequence Number + Address fields + Security Header */ 1194 dev->hard_header_len = 2 + 1 + 20 + 14; 1195 dev->needed_tailroom = 2; /* FCS */ 1196 dev->mtu = 1281; 1197 dev->tx_queue_len = 0; 1198 dev->flags = IFF_BROADCAST | IFF_MULTICAST; 1199 dev->watchdog_timeo = 0; 1200 1201 dev->netdev_ops = &lowpan_netdev_ops; 1202 dev->header_ops = &lowpan_header_ops; 1203 dev->ml_priv = &lowpan_mlme; 1204 dev->destructor = free_netdev; 1205} 1206 1207static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[]) 1208{ 1209 if (tb[IFLA_ADDRESS]) { 1210 if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN) 1211 return -EINVAL; 1212 } 1213 return 0; 1214} 1215 1216static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev, 1217 struct packet_type *pt, struct net_device *orig_dev) 1218{ 1219 struct sk_buff *local_skb; 1220 1221 if (!netif_running(dev)) 1222 goto drop; 1223 1224 if (dev->type != ARPHRD_IEEE802154) 1225 goto drop; 1226 1227 /* check that it's our buffer */ 1228 if (skb->data[0] == LOWPAN_DISPATCH_IPV6) { 1229 /* Copy the packet so that the IPv6 header is 1230 * properly aligned. 1231 */ 1232 local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1, 1233 skb_tailroom(skb), GFP_ATOMIC); 1234 if (!local_skb) 1235 goto drop; 1236 1237 local_skb->protocol = htons(ETH_P_IPV6); 1238 local_skb->pkt_type = PACKET_HOST; 1239 1240 /* Pull off the 1-byte of 6lowpan header. */ 1241 skb_pull(local_skb, 1); 1242 skb_reset_network_header(local_skb); 1243 skb_set_transport_header(local_skb, sizeof(struct ipv6hdr)); 1244 1245 lowpan_give_skb_to_devices(local_skb); 1246 1247 kfree_skb(local_skb); 1248 kfree_skb(skb); 1249 } else { 1250 switch (skb->data[0] & 0xe0) { 1251 case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */ 1252 case LOWPAN_DISPATCH_FRAG1: /* first fragment header */ 1253 case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */ 1254 local_skb = skb_clone(skb, GFP_ATOMIC); 1255 if (!local_skb) 1256 goto drop; 1257 lowpan_process_data(local_skb); 1258 1259 kfree_skb(skb); 1260 break; 1261 default: 1262 break; 1263 } 1264 } 1265 1266 return NET_RX_SUCCESS; 1267 1268drop: 1269 kfree_skb(skb); 1270 return NET_RX_DROP; 1271} 1272 1273static int lowpan_newlink(struct net *src_net, struct net_device *dev, 1274 struct nlattr *tb[], struct nlattr *data[]) 1275{ 1276 struct net_device *real_dev; 1277 struct lowpan_dev_record *entry; 1278 1279 pr_debug("adding new link\n"); 1280 1281 if (!tb[IFLA_LINK]) 1282 return -EINVAL; 1283 /* find and hold real wpan device */ 1284 real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK])); 1285 if (!real_dev) 1286 return -ENODEV; 1287 1288 lowpan_dev_info(dev)->real_dev = real_dev; 1289 lowpan_dev_info(dev)->fragment_tag = 0; 1290 mutex_init(&lowpan_dev_info(dev)->dev_list_mtx); 1291 1292 entry = kzalloc(sizeof(struct lowpan_dev_record), GFP_KERNEL); 1293 if (!entry) { 1294 dev_put(real_dev); 1295 lowpan_dev_info(dev)->real_dev = NULL; 1296 return -ENOMEM; 1297 } 1298 1299 entry->ldev = dev; 1300 1301 mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx); 1302 INIT_LIST_HEAD(&entry->list); 1303 list_add_tail(&entry->list, &lowpan_devices); 1304 mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx); 1305 1306 register_netdevice(dev); 1307 1308 return 0; 1309} 1310 1311static void lowpan_dellink(struct net_device *dev, struct list_head *head) 1312{ 1313 struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev); 1314 struct net_device *real_dev = lowpan_dev->real_dev; 1315 struct lowpan_dev_record *entry, *tmp; 1316 1317 ASSERT_RTNL(); 1318 1319 mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx); 1320 list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) { 1321 if (entry->ldev == dev) { 1322 list_del(&entry->list); 1323 kfree(entry); 1324 } 1325 } 1326 mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx); 1327 1328 mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx); 1329 1330 unregister_netdevice_queue(dev, head); 1331 1332 dev_put(real_dev); 1333} 1334 1335static struct rtnl_link_ops lowpan_link_ops __read_mostly = { 1336 .kind = "lowpan", 1337 .priv_size = sizeof(struct lowpan_dev_info), 1338 .setup = lowpan_setup, 1339 .newlink = lowpan_newlink, 1340 .dellink = lowpan_dellink, 1341 .validate = lowpan_validate, 1342}; 1343 1344static inline int __init lowpan_netlink_init(void) 1345{ 1346 return rtnl_link_register(&lowpan_link_ops); 1347} 1348 1349static inline void lowpan_netlink_fini(void) 1350{ 1351 rtnl_link_unregister(&lowpan_link_ops); 1352} 1353 1354static int lowpan_device_event(struct notifier_block *unused, 1355 unsigned long event, 1356 void *ptr) 1357{ 1358 struct net_device *dev = ptr; 1359 LIST_HEAD(del_list); 1360 struct lowpan_dev_record *entry, *tmp; 1361 1362 if (dev->type != ARPHRD_IEEE802154) 1363 goto out; 1364 1365 if (event == NETDEV_UNREGISTER) { 1366 list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) { 1367 if (lowpan_dev_info(entry->ldev)->real_dev == dev) 1368 lowpan_dellink(entry->ldev, &del_list); 1369 } 1370 1371 unregister_netdevice_many(&del_list); 1372 } 1373 1374out: 1375 return NOTIFY_DONE; 1376} 1377 1378static struct notifier_block lowpan_dev_notifier = { 1379 .notifier_call = lowpan_device_event, 1380}; 1381 1382static struct packet_type lowpan_packet_type = { 1383 .type = __constant_htons(ETH_P_IEEE802154), 1384 .func = lowpan_rcv, 1385}; 1386 1387static int __init lowpan_init_module(void) 1388{ 1389 int err = 0; 1390 1391 err = lowpan_netlink_init(); 1392 if (err < 0) 1393 goto out; 1394 1395 dev_add_pack(&lowpan_packet_type); 1396 1397 err = register_netdevice_notifier(&lowpan_dev_notifier); 1398 if (err < 0) { 1399 dev_remove_pack(&lowpan_packet_type); 1400 lowpan_netlink_fini(); 1401 } 1402out: 1403 return err; 1404} 1405 1406static void __exit lowpan_cleanup_module(void) 1407{ 1408 struct lowpan_fragment *frame, *tframe; 1409 1410 lowpan_netlink_fini(); 1411 1412 dev_remove_pack(&lowpan_packet_type); 1413 1414 unregister_netdevice_notifier(&lowpan_dev_notifier); 1415 1416 /* Now 6lowpan packet_type is removed, so no new fragments are 1417 * expected on RX, therefore that's the time to clean incomplete 1418 * fragments. 1419 */ 1420 spin_lock_bh(&flist_lock); 1421 list_for_each_entry_safe(frame, tframe, &lowpan_fragments, list) { 1422 del_timer_sync(&frame->timer); 1423 list_del(&frame->list); 1424 dev_kfree_skb(frame->skb); 1425 kfree(frame); 1426 } 1427 spin_unlock_bh(&flist_lock); 1428} 1429 1430module_init(lowpan_init_module); 1431module_exit(lowpan_cleanup_module); 1432MODULE_LICENSE("GPL"); 1433MODULE_ALIAS_RTNL_LINK("lowpan"); 1434