6lowpan_rtnl.c 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730
  1. /* Copyright 2011, Siemens AG
  2. * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
  3. */
  4. /* Based on patches from Jon Smirl <jonsmirl@gmail.com>
  5. * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
  6. *
  7. * This program is free software; you can redistribute it and/or modify
  8. * it under the terms of the GNU General Public License version 2
  9. * as published by the Free Software Foundation.
  10. *
  11. * This program is distributed in the hope that it will be useful,
  12. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  14. * GNU General Public License for more details.
  15. */
  16. /* Jon's code is based on 6lowpan implementation for Contiki which is:
  17. * Copyright (c) 2008, Swedish Institute of Computer Science.
  18. * All rights reserved.
  19. *
  20. * Redistribution and use in source and binary forms, with or without
  21. * modification, are permitted provided that the following conditions
  22. * are met:
  23. * 1. Redistributions of source code must retain the above copyright
  24. * notice, this list of conditions and the following disclaimer.
  25. * 2. Redistributions in binary form must reproduce the above copyright
  26. * notice, this list of conditions and the following disclaimer in the
  27. * documentation and/or other materials provided with the distribution.
  28. * 3. Neither the name of the Institute nor the names of its contributors
  29. * may be used to endorse or promote products derived from this software
  30. * without specific prior written permission.
  31. *
  32. * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
  33. * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  34. * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  35. * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
  36. * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  37. * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  38. * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  39. * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  40. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  41. * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  42. * SUCH DAMAGE.
  43. */
  44. #include <linux/bitops.h>
  45. #include <linux/if_arp.h>
  46. #include <linux/module.h>
  47. #include <linux/moduleparam.h>
  48. #include <linux/netdevice.h>
  49. #include <linux/ieee802154.h>
  50. #include <net/af_ieee802154.h>
  51. #include <net/ieee802154_netdev.h>
  52. #include <net/6lowpan.h>
  53. #include <net/ipv6.h>
  54. #include "reassembly.h"
  55. static LIST_HEAD(lowpan_devices);
  56. static int lowpan_open_count;
  57. /* private device info */
  58. struct lowpan_dev_info {
  59. struct net_device *real_dev; /* real WPAN device ptr */
  60. struct mutex dev_list_mtx; /* mutex for list ops */
  61. u16 fragment_tag;
  62. };
  63. struct lowpan_dev_record {
  64. struct net_device *ldev;
  65. struct list_head list;
  66. };
  67. /* don't save pan id, it's intra pan */
  68. struct lowpan_addr {
  69. u8 mode;
  70. union {
  71. /* IPv6 needs big endian here */
  72. __be64 extended_addr;
  73. __be16 short_addr;
  74. } u;
  75. };
  76. struct lowpan_addr_info {
  77. struct lowpan_addr daddr;
  78. struct lowpan_addr saddr;
  79. };
  80. static inline struct
  81. lowpan_dev_info *lowpan_dev_info(const struct net_device *dev)
  82. {
  83. return netdev_priv(dev);
  84. }
  85. static inline struct
  86. lowpan_addr_info *lowpan_skb_priv(const struct sk_buff *skb)
  87. {
  88. WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct lowpan_addr_info));
  89. return (struct lowpan_addr_info *)(skb->data -
  90. sizeof(struct lowpan_addr_info));
  91. }
  92. static int lowpan_header_create(struct sk_buff *skb, struct net_device *dev,
  93. unsigned short type, const void *_daddr,
  94. const void *_saddr, unsigned int len)
  95. {
  96. const u8 *saddr = _saddr;
  97. const u8 *daddr = _daddr;
  98. struct lowpan_addr_info *info;
  99. /* TODO:
  100. * if this package isn't ipv6 one, where should it be routed?
  101. */
  102. if (type != ETH_P_IPV6)
  103. return 0;
  104. if (!saddr)
  105. saddr = dev->dev_addr;
  106. raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
  107. raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
  108. info = lowpan_skb_priv(skb);
  109. /* TODO: Currently we only support extended_addr */
  110. info->daddr.mode = IEEE802154_ADDR_LONG;
  111. memcpy(&info->daddr.u.extended_addr, daddr,
  112. sizeof(info->daddr.u.extended_addr));
  113. info->saddr.mode = IEEE802154_ADDR_LONG;
  114. memcpy(&info->saddr.u.extended_addr, saddr,
  115. sizeof(info->daddr.u.extended_addr));
  116. return 0;
  117. }
  118. static int lowpan_give_skb_to_devices(struct sk_buff *skb,
  119. struct net_device *dev)
  120. {
  121. struct lowpan_dev_record *entry;
  122. struct sk_buff *skb_cp;
  123. int stat = NET_RX_SUCCESS;
  124. skb->protocol = htons(ETH_P_IPV6);
  125. skb->pkt_type = PACKET_HOST;
  126. rcu_read_lock();
  127. list_for_each_entry_rcu(entry, &lowpan_devices, list)
  128. if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
  129. skb_cp = skb_copy(skb, GFP_ATOMIC);
  130. if (!skb_cp) {
  131. kfree_skb(skb);
  132. rcu_read_unlock();
  133. return NET_RX_DROP;
  134. }
  135. skb_cp->dev = entry->ldev;
  136. stat = netif_rx(skb_cp);
  137. if (stat == NET_RX_DROP)
  138. break;
  139. }
  140. rcu_read_unlock();
  141. consume_skb(skb);
  142. return stat;
  143. }
  144. static int
  145. iphc_decompress(struct sk_buff *skb, const struct ieee802154_hdr *hdr)
  146. {
  147. u8 iphc0, iphc1;
  148. struct ieee802154_addr_sa sa, da;
  149. void *sap, *dap;
  150. raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
  151. /* at least two bytes will be used for the encoding */
  152. if (skb->len < 2)
  153. return -EINVAL;
  154. if (lowpan_fetch_skb_u8(skb, &iphc0))
  155. return -EINVAL;
  156. if (lowpan_fetch_skb_u8(skb, &iphc1))
  157. return -EINVAL;
  158. ieee802154_addr_to_sa(&sa, &hdr->source);
  159. ieee802154_addr_to_sa(&da, &hdr->dest);
  160. if (sa.addr_type == IEEE802154_ADDR_SHORT)
  161. sap = &sa.short_addr;
  162. else
  163. sap = &sa.hwaddr;
  164. if (da.addr_type == IEEE802154_ADDR_SHORT)
  165. dap = &da.short_addr;
  166. else
  167. dap = &da.hwaddr;
  168. return lowpan_header_decompress(skb, skb->dev, sap, sa.addr_type,
  169. IEEE802154_ADDR_LEN, dap, da.addr_type,
  170. IEEE802154_ADDR_LEN, iphc0, iphc1);
  171. }
  172. static struct sk_buff*
  173. lowpan_alloc_frag(struct sk_buff *skb, int size,
  174. const struct ieee802154_hdr *master_hdr)
  175. {
  176. struct net_device *real_dev = lowpan_dev_info(skb->dev)->real_dev;
  177. struct sk_buff *frag;
  178. int rc;
  179. frag = alloc_skb(real_dev->hard_header_len +
  180. real_dev->needed_tailroom + size,
  181. GFP_ATOMIC);
  182. if (likely(frag)) {
  183. frag->dev = real_dev;
  184. frag->priority = skb->priority;
  185. skb_reserve(frag, real_dev->hard_header_len);
  186. skb_reset_network_header(frag);
  187. *mac_cb(frag) = *mac_cb(skb);
  188. rc = dev_hard_header(frag, real_dev, 0, &master_hdr->dest,
  189. &master_hdr->source, size);
  190. if (rc < 0) {
  191. kfree_skb(frag);
  192. return ERR_PTR(rc);
  193. }
  194. } else {
  195. frag = ERR_PTR(-ENOMEM);
  196. }
  197. return frag;
  198. }
  199. static int
  200. lowpan_xmit_fragment(struct sk_buff *skb, const struct ieee802154_hdr *wpan_hdr,
  201. u8 *frag_hdr, int frag_hdrlen,
  202. int offset, int len)
  203. {
  204. struct sk_buff *frag;
  205. raw_dump_inline(__func__, " fragment header", frag_hdr, frag_hdrlen);
  206. frag = lowpan_alloc_frag(skb, frag_hdrlen + len, wpan_hdr);
  207. if (IS_ERR(frag))
  208. return -PTR_ERR(frag);
  209. memcpy(skb_put(frag, frag_hdrlen), frag_hdr, frag_hdrlen);
  210. memcpy(skb_put(frag, len), skb_network_header(skb) + offset, len);
  211. raw_dump_table(__func__, " fragment dump", frag->data, frag->len);
  212. return dev_queue_xmit(frag);
  213. }
  214. static int
  215. lowpan_xmit_fragmented(struct sk_buff *skb, struct net_device *dev,
  216. const struct ieee802154_hdr *wpan_hdr)
  217. {
  218. u16 dgram_size, dgram_offset;
  219. __be16 frag_tag;
  220. u8 frag_hdr[5];
  221. int frag_cap, frag_len, payload_cap, rc;
  222. int skb_unprocessed, skb_offset;
  223. dgram_size = lowpan_uncompress_size(skb, &dgram_offset) -
  224. skb->mac_len;
  225. frag_tag = htons(lowpan_dev_info(dev)->fragment_tag);
  226. lowpan_dev_info(dev)->fragment_tag++;
  227. frag_hdr[0] = LOWPAN_DISPATCH_FRAG1 | ((dgram_size >> 8) & 0x07);
  228. frag_hdr[1] = dgram_size & 0xff;
  229. memcpy(frag_hdr + 2, &frag_tag, sizeof(frag_tag));
  230. payload_cap = ieee802154_max_payload(wpan_hdr);
  231. frag_len = round_down(payload_cap - LOWPAN_FRAG1_HEAD_SIZE -
  232. skb_network_header_len(skb), 8);
  233. skb_offset = skb_network_header_len(skb);
  234. skb_unprocessed = skb->len - skb->mac_len - skb_offset;
  235. rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
  236. LOWPAN_FRAG1_HEAD_SIZE, 0,
  237. frag_len + skb_network_header_len(skb));
  238. if (rc) {
  239. pr_debug("%s unable to send FRAG1 packet (tag: %d)",
  240. __func__, ntohs(frag_tag));
  241. goto err;
  242. }
  243. frag_hdr[0] &= ~LOWPAN_DISPATCH_FRAG1;
  244. frag_hdr[0] |= LOWPAN_DISPATCH_FRAGN;
  245. frag_cap = round_down(payload_cap - LOWPAN_FRAGN_HEAD_SIZE, 8);
  246. do {
  247. dgram_offset += frag_len;
  248. skb_offset += frag_len;
  249. skb_unprocessed -= frag_len;
  250. frag_len = min(frag_cap, skb_unprocessed);
  251. frag_hdr[4] = dgram_offset >> 3;
  252. rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
  253. LOWPAN_FRAGN_HEAD_SIZE, skb_offset,
  254. frag_len);
  255. if (rc) {
  256. pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
  257. __func__, ntohs(frag_tag), skb_offset);
  258. goto err;
  259. }
  260. } while (skb_unprocessed > frag_cap);
  261. consume_skb(skb);
  262. return NET_XMIT_SUCCESS;
  263. err:
  264. kfree_skb(skb);
  265. return rc;
  266. }
  267. static int lowpan_header(struct sk_buff *skb, struct net_device *dev)
  268. {
  269. struct ieee802154_addr sa, da;
  270. struct ieee802154_mac_cb *cb = mac_cb_init(skb);
  271. struct lowpan_addr_info info;
  272. void *daddr, *saddr;
  273. memcpy(&info, lowpan_skb_priv(skb), sizeof(info));
  274. /* TODO: Currently we only support extended_addr */
  275. daddr = &info.daddr.u.extended_addr;
  276. saddr = &info.saddr.u.extended_addr;
  277. lowpan_header_compress(skb, dev, ETH_P_IPV6, daddr, saddr, skb->len);
  278. cb->type = IEEE802154_FC_TYPE_DATA;
  279. /* prepare wpan address data */
  280. sa.mode = IEEE802154_ADDR_LONG;
  281. sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
  282. sa.extended_addr = ieee802154_devaddr_from_raw(saddr);
  283. /* intra-PAN communications */
  284. da.pan_id = sa.pan_id;
  285. /* if the destination address is the broadcast address, use the
  286. * corresponding short address
  287. */
  288. if (lowpan_is_addr_broadcast((const u8 *)daddr)) {
  289. da.mode = IEEE802154_ADDR_SHORT;
  290. da.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
  291. cb->ackreq = false;
  292. } else {
  293. da.mode = IEEE802154_ADDR_LONG;
  294. da.extended_addr = ieee802154_devaddr_from_raw(daddr);
  295. cb->ackreq = true;
  296. }
  297. return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
  298. ETH_P_IPV6, (void *)&da, (void *)&sa, 0);
  299. }
  300. static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
  301. {
  302. struct ieee802154_hdr wpan_hdr;
  303. int max_single, ret;
  304. pr_debug("package xmit\n");
  305. /* We must take a copy of the skb before we modify/replace the ipv6
  306. * header as the header could be used elsewhere
  307. */
  308. skb = skb_unshare(skb, GFP_ATOMIC);
  309. if (!skb)
  310. return NET_XMIT_DROP;
  311. ret = lowpan_header(skb, dev);
  312. if (ret < 0) {
  313. kfree_skb(skb);
  314. return NET_XMIT_DROP;
  315. }
  316. if (ieee802154_hdr_peek(skb, &wpan_hdr) < 0) {
  317. kfree_skb(skb);
  318. return NET_XMIT_DROP;
  319. }
  320. max_single = ieee802154_max_payload(&wpan_hdr);
  321. if (skb_tail_pointer(skb) - skb_network_header(skb) <= max_single) {
  322. skb->dev = lowpan_dev_info(dev)->real_dev;
  323. return dev_queue_xmit(skb);
  324. } else {
  325. netdev_tx_t rc;
  326. pr_debug("frame is too big, fragmentation is needed\n");
  327. rc = lowpan_xmit_fragmented(skb, dev, &wpan_hdr);
  328. return rc < 0 ? NET_XMIT_DROP : rc;
  329. }
  330. }
  331. static __le16 lowpan_get_pan_id(const struct net_device *dev)
  332. {
  333. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  334. return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
  335. }
  336. static __le16 lowpan_get_short_addr(const struct net_device *dev)
  337. {
  338. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  339. return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
  340. }
  341. static u8 lowpan_get_dsn(const struct net_device *dev)
  342. {
  343. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  344. return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
  345. }
  346. static struct header_ops lowpan_header_ops = {
  347. .create = lowpan_header_create,
  348. };
  349. static struct lock_class_key lowpan_tx_busylock;
  350. static struct lock_class_key lowpan_netdev_xmit_lock_key;
  351. static void lowpan_set_lockdep_class_one(struct net_device *dev,
  352. struct netdev_queue *txq,
  353. void *_unused)
  354. {
  355. lockdep_set_class(&txq->_xmit_lock,
  356. &lowpan_netdev_xmit_lock_key);
  357. }
  358. static int lowpan_dev_init(struct net_device *dev)
  359. {
  360. netdev_for_each_tx_queue(dev, lowpan_set_lockdep_class_one, NULL);
  361. dev->qdisc_tx_busylock = &lowpan_tx_busylock;
  362. return 0;
  363. }
  364. static const struct net_device_ops lowpan_netdev_ops = {
  365. .ndo_init = lowpan_dev_init,
  366. .ndo_start_xmit = lowpan_xmit,
  367. };
  368. static struct ieee802154_mlme_ops lowpan_mlme = {
  369. .get_pan_id = lowpan_get_pan_id,
  370. .get_short_addr = lowpan_get_short_addr,
  371. .get_dsn = lowpan_get_dsn,
  372. };
  373. static void lowpan_setup(struct net_device *dev)
  374. {
  375. dev->addr_len = IEEE802154_ADDR_LEN;
  376. memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
  377. dev->type = ARPHRD_IEEE802154;
  378. /* Frame Control + Sequence Number + Address fields + Security Header */
  379. dev->hard_header_len = 2 + 1 + 20 + 14;
  380. dev->needed_tailroom = 2; /* FCS */
  381. dev->mtu = IPV6_MIN_MTU;
  382. dev->tx_queue_len = 0;
  383. dev->flags = IFF_BROADCAST | IFF_MULTICAST;
  384. dev->watchdog_timeo = 0;
  385. dev->netdev_ops = &lowpan_netdev_ops;
  386. dev->header_ops = &lowpan_header_ops;
  387. dev->ml_priv = &lowpan_mlme;
  388. dev->destructor = free_netdev;
  389. }
  390. static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
  391. {
  392. if (tb[IFLA_ADDRESS]) {
  393. if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
  394. return -EINVAL;
  395. }
  396. return 0;
  397. }
  398. static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
  399. struct packet_type *pt, struct net_device *orig_dev)
  400. {
  401. struct ieee802154_hdr hdr;
  402. int ret;
  403. skb = skb_share_check(skb, GFP_ATOMIC);
  404. if (!skb)
  405. goto drop;
  406. if (!netif_running(dev))
  407. goto drop_skb;
  408. if (skb->pkt_type == PACKET_OTHERHOST)
  409. goto drop_skb;
  410. if (dev->type != ARPHRD_IEEE802154)
  411. goto drop_skb;
  412. if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
  413. goto drop_skb;
  414. /* check that it's our buffer */
  415. if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
  416. /* Pull off the 1-byte of 6lowpan header. */
  417. skb_pull(skb, 1);
  418. return lowpan_give_skb_to_devices(skb, NULL);
  419. } else {
  420. switch (skb->data[0] & 0xe0) {
  421. case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */
  422. ret = iphc_decompress(skb, &hdr);
  423. if (ret < 0)
  424. goto drop_skb;
  425. return lowpan_give_skb_to_devices(skb, NULL);
  426. case LOWPAN_DISPATCH_FRAG1: /* first fragment header */
  427. ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
  428. if (ret == 1) {
  429. ret = iphc_decompress(skb, &hdr);
  430. if (ret < 0)
  431. goto drop_skb;
  432. return lowpan_give_skb_to_devices(skb, NULL);
  433. } else if (ret == -1) {
  434. return NET_RX_DROP;
  435. } else {
  436. return NET_RX_SUCCESS;
  437. }
  438. case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */
  439. ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
  440. if (ret == 1) {
  441. ret = iphc_decompress(skb, &hdr);
  442. if (ret < 0)
  443. goto drop_skb;
  444. return lowpan_give_skb_to_devices(skb, NULL);
  445. } else if (ret == -1) {
  446. return NET_RX_DROP;
  447. } else {
  448. return NET_RX_SUCCESS;
  449. }
  450. default:
  451. break;
  452. }
  453. }
  454. drop_skb:
  455. kfree_skb(skb);
  456. drop:
  457. return NET_RX_DROP;
  458. }
  459. static struct packet_type lowpan_packet_type = {
  460. .type = htons(ETH_P_IEEE802154),
  461. .func = lowpan_rcv,
  462. };
  463. static int lowpan_newlink(struct net *src_net, struct net_device *dev,
  464. struct nlattr *tb[], struct nlattr *data[])
  465. {
  466. struct net_device *real_dev;
  467. struct lowpan_dev_record *entry;
  468. int ret;
  469. ASSERT_RTNL();
  470. pr_debug("adding new link\n");
  471. if (!tb[IFLA_LINK])
  472. return -EINVAL;
  473. /* find and hold real wpan device */
  474. real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
  475. if (!real_dev)
  476. return -ENODEV;
  477. if (real_dev->type != ARPHRD_IEEE802154) {
  478. dev_put(real_dev);
  479. return -EINVAL;
  480. }
  481. lowpan_dev_info(dev)->real_dev = real_dev;
  482. mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
  483. entry = kzalloc(sizeof(*entry), GFP_KERNEL);
  484. if (!entry) {
  485. dev_put(real_dev);
  486. lowpan_dev_info(dev)->real_dev = NULL;
  487. return -ENOMEM;
  488. }
  489. entry->ldev = dev;
  490. /* Set the lowpan harware address to the wpan hardware address. */
  491. memcpy(dev->dev_addr, real_dev->dev_addr, IEEE802154_ADDR_LEN);
  492. mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
  493. INIT_LIST_HEAD(&entry->list);
  494. list_add_tail(&entry->list, &lowpan_devices);
  495. mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
  496. ret = register_netdevice(dev);
  497. if (ret >= 0) {
  498. if (!lowpan_open_count)
  499. dev_add_pack(&lowpan_packet_type);
  500. lowpan_open_count++;
  501. }
  502. return ret;
  503. }
  504. static void lowpan_dellink(struct net_device *dev, struct list_head *head)
  505. {
  506. struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
  507. struct net_device *real_dev = lowpan_dev->real_dev;
  508. struct lowpan_dev_record *entry, *tmp;
  509. ASSERT_RTNL();
  510. lowpan_open_count--;
  511. if (!lowpan_open_count)
  512. dev_remove_pack(&lowpan_packet_type);
  513. mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
  514. list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
  515. if (entry->ldev == dev) {
  516. list_del(&entry->list);
  517. kfree(entry);
  518. }
  519. }
  520. mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
  521. mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
  522. unregister_netdevice_queue(dev, head);
  523. dev_put(real_dev);
  524. }
  525. static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
  526. .kind = "lowpan",
  527. .priv_size = sizeof(struct lowpan_dev_info),
  528. .setup = lowpan_setup,
  529. .newlink = lowpan_newlink,
  530. .dellink = lowpan_dellink,
  531. .validate = lowpan_validate,
  532. };
  533. static inline int __init lowpan_netlink_init(void)
  534. {
  535. return rtnl_link_register(&lowpan_link_ops);
  536. }
  537. static inline void lowpan_netlink_fini(void)
  538. {
  539. rtnl_link_unregister(&lowpan_link_ops);
  540. }
  541. static int lowpan_device_event(struct notifier_block *unused,
  542. unsigned long event, void *ptr)
  543. {
  544. struct net_device *dev = netdev_notifier_info_to_dev(ptr);
  545. LIST_HEAD(del_list);
  546. struct lowpan_dev_record *entry, *tmp;
  547. if (dev->type != ARPHRD_IEEE802154)
  548. goto out;
  549. if (event == NETDEV_UNREGISTER) {
  550. list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
  551. if (lowpan_dev_info(entry->ldev)->real_dev == dev)
  552. lowpan_dellink(entry->ldev, &del_list);
  553. }
  554. unregister_netdevice_many(&del_list);
  555. }
  556. out:
  557. return NOTIFY_DONE;
  558. }
  559. static struct notifier_block lowpan_dev_notifier = {
  560. .notifier_call = lowpan_device_event,
  561. };
  562. static int __init lowpan_init_module(void)
  563. {
  564. int err = 0;
  565. err = lowpan_net_frag_init();
  566. if (err < 0)
  567. goto out;
  568. err = lowpan_netlink_init();
  569. if (err < 0)
  570. goto out_frag;
  571. err = register_netdevice_notifier(&lowpan_dev_notifier);
  572. if (err < 0)
  573. goto out_pack;
  574. return 0;
  575. out_pack:
  576. lowpan_netlink_fini();
  577. out_frag:
  578. lowpan_net_frag_exit();
  579. out:
  580. return err;
  581. }
  582. static void __exit lowpan_cleanup_module(void)
  583. {
  584. lowpan_netlink_fini();
  585. lowpan_net_frag_exit();
  586. unregister_netdevice_notifier(&lowpan_dev_notifier);
  587. }
  588. module_init(lowpan_init_module);
  589. module_exit(lowpan_cleanup_module);
  590. MODULE_LICENSE("GPL");
  591. MODULE_ALIAS_RTNL_LINK("lowpan");