6lowpan_rtnl.c 18.6 KB
Newer Older
1
/* Copyright 2011, Siemens AG
2 3 4
 * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
 */

5
/* Based on patches from Jon Smirl <jonsmirl@gmail.com>
6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54
 * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2
 * as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

/* Jon's code is based on 6lowpan implementation for Contiki which is:
 * Copyright (c) 2008, Swedish Institute of Computer Science.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the Institute nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include <linux/bitops.h>
#include <linux/if_arp.h>
#include <linux/module.h>
#include <linux/moduleparam.h>
#include <linux/netdevice.h>
#include <net/af_ieee802154.h>
#include <net/ieee802154.h>
#include <net/ieee802154_netdev.h>
55
#include <net/6lowpan.h>
56 57
#include <net/ipv6.h>

58
#include "reassembly.h"
59 60 61 62 63 64 65

static LIST_HEAD(lowpan_devices);

/* private device info */
struct lowpan_dev_info {
	struct net_device	*real_dev; /* real WPAN device ptr */
	struct mutex		dev_list_mtx; /* mutex for list ops */
66
	__be16			fragment_tag;
67 68 69 70 71 72 73
};

struct lowpan_dev_record {
	struct net_device *ldev;
	struct list_head list;
};

74 75 76 77 78 79 80 81 82 83 84 85 86 87 88
/* don't save pan id, it's intra pan */
struct lowpan_addr {
	u8 mode;
	union {
		/* IPv6 needs big endian here */
		__be64 extended_addr;
		__be16 short_addr;
	} u;
};

struct lowpan_addr_info {
	struct lowpan_addr daddr;
	struct lowpan_addr saddr;
};

89 90 91 92 93 94
static inline struct
lowpan_dev_info *lowpan_dev_info(const struct net_device *dev)
{
	return netdev_priv(dev);
}

95 96 97 98 99 100 101 102
static inline struct
lowpan_addr_info *lowpan_skb_priv(const struct sk_buff *skb)
{
	WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct lowpan_addr_info));
	return (struct lowpan_addr_info *)(skb->data -
			sizeof(struct lowpan_addr_info));
}

103 104 105
static int lowpan_header_create(struct sk_buff *skb, struct net_device *dev,
				unsigned short type, const void *_daddr,
				const void *_saddr, unsigned int len)
106 107 108
{
	const u8 *saddr = _saddr;
	const u8 *daddr = _daddr;
109
	struct lowpan_addr_info *info;
110

111 112 113
	/* TODO:
	 * if this package isn't ipv6 one, where should it be routed?
	 */
114 115 116 117 118 119
	if (type != ETH_P_IPV6)
		return 0;

	if (!saddr)
		saddr = dev->dev_addr;

120 121
	raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
	raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
122

123
	info = lowpan_skb_priv(skb);
124

125 126 127 128 129 130 131
	/* TODO: Currently we only support extended_addr */
	info->daddr.mode = IEEE802154_ADDR_LONG;
	memcpy(&info->daddr.u.extended_addr, daddr,
	       sizeof(info->daddr.u.extended_addr));
	info->saddr.mode = IEEE802154_ADDR_LONG;
	memcpy(&info->saddr.u.extended_addr, saddr,
	       sizeof(info->daddr.u.extended_addr));
132

133
	return 0;
134 135
}

136
static int lowpan_give_skb_to_devices(struct sk_buff *skb,
137
				      struct net_device *dev)
138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159
{
	struct lowpan_dev_record *entry;
	struct sk_buff *skb_cp;
	int stat = NET_RX_SUCCESS;

	rcu_read_lock();
	list_for_each_entry_rcu(entry, &lowpan_devices, list)
		if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
			skb_cp = skb_copy(skb, GFP_ATOMIC);
			if (!skb_cp) {
				stat = -ENOMEM;
				break;
			}

			skb_cp->dev = entry->ldev;
			stat = netif_rx(skb_cp);
		}
	rcu_read_unlock();

	return stat;
}

160
static int process_data(struct sk_buff *skb, const struct ieee802154_hdr *hdr)
161
{
162
	u8 iphc0, iphc1;
163 164
	struct ieee802154_addr_sa sa, da;
	void *sap, *dap;
165

166
	raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
167 168 169
	/* at least two bytes will be used for the encoding */
	if (skb->len < 2)
		goto drop;
170 171 172

	if (lowpan_fetch_skb_u8(skb, &iphc0))
		goto drop;
173

174 175
	if (lowpan_fetch_skb_u8(skb, &iphc1))
		goto drop;
176

177 178
	ieee802154_addr_to_sa(&sa, &hdr->source);
	ieee802154_addr_to_sa(&da, &hdr->dest);
179

180 181 182 183 184 185 186 187 188 189 190 191 192 193
	if (sa.addr_type == IEEE802154_ADDR_SHORT)
		sap = &sa.short_addr;
	else
		sap = &sa.hwaddr;

	if (da.addr_type == IEEE802154_ADDR_SHORT)
		dap = &da.short_addr;
	else
		dap = &da.hwaddr;

	return lowpan_process_data(skb, skb->dev, sap, sa.addr_type,
				   IEEE802154_ADDR_LEN, dap, da.addr_type,
				   IEEE802154_ADDR_LEN, iphc0, iphc1,
				   lowpan_give_skb_to_devices);
194

195
drop:
196
	kfree_skb(skb);
197 198 199
	return -EINVAL;
}

200 201 202 203 204 205 206 207 208 209 210 211 212
static int lowpan_set_address(struct net_device *dev, void *p)
{
	struct sockaddr *sa = p;

	if (netif_running(dev))
		return -EBUSY;

	/* TODO: validate addr */
	memcpy(dev->dev_addr, sa->sa_data, dev->addr_len);

	return 0;
}

P
Phoebe Buckheister 已提交
213 214 215
static struct sk_buff*
lowpan_alloc_frag(struct sk_buff *skb, int size,
		  const struct ieee802154_hdr *master_hdr)
216
{
P
Phoebe Buckheister 已提交
217
	struct net_device *real_dev = lowpan_dev_info(skb->dev)->real_dev;
218
	struct sk_buff *frag;
P
Phoebe Buckheister 已提交
219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238
	int rc;

	frag = alloc_skb(real_dev->hard_header_len +
			 real_dev->needed_tailroom + size,
			 GFP_ATOMIC);

	if (likely(frag)) {
		frag->dev = real_dev;
		frag->priority = skb->priority;
		skb_reserve(frag, real_dev->hard_header_len);
		skb_reset_network_header(frag);
		*mac_cb(frag) = *mac_cb(skb);

		rc = dev_hard_header(frag, real_dev, 0, &master_hdr->dest,
				     &master_hdr->source, size);
		if (rc < 0) {
			kfree_skb(frag);
			return ERR_PTR(-rc);
		}
	} else {
239
		frag = ERR_PTR(-ENOMEM);
P
Phoebe Buckheister 已提交
240
	}
241

P
Phoebe Buckheister 已提交
242 243
	return frag;
}
244

P
Phoebe Buckheister 已提交
245 246 247 248 249 250
static int
lowpan_xmit_fragment(struct sk_buff *skb, const struct ieee802154_hdr *wpan_hdr,
		     u8 *frag_hdr, int frag_hdrlen,
		     int offset, int len)
{
	struct sk_buff *frag;
251

P
Phoebe Buckheister 已提交
252
	raw_dump_inline(__func__, " fragment header", frag_hdr, frag_hdrlen);
253

P
Phoebe Buckheister 已提交
254 255 256
	frag = lowpan_alloc_frag(skb, frag_hdrlen + len, wpan_hdr);
	if (IS_ERR(frag))
		return -PTR_ERR(frag);
A
Alexander Aring 已提交
257

P
Phoebe Buckheister 已提交
258 259
	memcpy(skb_put(frag, frag_hdrlen), frag_hdr, frag_hdrlen);
	memcpy(skb_put(frag, len), skb_network_header(skb) + offset, len);
260

P
Phoebe Buckheister 已提交
261
	raw_dump_table(__func__, " fragment dump", frag->data, frag->len);
262

263
	return dev_queue_xmit(frag);
264 265 266
}

static int
P
Phoebe Buckheister 已提交
267 268
lowpan_xmit_fragmented(struct sk_buff *skb, struct net_device *dev,
		       const struct ieee802154_hdr *wpan_hdr)
269
{
P
Phoebe Buckheister 已提交
270 271 272 273 274 275
	u16 dgram_size, dgram_offset;
	__be16 frag_tag;
	u8 frag_hdr[5];
	int frag_cap, frag_len, payload_cap, rc;
	int skb_unprocessed, skb_offset;

276
	dgram_size = lowpan_uncompress_size(skb, &dgram_offset) -
P
Phoebe Buckheister 已提交
277 278
		     skb->mac_len;
	frag_tag = lowpan_dev_info(dev)->fragment_tag++;
279

P
Phoebe Buckheister 已提交
280 281 282
	frag_hdr[0] = LOWPAN_DISPATCH_FRAG1 | ((dgram_size >> 8) & 0x07);
	frag_hdr[1] = dgram_size & 0xff;
	memcpy(frag_hdr + 2, &frag_tag, sizeof(frag_tag));
283

P
Phoebe Buckheister 已提交
284
	payload_cap = ieee802154_max_payload(wpan_hdr);
285

P
Phoebe Buckheister 已提交
286 287
	frag_len = round_down(payload_cap - LOWPAN_FRAG1_HEAD_SIZE -
			      skb_network_header_len(skb), 8);
288

P
Phoebe Buckheister 已提交
289 290
	skb_offset = skb_network_header_len(skb);
	skb_unprocessed = skb->len - skb->mac_len - skb_offset;
291

P
Phoebe Buckheister 已提交
292 293 294 295 296 297 298 299
	rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
				  LOWPAN_FRAG1_HEAD_SIZE, 0,
				  frag_len + skb_network_header_len(skb));
	if (rc) {
		pr_debug("%s unable to send FRAG1 packet (tag: %d)",
			 __func__, frag_tag);
		goto err;
	}
300

P
Phoebe Buckheister 已提交
301 302 303
	frag_hdr[0] &= ~LOWPAN_DISPATCH_FRAG1;
	frag_hdr[0] |= LOWPAN_DISPATCH_FRAGN;
	frag_cap = round_down(payload_cap - LOWPAN_FRAGN_HEAD_SIZE, 8);
304

305
	do {
P
Phoebe Buckheister 已提交
306 307 308 309
		dgram_offset += frag_len;
		skb_offset += frag_len;
		skb_unprocessed -= frag_len;
		frag_len = min(frag_cap, skb_unprocessed);
310

P
Phoebe Buckheister 已提交
311
		frag_hdr[4] = dgram_offset >> 3;
312

P
Phoebe Buckheister 已提交
313 314 315 316
		rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
					  LOWPAN_FRAGN_HEAD_SIZE, skb_offset,
					  frag_len);
		if (rc) {
317
			pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
P
Phoebe Buckheister 已提交
318 319
				 __func__, frag_tag, skb_offset);
			goto err;
320
		}
321
	} while (skb_unprocessed > frag_cap);
322

P
Phoebe Buckheister 已提交
323 324 325 326 327 328
	consume_skb(skb);
	return NET_XMIT_SUCCESS;

err:
	kfree_skb(skb);
	return rc;
329 330
}

331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372
static int lowpan_header(struct sk_buff *skb, struct net_device *dev)
{
	struct ieee802154_addr sa, da;
	struct ieee802154_mac_cb *cb = mac_cb_init(skb);
	struct lowpan_addr_info info;
	void *daddr, *saddr;

	memcpy(&info, lowpan_skb_priv(skb), sizeof(info));

	/* TODO: Currently we only support extended_addr */
	daddr = &info.daddr.u.extended_addr;
	saddr = &info.saddr.u.extended_addr;

	lowpan_header_compress(skb, dev, ETH_P_IPV6, daddr, saddr, skb->len);

	cb->type = IEEE802154_FC_TYPE_DATA;

	/* prepare wpan address data */
	sa.mode = IEEE802154_ADDR_LONG;
	sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
	sa.extended_addr = ieee802154_devaddr_from_raw(saddr);

	/* intra-PAN communications */
	da.pan_id = sa.pan_id;

	/* if the destination address is the broadcast address, use the
	 * corresponding short address
	 */
	if (lowpan_is_addr_broadcast((const u8 *)daddr)) {
		da.mode = IEEE802154_ADDR_SHORT;
		da.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
		cb->ackreq = false;
	} else {
		da.mode = IEEE802154_ADDR_LONG;
		da.extended_addr = ieee802154_devaddr_from_raw(daddr);
		cb->ackreq = true;
	}

	return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
			ETH_P_IPV6, (void *)&da, (void *)&sa, 0);
}

373 374
static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
{
P
Phoebe Buckheister 已提交
375
	struct ieee802154_hdr wpan_hdr;
376
	int max_single, ret;
377

378
	pr_debug("package xmit\n");
379

380 381 382 383 384 385 386 387 388 389 390 391 392
	/* We must take a copy of the skb before we modify/replace the ipv6
	 * header as the header could be used elsewhere
	 */
	skb = skb_unshare(skb, GFP_ATOMIC);
	if (!skb)
		return NET_XMIT_DROP;

	ret = lowpan_header(skb, dev);
	if (ret < 0) {
		kfree_skb(skb);
		return NET_XMIT_DROP;
	}

P
Phoebe Buckheister 已提交
393 394 395
	if (ieee802154_hdr_peek(skb, &wpan_hdr) < 0) {
		kfree_skb(skb);
		return NET_XMIT_DROP;
396 397
	}

P
Phoebe Buckheister 已提交
398
	max_single = ieee802154_max_payload(&wpan_hdr);
399

P
Phoebe Buckheister 已提交
400 401 402 403 404
	if (skb_tail_pointer(skb) - skb_network_header(skb) <= max_single) {
		skb->dev = lowpan_dev_info(dev)->real_dev;
		return dev_queue_xmit(skb);
	} else {
		netdev_tx_t rc;
405

P
Phoebe Buckheister 已提交
406 407 408 409 410
		pr_debug("frame is too big, fragmentation is needed\n");
		rc = lowpan_xmit_fragmented(skb, dev, &wpan_hdr);

		return rc < 0 ? NET_XMIT_DROP : rc;
	}
411 412
}

413 414 415
static struct wpan_phy *lowpan_get_phy(const struct net_device *dev)
{
	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
416

417 418 419
	return ieee802154_mlme_ops(real_dev)->get_phy(real_dev);
}

420
static __le16 lowpan_get_pan_id(const struct net_device *dev)
421 422
{
	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
423

424 425 426
	return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
}

427
static __le16 lowpan_get_short_addr(const struct net_device *dev)
428 429
{
	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
430

431 432 433
	return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
}

434 435 436
static u8 lowpan_get_dsn(const struct net_device *dev)
{
	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
437

438 439 440
	return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
}

441 442 443 444
static struct header_ops lowpan_header_ops = {
	.create	= lowpan_header_create,
};

E
Eric Dumazet 已提交
445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463
static struct lock_class_key lowpan_tx_busylock;
static struct lock_class_key lowpan_netdev_xmit_lock_key;

static void lowpan_set_lockdep_class_one(struct net_device *dev,
					 struct netdev_queue *txq,
					 void *_unused)
{
	lockdep_set_class(&txq->_xmit_lock,
			  &lowpan_netdev_xmit_lock_key);
}


static int lowpan_dev_init(struct net_device *dev)
{
	netdev_for_each_tx_queue(dev, lowpan_set_lockdep_class_one, NULL);
	dev->qdisc_tx_busylock = &lowpan_tx_busylock;
	return 0;
}

464
static const struct net_device_ops lowpan_netdev_ops = {
E
Eric Dumazet 已提交
465
	.ndo_init		= lowpan_dev_init,
466
	.ndo_start_xmit		= lowpan_xmit,
467
	.ndo_set_mac_address	= lowpan_set_address,
468 469
};

470 471 472 473
static struct ieee802154_mlme_ops lowpan_mlme = {
	.get_pan_id = lowpan_get_pan_id,
	.get_phy = lowpan_get_phy,
	.get_short_addr = lowpan_get_short_addr,
474
	.get_dsn = lowpan_get_dsn,
475 476
};

477 478 479 480 481 482 483 484
static void lowpan_setup(struct net_device *dev)
{
	dev->addr_len		= IEEE802154_ADDR_LEN;
	memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
	dev->type		= ARPHRD_IEEE802154;
	/* Frame Control + Sequence Number + Address fields + Security Header */
	dev->hard_header_len	= 2 + 1 + 20 + 14;
	dev->needed_tailroom	= 2; /* FCS */
485
	dev->mtu		= IPV6_MIN_MTU;
486
	dev->tx_queue_len	= 0;
487
	dev->flags		= IFF_BROADCAST | IFF_MULTICAST;
488 489 490 491
	dev->watchdog_timeo	= 0;

	dev->netdev_ops		= &lowpan_netdev_ops;
	dev->header_ops		= &lowpan_header_ops;
492
	dev->ml_priv		= &lowpan_mlme;
493
	dev->destructor		= free_netdev;
494 495 496 497 498 499 500 501 502 503 504 505
}

static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
{
	if (tb[IFLA_ADDRESS]) {
		if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
			return -EINVAL;
	}
	return 0;
}

static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
506
		      struct packet_type *pt, struct net_device *orig_dev)
507
{
508
	struct ieee802154_hdr hdr;
509
	int ret;
510

511 512 513 514
	skb = skb_share_check(skb, GFP_ATOMIC);
	if (!skb)
		goto drop;

515
	if (!netif_running(dev))
516
		goto drop_skb;
517

518 519 520
	if (skb->pkt_type == PACKET_OTHERHOST)
		goto drop_skb;

521
	if (dev->type != ARPHRD_IEEE802154)
522 523
		goto drop_skb;

524 525 526
	if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
		goto drop_skb;

527
	/* check that it's our buffer */
528
	if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
529 530
		skb->protocol = htons(ETH_P_IPV6);
		skb->pkt_type = PACKET_HOST;
531 532

		/* Pull off the 1-byte of 6lowpan header. */
533
		skb_pull(skb, 1);
534

535
		ret = lowpan_give_skb_to_devices(skb, NULL);
536 537
		if (ret == NET_RX_DROP)
			goto drop;
538 539 540
	} else {
		switch (skb->data[0] & 0xe0) {
		case LOWPAN_DISPATCH_IPHC:	/* ipv6 datagram */
541
			ret = process_data(skb, &hdr);
542 543 544
			if (ret == NET_RX_DROP)
				goto drop;
			break;
545
		case LOWPAN_DISPATCH_FRAG1:	/* first fragment header */
546
			ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
547
			if (ret == 1) {
548
				ret = process_data(skb, &hdr);
549 550 551 552
				if (ret == NET_RX_DROP)
					goto drop;
			}
			break;
553
		case LOWPAN_DISPATCH_FRAGN:	/* next fragments headers */
554
			ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
555
			if (ret == 1) {
556
				ret = process_data(skb, &hdr);
557 558 559
				if (ret == NET_RX_DROP)
					goto drop;
			}
560 561 562 563
			break;
		default:
			break;
		}
564
	}
565 566

	return NET_RX_SUCCESS;
567
drop_skb:
568
	kfree_skb(skb);
569
drop:
570 571 572 573 574 575 576 577 578
	return NET_RX_DROP;
}

static int lowpan_newlink(struct net *src_net, struct net_device *dev,
			  struct nlattr *tb[], struct nlattr *data[])
{
	struct net_device *real_dev;
	struct lowpan_dev_record *entry;

579
	pr_debug("adding new link\n");
580 581 582 583 584 585 586

	if (!tb[IFLA_LINK])
		return -EINVAL;
	/* find and hold real wpan device */
	real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
	if (!real_dev)
		return -ENODEV;
587 588
	if (real_dev->type != ARPHRD_IEEE802154) {
		dev_put(real_dev);
589
		return -EINVAL;
590
	}
591 592 593 594

	lowpan_dev_info(dev)->real_dev = real_dev;
	mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);

595
	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
596 597 598
	if (!entry) {
		dev_put(real_dev);
		lowpan_dev_info(dev)->real_dev = NULL;
599
		return -ENOMEM;
600
	}
601 602 603

	entry->ldev = dev;

604 605 606
	/* Set the lowpan harware address to the wpan hardware address. */
	memcpy(dev->dev_addr, real_dev->dev_addr, IEEE802154_ADDR_LEN);

607 608 609 610 611 612 613 614 615 616 617 618 619 620
	mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
	INIT_LIST_HEAD(&entry->list);
	list_add_tail(&entry->list, &lowpan_devices);
	mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);

	register_netdevice(dev);

	return 0;
}

static void lowpan_dellink(struct net_device *dev, struct list_head *head)
{
	struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
	struct net_device *real_dev = lowpan_dev->real_dev;
621
	struct lowpan_dev_record *entry, *tmp;
622 623 624 625

	ASSERT_RTNL();

	mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
626
	list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
627 628 629 630
		if (entry->ldev == dev) {
			list_del(&entry->list);
			kfree(entry);
		}
631
	}
632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654
	mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);

	mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);

	unregister_netdevice_queue(dev, head);

	dev_put(real_dev);
}

static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
	.kind		= "lowpan",
	.priv_size	= sizeof(struct lowpan_dev_info),
	.setup		= lowpan_setup,
	.newlink	= lowpan_newlink,
	.dellink	= lowpan_dellink,
	.validate	= lowpan_validate,
};

static inline int __init lowpan_netlink_init(void)
{
	return rtnl_link_register(&lowpan_link_ops);
}

655
static inline void lowpan_netlink_fini(void)
656 657 658 659
{
	rtnl_link_unregister(&lowpan_link_ops);
}

660
static int lowpan_device_event(struct notifier_block *unused,
661
			       unsigned long event, void *ptr)
662
{
663
	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
664 665 666 667 668 669 670 671 672 673 674 675 676
	LIST_HEAD(del_list);
	struct lowpan_dev_record *entry, *tmp;

	if (dev->type != ARPHRD_IEEE802154)
		goto out;

	if (event == NETDEV_UNREGISTER) {
		list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
			if (lowpan_dev_info(entry->ldev)->real_dev == dev)
				lowpan_dellink(entry->ldev, &del_list);
		}

		unregister_netdevice_many(&del_list);
677
	}
678 679 680 681 682 683 684 685 686

out:
	return NOTIFY_DONE;
}

static struct notifier_block lowpan_dev_notifier = {
	.notifier_call = lowpan_device_event,
};

687
static struct packet_type lowpan_packet_type = {
688
	.type = htons(ETH_P_IEEE802154),
689 690 691 692 693 694 695
	.func = lowpan_rcv,
};

static int __init lowpan_init_module(void)
{
	int err = 0;

696
	err = lowpan_net_frag_init();
697 698 699
	if (err < 0)
		goto out;

700 701 702 703
	err = lowpan_netlink_init();
	if (err < 0)
		goto out_frag;

704
	dev_add_pack(&lowpan_packet_type);
705 706

	err = register_netdevice_notifier(&lowpan_dev_notifier);
707 708 709 710 711 712 713 714 715 716
	if (err < 0)
		goto out_pack;

	return 0;

out_pack:
	dev_remove_pack(&lowpan_packet_type);
	lowpan_netlink_fini();
out_frag:
	lowpan_net_frag_exit();
717 718 719 720 721 722 723 724 725
out:
	return err;
}

static void __exit lowpan_cleanup_module(void)
{
	lowpan_netlink_fini();

	dev_remove_pack(&lowpan_packet_type);
726

727
	lowpan_net_frag_exit();
728

729
	unregister_netdevice_notifier(&lowpan_dev_notifier);
730 731 732 733 734 735
}

module_init(lowpan_init_module);
module_exit(lowpan_cleanup_module);
MODULE_LICENSE("GPL");
MODULE_ALIAS_RTNL_LINK("lowpan");