PageRenderTime 137ms CodeModel.GetById 18ms app.highlight 108ms RepoModel.GetById 1ms app.codeStats 0ms

/net/ieee802154/6lowpan.c

http://github.com/mirrors/linux
C | 1525 lines | 1034 code | 244 blank | 247 comment | 156 complexity | c793e91ed55a29126ad26d1a0a6ffd0d MD5 | raw file
   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/* private device info */
  71struct lowpan_dev_info {
  72	struct net_device	*real_dev; /* real WPAN device ptr */
  73	struct mutex		dev_list_mtx; /* mutex for list ops */
  74	unsigned short		fragment_tag;
  75};
  76
  77struct lowpan_dev_record {
  78	struct net_device *ldev;
  79	struct list_head list;
  80};
  81
  82struct lowpan_fragment {
  83	struct sk_buff		*skb;		/* skb to be assembled */
  84	u16			length;		/* length to be assemled */
  85	u32			bytes_rcv;	/* bytes received */
  86	u16			tag;		/* current fragment tag */
  87	struct timer_list	timer;		/* assembling timer */
  88	struct list_head	list;		/* fragments list */
  89};
  90
  91static LIST_HEAD(lowpan_fragments);
  92static DEFINE_SPINLOCK(flist_lock);
  93
  94static inline struct
  95lowpan_dev_info *lowpan_dev_info(const struct net_device *dev)
  96{
  97	return netdev_priv(dev);
  98}
  99
 100static inline void lowpan_address_flip(u8 *src, u8 *dest)
 101{
 102	int i;
 103	for (i = 0; i < IEEE802154_ADDR_LEN; i++)
 104		(dest)[IEEE802154_ADDR_LEN - i - 1] = (src)[i];
 105}
 106
 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			break;
 444		}
 445
 446		pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
 447			 uh->source, uh->dest);
 448
 449		/* copy checksum */
 450		memcpy(&uh->check, &skb->data[0], 2);
 451		skb_pull(skb, 2);
 452
 453		/*
 454		 * UDP lenght needs to be infered from the lower layers
 455		 * here, we obtain the hint from the remaining size of the
 456		 * frame
 457		 */
 458		uh->len = htons(skb->len + sizeof(struct udphdr));
 459		pr_debug("uncompressed UDP length: src = %d", uh->len);
 460	} else {
 461		pr_debug("ERROR: unsupported NH format\n");
 462		goto err;
 463	}
 464
 465	return 0;
 466err:
 467	return -EINVAL;
 468}
 469
 470static int lowpan_header_create(struct sk_buff *skb,
 471			   struct net_device *dev,
 472			   unsigned short type, const void *_daddr,
 473			   const void *_saddr, unsigned int len)
 474{
 475	u8 tmp, iphc0, iphc1, *hc06_ptr;
 476	struct ipv6hdr *hdr;
 477	const u8 *saddr = _saddr;
 478	const u8 *daddr = _daddr;
 479	u8 head[100];
 480	struct ieee802154_addr sa, da;
 481
 482	/* TODO:
 483	 * if this package isn't ipv6 one, where should it be routed?
 484	 */
 485	if (type != ETH_P_IPV6)
 486		return 0;
 487
 488	hdr = ipv6_hdr(skb);
 489	hc06_ptr = head + 2;
 490
 491	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n"
 492		 "\tnexthdr = 0x%02x\n\thop_lim = %d\n", hdr->version,
 493		 ntohs(hdr->payload_len), hdr->nexthdr, hdr->hop_limit);
 494
 495	lowpan_raw_dump_table(__func__, "raw skb network header dump",
 496		skb_network_header(skb), sizeof(struct ipv6hdr));
 497
 498	if (!saddr)
 499		saddr = dev->dev_addr;
 500
 501	lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
 502
 503	/*
 504	 * As we copy some bit-length fields, in the IPHC encoding bytes,
 505	 * we sometimes use |=
 506	 * If the field is 0, and the current bit value in memory is 1,
 507	 * this does not work. We therefore reset the IPHC encoding here
 508	 */
 509	iphc0 = LOWPAN_DISPATCH_IPHC;
 510	iphc1 = 0;
 511
 512	/* TODO: context lookup */
 513
 514	lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
 515
 516	/*
 517	 * Traffic class, flow label
 518	 * If flow label is 0, compress it. If traffic class is 0, compress it
 519	 * We have to process both in the same time as the offset of traffic
 520	 * class depends on the presence of version and flow label
 521	 */
 522
 523	/* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */
 524	tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4);
 525	tmp = ((tmp & 0x03) << 6) | (tmp >> 2);
 526
 527	if (((hdr->flow_lbl[0] & 0x0F) == 0) &&
 528	     (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) {
 529		/* flow label can be compressed */
 530		iphc0 |= LOWPAN_IPHC_FL_C;
 531		if ((hdr->priority == 0) &&
 532		   ((hdr->flow_lbl[0] & 0xF0) == 0)) {
 533			/* compress (elide) all */
 534			iphc0 |= LOWPAN_IPHC_TC_C;
 535		} else {
 536			/* compress only the flow label */
 537			*hc06_ptr = tmp;
 538			hc06_ptr += 1;
 539		}
 540	} else {
 541		/* Flow label cannot be compressed */
 542		if ((hdr->priority == 0) &&
 543		   ((hdr->flow_lbl[0] & 0xF0) == 0)) {
 544			/* compress only traffic class */
 545			iphc0 |= LOWPAN_IPHC_TC_C;
 546			*hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F);
 547			memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2);
 548			hc06_ptr += 3;
 549		} else {
 550			/* compress nothing */
 551			memcpy(hc06_ptr, &hdr, 4);
 552			/* replace the top byte with new ECN | DSCP format */
 553			*hc06_ptr = tmp;
 554			hc06_ptr += 4;
 555		}
 556	}
 557
 558	/* NOTE: payload length is always compressed */
 559
 560	/* Next Header is compress if UDP */
 561	if (hdr->nexthdr == UIP_PROTO_UDP)
 562		iphc0 |= LOWPAN_IPHC_NH_C;
 563
 564	if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
 565		*hc06_ptr = hdr->nexthdr;
 566		hc06_ptr += 1;
 567	}
 568
 569	/*
 570	 * Hop limit
 571	 * if 1:   compress, encoding is 01
 572	 * if 64:  compress, encoding is 10
 573	 * if 255: compress, encoding is 11
 574	 * else do not compress
 575	 */
 576	switch (hdr->hop_limit) {
 577	case 1:
 578		iphc0 |= LOWPAN_IPHC_TTL_1;
 579		break;
 580	case 64:
 581		iphc0 |= LOWPAN_IPHC_TTL_64;
 582		break;
 583	case 255:
 584		iphc0 |= LOWPAN_IPHC_TTL_255;
 585		break;
 586	default:
 587		*hc06_ptr = hdr->hop_limit;
 588		hc06_ptr += 1;
 589		break;
 590	}
 591
 592	/* source address compression */
 593	if (is_addr_unspecified(&hdr->saddr)) {
 594		pr_debug("source address is unspecified, setting SAC\n");
 595		iphc1 |= LOWPAN_IPHC_SAC;
 596	/* TODO: context lookup */
 597	} else if (is_addr_link_local(&hdr->saddr)) {
 598		pr_debug("source address is link-local\n");
 599		iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
 600				LOWPAN_IPHC_SAM_BIT, &hdr->saddr, saddr);
 601	} else {
 602		pr_debug("send the full source address\n");
 603		memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16);
 604		hc06_ptr += 16;
 605	}
 606
 607	/* destination address compression */
 608	if (is_addr_mcast(&hdr->daddr)) {
 609		pr_debug("destination address is multicast: ");
 610		iphc1 |= LOWPAN_IPHC_M;
 611		if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) {
 612			pr_debug("compressed to 1 octet\n");
 613			iphc1 |= LOWPAN_IPHC_DAM_11;
 614			/* use last byte */
 615			*hc06_ptr = hdr->daddr.s6_addr[15];
 616			hc06_ptr += 1;
 617		} else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) {
 618			pr_debug("compressed to 4 octets\n");
 619			iphc1 |= LOWPAN_IPHC_DAM_10;
 620			/* second byte + the last three */
 621			*hc06_ptr = hdr->daddr.s6_addr[1];
 622			memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3);
 623			hc06_ptr += 4;
 624		} else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) {
 625			pr_debug("compressed to 6 octets\n");
 626			iphc1 |= LOWPAN_IPHC_DAM_01;
 627			/* second byte + the last five */
 628			*hc06_ptr = hdr->daddr.s6_addr[1];
 629			memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5);
 630			hc06_ptr += 6;
 631		} else {
 632			pr_debug("using full address\n");
 633			iphc1 |= LOWPAN_IPHC_DAM_00;
 634			memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16);
 635			hc06_ptr += 16;
 636		}
 637	} else {
 638		/* TODO: context lookup */
 639		if (is_addr_link_local(&hdr->daddr)) {
 640			pr_debug("dest address is unicast and link-local\n");
 641			iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
 642				LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr);
 643		} else {
 644			pr_debug("dest address is unicast: using full one\n");
 645			memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16);
 646			hc06_ptr += 16;
 647		}
 648	}
 649
 650	/* UDP header compression */
 651	if (hdr->nexthdr == UIP_PROTO_UDP)
 652		lowpan_compress_udp_header(&hc06_ptr, skb);
 653
 654	head[0] = iphc0;
 655	head[1] = iphc1;
 656
 657	skb_pull(skb, sizeof(struct ipv6hdr));
 658	memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head);
 659
 660	lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
 661				skb->len);
 662
 663	/*
 664	 * NOTE1: I'm still unsure about the fact that compression and WPAN
 665	 * header are created here and not later in the xmit. So wait for
 666	 * an opinion of net maintainers.
 667	 */
 668	/*
 669	 * NOTE2: to be absolutely correct, we must derive PANid information
 670	 * from MAC subif of the 'dev' and 'real_dev' network devices, but
 671	 * this isn't implemented in mainline yet, so currently we assign 0xff
 672	 */
 673	{
 674		mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA;
 675		mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
 676
 677		/* prepare wpan address data */
 678		sa.addr_type = IEEE802154_ADDR_LONG;
 679		sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
 680
 681		memcpy(&(sa.hwaddr), saddr, 8);
 682		/* intra-PAN communications */
 683		da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
 684
 685		/*
 686		 * if the destination address is the broadcast address, use the
 687		 * corresponding short address
 688		 */
 689		if (lowpan_is_addr_broadcast(daddr)) {
 690			da.addr_type = IEEE802154_ADDR_SHORT;
 691			da.short_addr = IEEE802154_ADDR_BROADCAST;
 692		} else {
 693			da.addr_type = IEEE802154_ADDR_LONG;
 694			memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN);
 695
 696			/* request acknowledgment */
 697			mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
 698		}
 699
 700		return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
 701				type, (void *)&da, (void *)&sa, skb->len);
 702	}
 703}
 704
 705static int lowpan_give_skb_to_devices(struct sk_buff *skb)
 706{
 707	struct lowpan_dev_record *entry;
 708	struct sk_buff *skb_cp;
 709	int stat = NET_RX_SUCCESS;
 710
 711	rcu_read_lock();
 712	list_for_each_entry_rcu(entry, &lowpan_devices, list)
 713		if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
 714			skb_cp = skb_copy(skb, GFP_ATOMIC);
 715			if (!skb_cp) {
 716				stat = -ENOMEM;
 717				break;
 718			}
 719
 720			skb_cp->dev = entry->ldev;
 721			stat = netif_rx(skb_cp);
 722		}
 723	rcu_read_unlock();
 724
 725	return stat;
 726}
 727
 728static int lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr)
 729{
 730	struct sk_buff *new;
 731	int stat = NET_RX_SUCCESS;
 732
 733	new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb),
 734								GFP_ATOMIC);
 735	kfree_skb(skb);
 736
 737	if (!new)
 738		return -ENOMEM;
 739
 740	skb_push(new, sizeof(struct ipv6hdr));
 741	skb_reset_network_header(new);
 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)
 755{
 756	struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr;
 757
 758	pr_debug("timer expired for frame with tag %d\n", entry->tag);
 759
 760	list_del(&entry->list);
 761	dev_kfree_skb(entry->skb);
 762	kfree(entry);
 763}
 764
 765static struct lowpan_fragment *
 766lowpan_alloc_new_frame(struct sk_buff *skb, u16 len, u16 tag)
 767{
 768	struct lowpan_fragment *frame;
 769
 770	frame = kzalloc(sizeof(struct lowpan_fragment),
 771			GFP_ATOMIC);
 772	if (!frame)
 773		goto frame_err;
 774
 775	INIT_LIST_HEAD(&frame->list);
 776
 777	frame->length = len;
 778	frame->tag = tag;
 779
 780	/* allocate buffer for frame assembling */
 781	frame->skb = netdev_alloc_skb_ip_align(skb->dev, frame->length +
 782					       sizeof(struct ipv6hdr));
 783
 784	if (!frame->skb)
 785		goto skb_err;
 786
 787	frame->skb->priority = skb->priority;
 788	frame->skb->dev = skb->dev;
 789
 790	/* reserve headroom for uncompressed ipv6 header */
 791	skb_reserve(frame->skb, sizeof(struct ipv6hdr));
 792	skb_put(frame->skb, frame->length);
 793
 794	/* copy the first control block to keep a
 795	 * trace of the link-layer addresses in case
 796	 * of a link-local compressed address
 797	 */
 798	memcpy(frame->skb->cb, skb->cb, sizeof(skb->cb));
 799
 800	init_timer(&frame->timer);
 801	/* time out is the same as for ipv6 - 60 sec */
 802	frame->timer.expires = jiffies + LOWPAN_FRAG_TIMEOUT;
 803	frame->timer.data = (unsigned long)frame;
 804	frame->timer.function = lowpan_fragment_timer_expired;
 805
 806	add_timer(&frame->timer);
 807
 808	list_add_tail(&frame->list, &lowpan_fragments);
 809
 810	return frame;
 811
 812skb_err:
 813	kfree(frame);
 814frame_err:
 815	return NULL;
 816}
 817
 818static int
 819lowpan_process_data(struct sk_buff *skb)
 820{
 821	struct ipv6hdr hdr = {};
 822	u8 tmp, iphc0, iphc1, num_context = 0;
 823	const struct ieee802154_addr *_saddr, *_daddr;
 824	int err;
 825
 826	lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
 827				skb->len);
 828	/* at least two bytes will be used for the encoding */
 829	if (skb->len < 2)
 830		goto drop;
 831
 832	if (lowpan_fetch_skb_u8(skb, &iphc0))
 833		goto drop;
 834
 835	/* fragments assembling */
 836	switch (iphc0 & LOWPAN_DISPATCH_MASK) {
 837	case LOWPAN_DISPATCH_FRAG1:
 838	case LOWPAN_DISPATCH_FRAGN:
 839	{
 840		struct lowpan_fragment *frame;
 841		/* slen stores the rightmost 8 bits of the 11 bits length */
 842		u8 slen, offset = 0;
 843		u16 len, tag;
 844		bool found = false;
 845
 846		if (lowpan_fetch_skb_u8(skb, &slen) || /* frame length */
 847		    lowpan_fetch_skb_u16(skb, &tag))  /* fragment tag */
 848			goto drop;
 849
 850		/* adds the 3 MSB to the 8 LSB to retrieve the 11 bits length */
 851		len = ((iphc0 & 7) << 8) | slen;
 852
 853		if ((iphc0 & LOWPAN_DISPATCH_MASK) == LOWPAN_DISPATCH_FRAG1) {
 854			pr_debug("%s received a FRAG1 packet (tag: %d, "
 855				 "size of the entire IP packet: %d)",
 856				 __func__, tag, len);
 857		} else { /* FRAGN */
 858			if (lowpan_fetch_skb_u8(skb, &offset))
 859				goto unlock_and_drop;
 860			pr_debug("%s received a FRAGN packet (tag: %d, "
 861				 "size of the entire IP packet: %d, "
 862				 "offset: %d)", __func__, tag, len, offset * 8);
 863		}
 864
 865		/*
 866		 * check if frame assembling with the same tag is
 867		 * already in progress
 868		 */
 869		spin_lock_bh(&flist_lock);
 870
 871		list_for_each_entry(frame, &lowpan_fragments, list)
 872			if (frame->tag == tag) {
 873				found = true;
 874				break;
 875			}
 876
 877		/* alloc new frame structure */
 878		if (!found) {
 879			pr_debug("%s first fragment received for tag %d, "
 880				 "begin packet reassembly", __func__, tag);
 881			frame = lowpan_alloc_new_frame(skb, len, tag);
 882			if (!frame)
 883				goto unlock_and_drop;
 884		}
 885
 886		/* if payload fits buffer, copy it */
 887		if (likely((offset * 8 + skb->len) <= frame->length))
 888			skb_copy_to_linear_data_offset(frame->skb, offset * 8,
 889							skb->data, skb->len);
 890		else
 891			goto unlock_and_drop;
 892
 893		frame->bytes_rcv += skb->len;
 894
 895		/* frame assembling complete */
 896		if ((frame->bytes_rcv == frame->length) &&
 897		     frame->timer.expires > jiffies) {
 898			/* if timer haven't expired - first of all delete it */
 899			del_timer_sync(&frame->timer);
 900			list_del(&frame->list);
 901			spin_unlock_bh(&flist_lock);
 902
 903			pr_debug("%s successfully reassembled fragment "
 904				 "(tag %d)", __func__, tag);
 905
 906			dev_kfree_skb(skb);
 907			skb = frame->skb;
 908			kfree(frame);
 909
 910			if (lowpan_fetch_skb_u8(skb, &iphc0))
 911				goto drop;
 912
 913			break;
 914		}
 915		spin_unlock_bh(&flist_lock);
 916
 917		return kfree_skb(skb), 0;
 918	}
 919	default:
 920		break;
 921	}
 922
 923	if (lowpan_fetch_skb_u8(skb, &iphc1))
 924		goto drop;
 925
 926	_saddr = &mac_cb(skb)->sa;
 927	_daddr = &mac_cb(skb)->da;
 928
 929	pr_debug("iphc0 = %02x, iphc1 = %02x\n", iphc0, iphc1);
 930
 931	/* another if the CID flag is set */
 932	if (iphc1 & LOWPAN_IPHC_CID) {
 933		pr_debug("CID flag is set, increase header with one\n");
 934		if (lowpan_fetch_skb_u8(skb, &num_context))
 935			goto drop;
 936	}
 937
 938	hdr.version = 6;
 939
 940	/* Traffic Class and Flow Label */
 941	switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) {
 942	/*
 943	 * Traffic Class and FLow Label carried in-line
 944	 * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
 945	 */
 946	case 0: /* 00b */
 947		if (lowpan_fetch_skb_u8(skb, &tmp))
 948			goto drop;
 949
 950		memcpy(&hdr.flow_lbl, &skb->data[0], 3);
 951		skb_pull(skb, 3);
 952		hdr.priority = ((tmp >> 2) & 0x0f);
 953		hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) |
 954					(hdr.flow_lbl[0] & 0x0f);
 955		break;
 956	/*
 957	 * Traffic class carried in-line
 958	 * ECN + DSCP (1 byte), Flow Label is elided
 959	 */
 960	case 1: /* 10b */
 961		if (lowpan_fetch_skb_u8(skb, &tmp))
 962			goto drop;
 963
 964		hdr.priority = ((tmp >> 2) & 0x0f);
 965		hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30);
 966		break;
 967	/*
 968	 * Flow Label carried in-line
 969	 * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
 970	 */
 971	case 2: /* 01b */
 972		if (lowpan_fetch_skb_u8(skb, &tmp))
 973			goto drop;
 974
 975		hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30);
 976		memcpy(&hdr.flow_lbl[1], &skb->data[0], 2);
 977		skb_pull(skb, 2);
 978		break;
 979	/* Traffic Class and Flow Label are elided */
 980	case 3: /* 11b */
 981		break;
 982	default:
 983		break;
 984	}
 985
 986	/* Next Header */
 987	if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
 988		/* Next header is carried inline */
 989		if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr)))
 990			goto drop;
 991
 992		pr_debug("NH flag is set, next header carried inline: %02x\n",
 993			 hdr.nexthdr);
 994	}
 995
 996	/* Hop Limit */
 997	if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I)
 998		hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03];
 999	else {
1000		if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit)))
1001			goto drop;
1002	}
1003
1004	/* Extract SAM to the tmp variable */
1005	tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03;
1006
1007	if (iphc1 & LOWPAN_IPHC_SAC) {
1008		/* Source address context based uncompression */
1009		pr_debug("SAC bit is set. Handle context based source address.\n");
1010		err = lowpan_uncompress_context_based_src_addr(
1011				skb, &hdr.saddr, tmp);
1012	} else {
1013		/* Source address uncompression */
1014		pr_debug("source address stateless compression\n");
1015		err = lowpan_uncompress_addr(skb, &hdr.saddr, tmp, _saddr);
1016	}
1017
1018	/* Check on error of previous branch */
1019	if (err)
1020		goto drop;
1021
1022	/* Extract DAM to the tmp variable */
1023	tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03;
1024
1025	/* check for Multicast Compression */
1026	if (iphc1 & LOWPAN_IPHC_M) {
1027		if (iphc1 & LOWPAN_IPHC_DAC) {
1028			pr_debug("dest: context-based mcast compression\n");
1029			/* TODO: implement this */
1030		} else {
1031			err = lowpan_uncompress_multicast_daddr(
1032					skb, &hdr.daddr, tmp);
1033			if (err)
1034				goto drop;
1035		}
1036	} else {
1037		pr_debug("dest: stateless compression\n");
1038		err = lowpan_uncompress_addr(skb, &hdr.daddr, tmp, _daddr);
1039		if (err)
1040			goto drop;
1041	}
1042
1043	/* UDP data uncompression */
1044	if (iphc0 & LOWPAN_IPHC_NH_C) {
1045		struct udphdr uh;
1046		struct sk_buff *new;
1047		if (lowpan_uncompress_udp_header(skb, &uh))
1048			goto drop;
1049
1050		/*
1051		 * replace the compressed UDP head by the uncompressed UDP
1052		 * header
1053		 */
1054		new = skb_copy_expand(skb, sizeof(struct udphdr),
1055				      skb_tailroom(skb), GFP_ATOMIC);
1056		kfree_skb(skb);
1057
1058		if (!new)
1059			return -ENOMEM;
1060
1061		skb = new;
1062
1063		skb_push(skb, sizeof(struct udphdr));
1064		skb_reset_transport_header(skb);
1065		skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
1066
1067		lowpan_raw_dump_table(__func__, "raw UDP header dump",
1068				      (u8 *)&uh, sizeof(uh));
1069
1070		hdr.nexthdr = UIP_PROTO_UDP;
1071	}
1072
1073	/* Not fragmented package */
1074	hdr.payload_len = htons(skb->len);
1075
1076	pr_debug("skb headroom size = %d, data length = %d\n",
1077		 skb_headroom(skb), skb->len);
1078
1079	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n\t"
1080		 "nexthdr = 0x%02x\n\thop_lim = %d\n", hdr.version,
1081		 ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit);
1082
1083	lowpan_raw_dump_table(__func__, "raw header dump", (u8 *)&hdr,
1084							sizeof(hdr));
1085	return lowpan_skb_deliver(skb, &hdr);
1086
1087unlock_and_drop:
1088	spin_unlock_bh(&flist_lock);
1089drop:
1090	kfree_skb(skb);
1091	return -EINVAL;
1092}
1093
1094static int lowpan_set_address(struct net_device *dev, void *p)
1095{
1096	struct sockaddr *sa = p;
1097
1098	if (netif_running(dev))
1099		return -EBUSY;
1100
1101	/* TODO: validate addr */
1102	memcpy(dev->dev_addr, sa->sa_data, dev->addr_len);
1103
1104	return 0;
1105}
1106
1107static int lowpan_get_mac_header_length(struct sk_buff *skb)
1108{
1109	/*
1110	 * Currently long addressing mode is supported only, so the overall
1111	 * header size is 21:
1112	 * FC SeqNum DPAN DA  SA  Sec
1113	 * 2  +  1  +  2 + 8 + 8 + 0  = 21
1114	 */
1115	return 21;
1116}
1117
1118static int
1119lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
1120			int mlen, int plen, int offset, int type)
1121{
1122	struct sk_buff *frag;
1123	int hlen, ret;
1124
1125	hlen = (type == LOWPAN_DISPATCH_FRAG1) ?
1126			LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE;
1127
1128	lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen);
1129
1130	frag = dev_alloc_skb(hlen + mlen + plen + IEEE802154_MFR_SIZE);
1131	if (!frag)
1132		return -ENOMEM;
1133
1134	frag->priority = skb->priority;
1135	frag->dev = skb->dev;
1136
1137	/* copy header, MFR and payload */
1138	memcpy(skb_put(frag, mlen), skb->data, mlen);
1139	memcpy(skb_put(frag, hlen), head, hlen);
1140
1141	if (plen)
1142		skb_copy_from_linear_data_offset(skb, offset + mlen,
1143					skb_put(frag, plen), plen);
1144
1145	lowpan_raw_dump_table(__func__, " raw fragment dump", frag->data,
1146								frag->len);
1147
1148	ret = dev_queue_xmit(frag);
1149
1150	return ret;
1151}
1152
1153static int
1154lowpan_skb_fragmentation(struct sk_buff *skb, struct net_device *dev)
1155{
1156	int  err, header_length, payload_length, tag, offset = 0;
1157	u8 head[5];
1158
1159	header_length = lowpan_get_mac_header_length(skb);
1160	payload_length = skb->len - header_length;
1161	tag = lowpan_dev_info(dev)->fragment_tag++;
1162
1163	/* first fragment header */
1164	head[0] = LOWPAN_DISPATCH_FRAG1 | ((payload_length >> 8) & 0x7);
1165	head[1] = payload_length & 0xff;
1166	head[2] = tag >> 8;
1167	head[3] = tag & 0xff;
1168
1169	err = lowpan_fragment_xmit(skb, head, header_length, LOWPAN_FRAG_SIZE,
1170				   0, LOWPAN_DISPATCH_FRAG1);
1171
1172	if (err) {
1173		pr_debug("%s unable to send FRAG1 packet (tag: %d)",
1174			 __func__, tag);
1175		goto exit;
1176	}
1177
1178	offset = LOWPAN_FRAG_SIZE;
1179
1180	/* next fragment header */
1181	head[0] &= ~LOWPAN_DISPATCH_FRAG1;
1182	head[0] |= LOWPAN_DISPATCH_FRAGN;
1183
1184	while ((payload_length - offset > 0) && (err >= 0)) {
1185		int len = LOWPAN_FRAG_SIZE;
1186
1187		head[4] = offset / 8;
1188
1189		if (payload_length - offset < len)
1190			len = payload_length - offset;
1191
1192		err = lowpan_fragment_xmit(skb, head, header_length,
1193					   len, offset, LOWPAN_DISPATCH_FRAGN);
1194		if (err) {
1195			pr_debug("%s unable to send a subsequent FRAGN packet "
1196				 "(tag: %d, offset: %d", __func__, tag, offset);
1197			goto exit;
1198		}
1199
1200		offset += len;
1201	}
1202
1203exit:
1204	return err;
1205}
1206
1207static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
1208{
1209	int err = -1;
1210
1211	pr_debug("package xmit\n");
1212
1213	skb->dev = lowpan_dev_info(dev)->real_dev;
1214	if (skb->dev == NULL) {
1215		pr_debug("ERROR: no real wpan device found\n");
1216		goto error;
1217	}
1218
1219	/* Send directly if less than the MTU minus the 2 checksum bytes. */
1220	if (skb->len <= IEEE802154_MTU - IEEE802154_MFR_SIZE) {
1221		err = dev_queue_xmit(skb);
1222		goto out;
1223	}
1224
1225	pr_debug("frame is too big, fragmentation is needed\n");
1226	err = lowpan_skb_fragmentation(skb, dev);
1227error:
1228	dev_kfree_skb(skb);
1229out:
1230	if (err)
1231		pr_debug("ERROR: xmit failed\n");
1232
1233	return (err < 0) ? NET_XMIT_DROP : err;
1234}
1235
1236static struct wpan_phy *lowpan_get_phy(const struct net_device *dev)
1237{
1238	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1239	return ieee802154_mlme_ops(real_dev)->get_phy(real_dev);
1240}
1241
1242static u16 lowpan_get_pan_id(const struct net_device *dev)
1243{
1244	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1245	return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
1246}
1247
1248static u16 lowpan_get_short_addr(const struct net_device *dev)
1249{
1250	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1251	return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
1252}
1253
1254static u8 lowpan_get_dsn(const struct net_device *dev)
1255{
1256	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1257	return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
1258}
1259
1260static struct header_ops lowpan_header_ops = {
1261	.create	= lowpan_header_create,
1262};
1263
1264static const struct net_device_ops lowpan_netdev_ops = {
1265	.ndo_start_xmit		= lowpan_xmit,
1266	.ndo_set_mac_address	= lowpan_set_address,
1267};
1268
1269static struct ieee802154_mlme_ops lowpan_mlme = {
1270	.get_pan_id = lowpan_get_pan_id,
1271	.get_phy = lowpan_get_phy,
1272	.get_short_addr = lowpan_get_short_addr,
1273	.get_dsn = lowpan_get_dsn,
1274};
1275
1276static void lowpan_setup(struct net_device *dev)
1277{
1278	dev->addr_len		= IEEE802154_ADDR_LEN;
1279	memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
1280	dev->type		= ARPHRD_IEEE802154;
1281	/* Frame Control + Sequence Number + Address fields + Security Header */
1282	dev->hard_header_len	= 2 + 1 + 20 + 14;
1283	dev->needed_tailroom	= 2; /* FCS */
1284	dev->mtu		= 1281;
1285	dev->tx_queue_len	= 0;
1286	dev->flags		= IFF_BROADCAST | IFF_MULTICAST;
1287	dev->watchdog_timeo	= 0;
1288
1289	dev->netdev_ops		= &lowpan_netdev_ops;
1290	dev->header_ops		= &lowpan_header_ops;
1291	dev->ml_priv		= &lowpan_mlme;
1292	dev->destructor		= free_netdev;
1293}
1294
1295static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
1296{
1297	if (tb[IFLA_ADDRESS]) {
1298		if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
1299			return -EINVAL;
1300	}
1301	return 0;
1302}
1303
1304static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
1305	struct packet_type *pt, struct net_device *orig_dev)
1306{
1307	struct sk_buff *local_skb;
1308
1309	if (!netif_running(dev))
1310		goto drop;
1311
1312	if (dev->type != ARPHRD_IEEE802154)
1313		goto drop;
1314
1315	/* check that it's our buffer */
1316	if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
1317		/* Copy the packet so that the IPv6 header is
1318		 * properly aligned.
1319		 */
1320		local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
1321					    skb_tailroom(skb), GFP_ATOMIC);
1322		if (!local_skb)
1323			goto drop;
1324
1325		local_skb->protocol = htons(ETH_P_IPV6);
1326		local_skb->pkt_type = PACKET_HOST;
1327
1328		/* Pull off the 1-byte of 6lowpan header. */
1329		skb_pull(local_skb, 1);
1330		skb_reset_network_header(local_skb);
1331		skb_set_transport_header(local_skb, sizeof(struct ipv6hdr));
1332
1333		lowpan_give_skb_to_devices(local_skb);
1334
1335		kfree_skb(local_skb);
1336		kfree_skb(skb);
1337	} else {
1338		switch (skb->data[0] & 0xe0) {
1339		case LOWPAN_DISPATCH_IPHC:	/* ipv6 datagram */
1340		case LOWPAN_DISPATCH_FRAG1:	/* first fragment header */
1341		case LOWPAN_DISPATCH_FRAGN:	/* next fragments headers */
1342			local_skb = skb_clone(skb, GFP_ATOMIC);
1343			if (!local_skb)
1344				goto drop;
1345			lowpan_process_data(local_skb);
1346
1347			kfree_skb(skb);
1348			break;
1349		default:
1350			break;
1351		}
1352	}
1353
1354	return NET_RX_SUCCESS;
1355
1356drop:
1357	kfree_skb(skb);
1358	return NET_RX_DROP;
1359}
1360
1361static int lowpan_newlink(struct net *src_net, struct net_device *dev,
1362			  struct nlattr *tb[], struct nlattr *data[])
1363{
1364	struct net_device *real_dev;
1365	struct lowpan_dev_record *entry;
1366
1367	pr_debug("adding new link\n");
1368
1369	if (!tb[IFLA_LINK])
1370		return -EINVAL;
1371	/* find and hold real wpan device */
1372	real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
1373	if (!real_dev)
1374		return -ENODEV;
1375	if (real_dev->type != ARPHRD_IEEE802154)
1376		return -EINVAL;
1377
1378	lowpan_dev_info(dev)->real_dev = real_dev;
1379	lowpan_dev_info(dev)->fragment_tag = 0;
1380	mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
1381
1382	entry = kzalloc(sizeof(struct lowpan_dev_record), GFP_KERNEL);
1383	if (!entry) {
1384		dev_put(real_dev);
1385		lowpan_dev_info(dev)->real_dev = NULL;
1386		return -ENOMEM;
1387	}
1388
1389	entry->ldev = dev;
1390
1391	/* Set the lowpan harware address to the wpan hardware address. */
1392	memcpy(dev->dev_addr, real_dev->dev_addr, IEEE802154_ADDR_LEN);
1393
1394	mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
1395	INIT_LIST_HEAD(&entry->list);
1396	list_add_tail(&entry->list, &lowpan_devices);
1397	mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1398
1399	register_netdevice(dev);
1400
1401	return 0;
1402}
1403
1404static void lowpan_dellink(struct net_device *dev, struct list_head *head)
1405{
1406	struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
1407	struct net_device *real_dev = lowpan_dev->real_dev;
1408	struct lowpan_dev_record *entry, *tmp;
1409
1410	ASSERT_RTNL();
1411
1412	mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
1413	list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
1414		if (entry->ldev == dev) {
1415			list_del(&entry->list);
1416			kfree(entry);
1417		}
1418	}
1419	mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1420
1421	mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
1422
1423	unregister_netdevice_queue(dev, head);
1424
1425	dev_put(real_dev);
1426}
1427
1428static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
1429	.kind		= "lowpan",
1430	.priv_size	= sizeof(struct lowpan_dev_info),
1431	.setup		= lowpan_setup,
1432	.newlink	= lowpan_newlink,
1433	.dellink	= lowpan_dellink,
1434	.validate	= lowpan_validate,
1435};
1436
1437static inline int __init lowpan_netlink_init(void)
1438{
1439	return rtnl_link_register(&lowpan_link_ops);
1440}
1441
1442static inline void lowpan_netlink_fini(void)
1443{
1444	rtnl_link_unregister(&lowpan_link_ops);
1445}
1446
1447static int lowpan_device_event(struct notifier_block *unused,
1448			       unsigned long event, void *ptr)
1449{
1450	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1451	LIST_HEAD(del_list);
1452	struct lowpan_dev_record *entry, *tmp;
1453
1454	if (dev->type != ARPHRD_IEEE802154)
1455		goto out;
1456
1457	if (event == NETDEV_UNREGISTER) {
1458		list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
1459			if (lowpan_dev_info(entry->ldev)->real_dev == dev)
1460				lowpan_dellink(entry->ldev, &del_list);
1461		}
1462
1463		unregister_netdevice_many(&del_list);
1464	}
1465
1466out:
1467	return NOTIFY_DONE;
1468}
1469
1470static struct notifier_block lowpan_dev_notifier = {
1471	.notifier_call = lowpan_device_event,
1472};
1473
1474static struct packet_type lowpan_packet_type = {
1475	.type = __constant_htons(ETH_P_IEEE802154),
1476	.func = lowpan_rcv,
1477};
1478
1479static int __init lowpan_init_module(void)
1480{
1481	int err = 0;
1482
1483	err = lowpan_netlink_init();
1484	if (err < 0)
1485		goto out;
1486
1487	dev_add_pack(&lowpan_packet_type);
1488
1489	err = register_netdevice_notifier(&lowpan_dev_notifier);
1490	if (err < 0) {
1491		dev_remove_pack(&lowpan_packet_type);
1492		lowpan_netlink_fini();
1493	}
1494out:
1495	return err;
1496}
1497
1498static void __exit lowpan_cleanup_module(void)
1499{
1500	struct lowpan_fragment *frame, *tframe;
1501
1502	lowpan_netlink_fini();
1503
1504	dev_remove_pack(&lowpan_packet_type);
1505
1506	unregister_netdevice_notifier(&lowpan_dev_notifier);
1507
1508	/* Now 6lowpan packet_type is removed, so no new fragments are
1509	 * expected on RX, therefore that's the time to clean incomplete
1510	 * fragments.
1511	 */
1512	spin_lock_bh(&flist_lock);
1513	list_for_each_entry_safe(frame, tframe, &lowpan_fragments, list) {
1514		del_timer_sync(&frame->timer);
1515		list_del(&frame->list);
1516		dev_kfree_skb(frame->skb);
1517		kfree(frame);
1518	}
1519	spin_unlock_bh(&flist_lock);
1520}
1521
1522module_init(lowpan_init_module);
1523module_exit(lowpan_cleanup_module);
1524MODULE_LICENSE("GPL");
1525MODULE_ALIAS_RTNL_LINK("lowpan");