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 "6lowpan_i.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
	/* at least two bytes will be used for the encoding */
	if (skb->len < 2)
179
		return -EINVAL;
180 181

	if (lowpan_fetch_skb_u8(skb, &iphc0))
182
		return -EINVAL;
183

184
	if (lowpan_fetch_skb_u8(skb, &iphc1))
185
		return -EINVAL;
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
}

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

P
Phoebe Buckheister 已提交
234 235
	return frag;
}
236

P
Phoebe Buckheister 已提交
237 238 239 240 241 242
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;
243

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

P
Phoebe Buckheister 已提交
246 247 248
	frag = lowpan_alloc_frag(skb, frag_hdrlen + len, wpan_hdr);
	if (IS_ERR(frag))
		return -PTR_ERR(frag);
A
Alexander Aring 已提交
249

P
Phoebe Buckheister 已提交
250 251
	memcpy(skb_put(frag, frag_hdrlen), frag_hdr, frag_hdrlen);
	memcpy(skb_put(frag, len), skb_network_header(skb) + offset, len);
252

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

255
	return dev_queue_xmit(frag);
256 257 258
}

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

268
	dgram_size = lowpan_uncompress_size(skb, &dgram_offset) -
P
Phoebe Buckheister 已提交
269
		     skb->mac_len;
270 271
	frag_tag = htons(lowpan_dev_info(dev)->fragment_tag);
	lowpan_dev_info(dev)->fragment_tag++;
272

P
Phoebe Buckheister 已提交
273 274 275
	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));
276

P
Phoebe Buckheister 已提交
277
	payload_cap = ieee802154_max_payload(wpan_hdr);
278

P
Phoebe Buckheister 已提交
279 280
	frag_len = round_down(payload_cap - LOWPAN_FRAG1_HEAD_SIZE -
			      skb_network_header_len(skb), 8);
281

P
Phoebe Buckheister 已提交
282 283
	skb_offset = skb_network_header_len(skb);
	skb_unprocessed = skb->len - skb->mac_len - skb_offset;
284

P
Phoebe Buckheister 已提交
285 286 287 288 289
	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)",
290
			 __func__, ntohs(frag_tag));
P
Phoebe Buckheister 已提交
291 292
		goto err;
	}
293

P
Phoebe Buckheister 已提交
294 295 296
	frag_hdr[0] &= ~LOWPAN_DISPATCH_FRAG1;
	frag_hdr[0] |= LOWPAN_DISPATCH_FRAGN;
	frag_cap = round_down(payload_cap - LOWPAN_FRAGN_HEAD_SIZE, 8);
297

298
	do {
P
Phoebe Buckheister 已提交
299 300 301 302
		dgram_offset += frag_len;
		skb_offset += frag_len;
		skb_unprocessed -= frag_len;
		frag_len = min(frag_cap, skb_unprocessed);
303

P
Phoebe Buckheister 已提交
304
		frag_hdr[4] = dgram_offset >> 3;
305

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

P
Phoebe Buckheister 已提交
316 317 318 319 320 321
	consume_skb(skb);
	return NET_XMIT_SUCCESS;

err:
	kfree_skb(skb);
	return rc;
322 323
}

324 325 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
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);
}

366 367
static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
{
P
Phoebe Buckheister 已提交
368
	struct ieee802154_hdr wpan_hdr;
369
	int max_single, ret;
370

371
	pr_debug("package xmit\n");
372

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

P
Phoebe Buckheister 已提交
391
	max_single = ieee802154_max_payload(&wpan_hdr);
392

P
Phoebe Buckheister 已提交
393 394 395 396 397
	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;
398

P
Phoebe Buckheister 已提交
399 400 401 402 403
		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;
	}
404 405
}

406
static __le16 lowpan_get_pan_id(const struct net_device *dev)
407 408
{
	struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
409

410 411 412
	return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
}

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

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

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

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

427 428 429 430
static struct header_ops lowpan_header_ops = {
	.create	= lowpan_header_create,
};

E
Eric Dumazet 已提交
431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448
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;
}

449
static const struct net_device_ops lowpan_netdev_ops = {
E
Eric Dumazet 已提交
450
	.ndo_init		= lowpan_dev_init,
451 452 453
	.ndo_start_xmit		= lowpan_xmit,
};

454 455 456
static struct ieee802154_mlme_ops lowpan_mlme = {
	.get_pan_id = lowpan_get_pan_id,
	.get_short_addr = lowpan_get_short_addr,
457
	.get_dsn = lowpan_get_dsn,
458 459
};

460 461 462 463 464 465 466 467
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 */
468
	dev->mtu		= IPV6_MIN_MTU;
469
	dev->tx_queue_len	= 0;
470
	dev->flags		= IFF_BROADCAST | IFF_MULTICAST;
471 472 473 474
	dev->watchdog_timeo	= 0;

	dev->netdev_ops		= &lowpan_netdev_ops;
	dev->header_ops		= &lowpan_header_ops;
475
	dev->ml_priv		= &lowpan_mlme;
476
	dev->destructor		= free_netdev;
477 478 479 480 481 482 483 484 485 486 487 488
}

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,
489
		      struct packet_type *pt, struct net_device *orig_dev)
490
{
491
	struct ieee802154_hdr hdr;
492
	int ret;
493

494 495 496 497
	skb = skb_share_check(skb, GFP_ATOMIC);
	if (!skb)
		goto drop;

498
	if (!netif_running(dev))
499
		goto drop_skb;
500

501 502 503
	if (skb->pkt_type == PACKET_OTHERHOST)
		goto drop_skb;

504
	if (dev->type != ARPHRD_IEEE802154)
505 506
		goto drop_skb;

507 508 509
	if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
		goto drop_skb;

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

			return lowpan_give_skb_to_devices(skb, NULL);
523
		case LOWPAN_DISPATCH_FRAG1:	/* first fragment header */
524
			ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
525
			if (ret == 1) {
526
				ret = iphc_decompress(skb, &hdr);
527
				if (ret < 0)
528
					goto drop_skb;
529 530 531 532 533 534

				return lowpan_give_skb_to_devices(skb, NULL);
			} else if (ret == -1) {
				return NET_RX_DROP;
			} else {
				return NET_RX_SUCCESS;
535
			}
536
		case LOWPAN_DISPATCH_FRAGN:	/* next fragments headers */
537
			ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
538
			if (ret == 1) {
539
				ret = iphc_decompress(skb, &hdr);
540
				if (ret < 0)
541
					goto drop_skb;
542 543 544 545 546 547

				return lowpan_give_skb_to_devices(skb, NULL);
			} else if (ret == -1) {
				return NET_RX_DROP;
			} else {
				return NET_RX_SUCCESS;
548
			}
549 550 551
		default:
			break;
		}
552
	}
553

554
drop_skb:
555
	kfree_skb(skb);
556
drop:
557 558 559
	return NET_RX_DROP;
}

560 561 562 563 564
static struct packet_type lowpan_packet_type = {
	.type = htons(ETH_P_IEEE802154),
	.func = lowpan_rcv,
};

565 566 567 568 569
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;
570
	int ret;
571

572 573
	ASSERT_RTNL();

574
	pr_debug("adding new link\n");
575 576 577 578 579 580 581

	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;
582 583
	if (real_dev->type != ARPHRD_IEEE802154) {
		dev_put(real_dev);
584
		return -EINVAL;
585
	}
586 587 588 589

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

590
	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
591 592 593
	if (!entry) {
		dev_put(real_dev);
		lowpan_dev_info(dev)->real_dev = NULL;
594
		return -ENOMEM;
595
	}
596 597 598

	entry->ldev = dev;

V
Varka Bhadram 已提交
599
	/* Set the lowpan hardware address to the wpan hardware address. */
600 601
	memcpy(dev->dev_addr, real_dev->dev_addr, IEEE802154_ADDR_LEN);

602 603 604 605 606
	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);

607 608 609 610 611 612
	ret = register_netdevice(dev);
	if (ret >= 0) {
		if (!lowpan_open_count)
			dev_add_pack(&lowpan_packet_type);
		lowpan_open_count++;
	}
613

614
	return ret;
615 616 617 618 619 620
}

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

	ASSERT_RTNL();

625 626 627 628
	lowpan_open_count--;
	if (!lowpan_open_count)
		dev_remove_pack(&lowpan_packet_type);

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

659
static inline void lowpan_netlink_fini(void)
660 661 662 663
{
	rtnl_link_unregister(&lowpan_link_ops);
}

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

out:
	return NOTIFY_DONE;
}

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

691 692 693 694
static int __init lowpan_init_module(void)
{
	int err = 0;

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

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

703
	err = register_netdevice_notifier(&lowpan_dev_notifier);
704 705 706 707 708 709 710 711 712
	if (err < 0)
		goto out_pack;

	return 0;

out_pack:
	lowpan_netlink_fini();
out_frag:
	lowpan_net_frag_exit();
713 714 715 716 717 718 719 720
out:
	return err;
}

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

721
	lowpan_net_frag_exit();
722

723
	unregister_netdevice_notifier(&lowpan_dev_notifier);
724 725 726 727 728 729
}

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