6lowpan_rtnl.c 18.5 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
 * 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>
52
#include <linux/ieee802154.h>
53 54
#include <net/af_ieee802154.h>
#include <net/ieee802154_netdev.h>
55
#include <net/6lowpan.h>
56 57
#include <net/ipv6.h>

58
#include "reassembly.h"
59 60

static LIST_HEAD(lowpan_devices);
61
static int lowpan_open_count;
62 63 64 65 66

/* 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 */
67
	u16			fragment_tag;
68 69 70 71 72 73 74
};

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

75 76 77 78 79 80 81 82 83 84 85 86 87 88 89
/* 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;
};

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

96 97 98 99 100 101 102 103
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));
}

104 105 106
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)
107 108 109
{
	const u8 *saddr = _saddr;
	const u8 *daddr = _daddr;
110
	struct lowpan_addr_info *info;
111

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

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

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

124
	info = lowpan_skb_priv(skb);
125

126 127 128 129 130 131 132
	/* 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));
133

134
	return 0;
135 136
}

137
static int lowpan_give_skb_to_devices(struct sk_buff *skb,
138
				      struct net_device *dev)
139 140 141 142 143
{
	struct lowpan_dev_record *entry;
	struct sk_buff *skb_cp;
	int stat = NET_RX_SUCCESS;

144 145 146
	skb->protocol = htons(ETH_P_IPV6);
	skb->pkt_type = PACKET_HOST;

147 148 149 150 151
	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) {
152 153 154
				kfree_skb(skb);
				rcu_read_unlock();
				return NET_RX_DROP;
155 156 157 158
			}

			skb_cp->dev = entry->ldev;
			stat = netif_rx(skb_cp);
159 160
			if (stat == NET_RX_DROP)
				break;
161 162 163
		}
	rcu_read_unlock();

164 165
	consume_skb(skb);

166 167 168
	return stat;
}

169 170
static int
iphc_decompress(struct sk_buff *skb, const struct ieee802154_hdr *hdr)
171
{
172
	u8 iphc0, iphc1;
173 174
	struct ieee802154_addr_sa sa, da;
	void *sap, *dap;
175

176
	raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
177 178 179
	/* at least two bytes will be used for the encoding */
	if (skb->len < 2)
		goto drop;
180 181 182

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

184 185
	if (lowpan_fetch_skb_u8(skb, &iphc1))
		goto drop;
186

187 188
	ieee802154_addr_to_sa(&sa, &hdr->source);
	ieee802154_addr_to_sa(&da, &hdr->dest);
189

190 191 192 193 194 195 196 197 198 199
	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;

200 201 202
	return lowpan_header_decompress(skb, skb->dev, sap, sa.addr_type,
					IEEE802154_ADDR_LEN, dap, da.addr_type,
					IEEE802154_ADDR_LEN, iphc0, iphc1);
203

204
drop:
205
	kfree_skb(skb);
206 207 208
	return -EINVAL;
}

P
Phoebe Buckheister 已提交
209 210 211
static struct sk_buff*
lowpan_alloc_frag(struct sk_buff *skb, int size,
		  const struct ieee802154_hdr *master_hdr)
212
{
P
Phoebe Buckheister 已提交
213
	struct net_device *real_dev = lowpan_dev_info(skb->dev)->real_dev;
214
	struct sk_buff *frag;
P
Phoebe Buckheister 已提交
215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231
	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);
232
			return ERR_PTR(rc);
P
Phoebe Buckheister 已提交
233 234
		}
	} else {
235
		frag = ERR_PTR(-ENOMEM);
P
Phoebe Buckheister 已提交
236
	}
237

P
Phoebe Buckheister 已提交
238 239
	return frag;
}
240

P
Phoebe Buckheister 已提交
241 242 243 244 245 246
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;
247

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

P
Phoebe Buckheister 已提交
250 251 252
	frag = lowpan_alloc_frag(skb, frag_hdrlen + len, wpan_hdr);
	if (IS_ERR(frag))
		return -PTR_ERR(frag);
A
Alexander Aring 已提交
253

P
Phoebe Buckheister 已提交
254 255
	memcpy(skb_put(frag, frag_hdrlen), frag_hdr, frag_hdrlen);
	memcpy(skb_put(frag, len), skb_network_header(skb) + offset, len);
256

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

259
	return dev_queue_xmit(frag);
260 261 262
}

static int
P
Phoebe Buckheister 已提交
263 264
lowpan_xmit_fragmented(struct sk_buff *skb, struct net_device *dev,
		       const struct ieee802154_hdr *wpan_hdr)
265
{
P
Phoebe Buckheister 已提交
266 267 268 269 270 271
	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;

272
	dgram_size = lowpan_uncompress_size(skb, &dgram_offset) -
P
Phoebe Buckheister 已提交
273
		     skb->mac_len;
274 275
	frag_tag = htons(lowpan_dev_info(dev)->fragment_tag);
	lowpan_dev_info(dev)->fragment_tag++;
276

P
Phoebe Buckheister 已提交
277 278 279
	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));
280

P
Phoebe Buckheister 已提交
281
	payload_cap = ieee802154_max_payload(wpan_hdr);
282

P
Phoebe Buckheister 已提交
283 284
	frag_len = round_down(payload_cap - LOWPAN_FRAG1_HEAD_SIZE -
			      skb_network_header_len(skb), 8);
285

P
Phoebe Buckheister 已提交
286 287
	skb_offset = skb_network_header_len(skb);
	skb_unprocessed = skb->len - skb->mac_len - skb_offset;
288

P
Phoebe Buckheister 已提交
289 290 291 292 293
	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)",
294
			 __func__, ntohs(frag_tag));
P
Phoebe Buckheister 已提交
295 296
		goto err;
	}
297

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

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

P
Phoebe Buckheister 已提交
308
		frag_hdr[4] = dgram_offset >> 3;
309

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

P
Phoebe Buckheister 已提交
320 321 322 323 324 325
	consume_skb(skb);
	return NET_XMIT_SUCCESS;

err:
	kfree_skb(skb);
	return rc;
326 327
}

328 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
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);
}

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

375
	pr_debug("package xmit\n");
376

377 378 379 380 381 382 383 384 385 386 387 388 389
	/* 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 已提交
390 391 392
	if (ieee802154_hdr_peek(skb, &wpan_hdr) < 0) {
		kfree_skb(skb);
		return NET_XMIT_DROP;
393 394
	}

P
Phoebe Buckheister 已提交
395
	max_single = ieee802154_max_payload(&wpan_hdr);
396

P
Phoebe Buckheister 已提交
397 398 399 400 401
	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;
402

P
Phoebe Buckheister 已提交
403 404 405 406 407
		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;
	}
408 409
}

410
static __le16 lowpan_get_pan_id(const struct net_device *dev)
411 412
{
	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
413

414 415 416
	return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
}

417
static __le16 lowpan_get_short_addr(const struct net_device *dev)
418 419
{
	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
420

421 422 423
	return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
}

424 425 426
static u8 lowpan_get_dsn(const struct net_device *dev)
{
	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
427

428 429 430
	return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
}

431 432 433 434
static struct header_ops lowpan_header_ops = {
	.create	= lowpan_header_create,
};

E
Eric Dumazet 已提交
435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453
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;
}

454
static const struct net_device_ops lowpan_netdev_ops = {
E
Eric Dumazet 已提交
455
	.ndo_init		= lowpan_dev_init,
456 457 458
	.ndo_start_xmit		= lowpan_xmit,
};

459 460 461
static struct ieee802154_mlme_ops lowpan_mlme = {
	.get_pan_id = lowpan_get_pan_id,
	.get_short_addr = lowpan_get_short_addr,
462
	.get_dsn = lowpan_get_dsn,
463 464
};

465 466 467 468 469 470 471 472
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 */
473
	dev->mtu		= IPV6_MIN_MTU;
474
	dev->tx_queue_len	= 0;
475
	dev->flags		= IFF_BROADCAST | IFF_MULTICAST;
476 477 478 479
	dev->watchdog_timeo	= 0;

	dev->netdev_ops		= &lowpan_netdev_ops;
	dev->header_ops		= &lowpan_header_ops;
480
	dev->ml_priv		= &lowpan_mlme;
481
	dev->destructor		= free_netdev;
482 483 484 485 486 487 488 489 490 491 492 493
}

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,
494
		      struct packet_type *pt, struct net_device *orig_dev)
495
{
496
	struct ieee802154_hdr hdr;
497
	int ret;
498

499 500 501 502
	skb = skb_share_check(skb, GFP_ATOMIC);
	if (!skb)
		goto drop;

503
	if (!netif_running(dev))
504
		goto drop_skb;
505

506 507 508
	if (skb->pkt_type == PACKET_OTHERHOST)
		goto drop_skb;

509
	if (dev->type != ARPHRD_IEEE802154)
510 511
		goto drop_skb;

512 513 514
	if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
		goto drop_skb;

515
	/* check that it's our buffer */
516 517
	if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
		/* Pull off the 1-byte of 6lowpan header. */
518
		skb_pull(skb, 1);
519
		return lowpan_give_skb_to_devices(skb, NULL);
520 521 522
	} else {
		switch (skb->data[0] & 0xe0) {
		case LOWPAN_DISPATCH_IPHC:	/* ipv6 datagram */
523
			ret = iphc_decompress(skb, &hdr);
524
			if (ret < 0)
525
				goto drop;
526 527

			return lowpan_give_skb_to_devices(skb, NULL);
528
		case LOWPAN_DISPATCH_FRAG1:	/* first fragment header */
529
			ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
530
			if (ret == 1) {
531
				ret = iphc_decompress(skb, &hdr);
532
				if (ret < 0)
533
					goto drop;
534 535 536 537 538 539

				return lowpan_give_skb_to_devices(skb, NULL);
			} else if (ret == -1) {
				return NET_RX_DROP;
			} else {
				return NET_RX_SUCCESS;
540
			}
541
		case LOWPAN_DISPATCH_FRAGN:	/* next fragments headers */
542
			ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
543
			if (ret == 1) {
544
				ret = iphc_decompress(skb, &hdr);
545
				if (ret < 0)
546
					goto drop;
547 548 549 550 551 552

				return lowpan_give_skb_to_devices(skb, NULL);
			} else if (ret == -1) {
				return NET_RX_DROP;
			} else {
				return NET_RX_SUCCESS;
553
			}
554 555 556
		default:
			break;
		}
557
	}
558

559
drop_skb:
560
	kfree_skb(skb);
561
drop:
562 563 564
	return NET_RX_DROP;
}

565 566 567 568 569
static struct packet_type lowpan_packet_type = {
	.type = htons(ETH_P_IEEE802154),
	.func = lowpan_rcv,
};

570 571 572 573 574
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;
575
	int ret;
576

577 578
	ASSERT_RTNL();

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
	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);

612 613 614 615 616 617
	ret = register_netdevice(dev);
	if (ret >= 0) {
		if (!lowpan_open_count)
			dev_add_pack(&lowpan_packet_type);
		lowpan_open_count++;
	}
618

619
	return ret;
620 621 622 623 624 625
}

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;
626
	struct lowpan_dev_record *entry, *tmp;
627 628 629

	ASSERT_RTNL();

630 631 632 633
	lowpan_open_count--;
	if (!lowpan_open_count)
		dev_remove_pack(&lowpan_packet_type);

634
	mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
635
	list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
636 637 638 639
		if (entry->ldev == dev) {
			list_del(&entry->list);
			kfree(entry);
		}
640
	}
641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663
	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);
}

664
static inline void lowpan_netlink_fini(void)
665 666 667 668
{
	rtnl_link_unregister(&lowpan_link_ops);
}

669
static int lowpan_device_event(struct notifier_block *unused,
670
			       unsigned long event, void *ptr)
671
{
672
	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
673 674 675 676 677 678 679 680 681 682 683 684 685
	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);
686
	}
687 688 689 690 691 692 693 694 695

out:
	return NOTIFY_DONE;
}

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

696 697 698 699
static int __init lowpan_init_module(void)
{
	int err = 0;

700
	err = lowpan_net_frag_init();
701 702 703
	if (err < 0)
		goto out;

704 705 706 707
	err = lowpan_netlink_init();
	if (err < 0)
		goto out_frag;

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

	return 0;

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

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

726
	lowpan_net_frag_exit();
727

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

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