6lowpan_rtnl.c 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742
  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. goto drop;
  154. if (lowpan_fetch_skb_u8(skb, &iphc0))
  155. goto drop;
  156. if (lowpan_fetch_skb_u8(skb, &iphc1))
  157. goto drop;
  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. drop:
  172. kfree_skb(skb);
  173. return -EINVAL;
  174. }
  175. static struct sk_buff*
  176. lowpan_alloc_frag(struct sk_buff *skb, int size,
  177. const struct ieee802154_hdr *master_hdr)
  178. {
  179. struct net_device *real_dev = lowpan_dev_info(skb->dev)->real_dev;
  180. struct sk_buff *frag;
  181. int rc;
  182. frag = alloc_skb(real_dev->hard_header_len +
  183. real_dev->needed_tailroom + size,
  184. GFP_ATOMIC);
  185. if (likely(frag)) {
  186. frag->dev = real_dev;
  187. frag->priority = skb->priority;
  188. skb_reserve(frag, real_dev->hard_header_len);
  189. skb_reset_network_header(frag);
  190. *mac_cb(frag) = *mac_cb(skb);
  191. rc = dev_hard_header(frag, real_dev, 0, &master_hdr->dest,
  192. &master_hdr->source, size);
  193. if (rc < 0) {
  194. kfree_skb(frag);
  195. return ERR_PTR(rc);
  196. }
  197. } else {
  198. frag = ERR_PTR(-ENOMEM);
  199. }
  200. return frag;
  201. }
  202. static int
  203. lowpan_xmit_fragment(struct sk_buff *skb, const struct ieee802154_hdr *wpan_hdr,
  204. u8 *frag_hdr, int frag_hdrlen,
  205. int offset, int len)
  206. {
  207. struct sk_buff *frag;
  208. raw_dump_inline(__func__, " fragment header", frag_hdr, frag_hdrlen);
  209. frag = lowpan_alloc_frag(skb, frag_hdrlen + len, wpan_hdr);
  210. if (IS_ERR(frag))
  211. return -PTR_ERR(frag);
  212. memcpy(skb_put(frag, frag_hdrlen), frag_hdr, frag_hdrlen);
  213. memcpy(skb_put(frag, len), skb_network_header(skb) + offset, len);
  214. raw_dump_table(__func__, " fragment dump", frag->data, frag->len);
  215. return dev_queue_xmit(frag);
  216. }
  217. static int
  218. lowpan_xmit_fragmented(struct sk_buff *skb, struct net_device *dev,
  219. const struct ieee802154_hdr *wpan_hdr)
  220. {
  221. u16 dgram_size, dgram_offset;
  222. __be16 frag_tag;
  223. u8 frag_hdr[5];
  224. int frag_cap, frag_len, payload_cap, rc;
  225. int skb_unprocessed, skb_offset;
  226. dgram_size = lowpan_uncompress_size(skb, &dgram_offset) -
  227. skb->mac_len;
  228. frag_tag = htons(lowpan_dev_info(dev)->fragment_tag);
  229. lowpan_dev_info(dev)->fragment_tag++;
  230. frag_hdr[0] = LOWPAN_DISPATCH_FRAG1 | ((dgram_size >> 8) & 0x07);
  231. frag_hdr[1] = dgram_size & 0xff;
  232. memcpy(frag_hdr + 2, &frag_tag, sizeof(frag_tag));
  233. payload_cap = ieee802154_max_payload(wpan_hdr);
  234. frag_len = round_down(payload_cap - LOWPAN_FRAG1_HEAD_SIZE -
  235. skb_network_header_len(skb), 8);
  236. skb_offset = skb_network_header_len(skb);
  237. skb_unprocessed = skb->len - skb->mac_len - skb_offset;
  238. rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
  239. LOWPAN_FRAG1_HEAD_SIZE, 0,
  240. frag_len + skb_network_header_len(skb));
  241. if (rc) {
  242. pr_debug("%s unable to send FRAG1 packet (tag: %d)",
  243. __func__, ntohs(frag_tag));
  244. goto err;
  245. }
  246. frag_hdr[0] &= ~LOWPAN_DISPATCH_FRAG1;
  247. frag_hdr[0] |= LOWPAN_DISPATCH_FRAGN;
  248. frag_cap = round_down(payload_cap - LOWPAN_FRAGN_HEAD_SIZE, 8);
  249. do {
  250. dgram_offset += frag_len;
  251. skb_offset += frag_len;
  252. skb_unprocessed -= frag_len;
  253. frag_len = min(frag_cap, skb_unprocessed);
  254. frag_hdr[4] = dgram_offset >> 3;
  255. rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
  256. LOWPAN_FRAGN_HEAD_SIZE, skb_offset,
  257. frag_len);
  258. if (rc) {
  259. pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
  260. __func__, ntohs(frag_tag), skb_offset);
  261. goto err;
  262. }
  263. } while (skb_unprocessed > frag_cap);
  264. consume_skb(skb);
  265. return NET_XMIT_SUCCESS;
  266. err:
  267. kfree_skb(skb);
  268. return rc;
  269. }
  270. static int lowpan_header(struct sk_buff *skb, struct net_device *dev)
  271. {
  272. struct ieee802154_addr sa, da;
  273. struct ieee802154_mac_cb *cb = mac_cb_init(skb);
  274. struct lowpan_addr_info info;
  275. void *daddr, *saddr;
  276. memcpy(&info, lowpan_skb_priv(skb), sizeof(info));
  277. /* TODO: Currently we only support extended_addr */
  278. daddr = &info.daddr.u.extended_addr;
  279. saddr = &info.saddr.u.extended_addr;
  280. lowpan_header_compress(skb, dev, ETH_P_IPV6, daddr, saddr, skb->len);
  281. cb->type = IEEE802154_FC_TYPE_DATA;
  282. /* prepare wpan address data */
  283. sa.mode = IEEE802154_ADDR_LONG;
  284. sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
  285. sa.extended_addr = ieee802154_devaddr_from_raw(saddr);
  286. /* intra-PAN communications */
  287. da.pan_id = sa.pan_id;
  288. /* if the destination address is the broadcast address, use the
  289. * corresponding short address
  290. */
  291. if (lowpan_is_addr_broadcast((const u8 *)daddr)) {
  292. da.mode = IEEE802154_ADDR_SHORT;
  293. da.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
  294. cb->ackreq = false;
  295. } else {
  296. da.mode = IEEE802154_ADDR_LONG;
  297. da.extended_addr = ieee802154_devaddr_from_raw(daddr);
  298. cb->ackreq = true;
  299. }
  300. return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
  301. ETH_P_IPV6, (void *)&da, (void *)&sa, 0);
  302. }
  303. static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
  304. {
  305. struct ieee802154_hdr wpan_hdr;
  306. int max_single, ret;
  307. pr_debug("package xmit\n");
  308. /* We must take a copy of the skb before we modify/replace the ipv6
  309. * header as the header could be used elsewhere
  310. */
  311. skb = skb_unshare(skb, GFP_ATOMIC);
  312. if (!skb)
  313. return NET_XMIT_DROP;
  314. ret = lowpan_header(skb, dev);
  315. if (ret < 0) {
  316. kfree_skb(skb);
  317. return NET_XMIT_DROP;
  318. }
  319. if (ieee802154_hdr_peek(skb, &wpan_hdr) < 0) {
  320. kfree_skb(skb);
  321. return NET_XMIT_DROP;
  322. }
  323. max_single = ieee802154_max_payload(&wpan_hdr);
  324. if (skb_tail_pointer(skb) - skb_network_header(skb) <= max_single) {
  325. skb->dev = lowpan_dev_info(dev)->real_dev;
  326. return dev_queue_xmit(skb);
  327. } else {
  328. netdev_tx_t rc;
  329. pr_debug("frame is too big, fragmentation is needed\n");
  330. rc = lowpan_xmit_fragmented(skb, dev, &wpan_hdr);
  331. return rc < 0 ? NET_XMIT_DROP : rc;
  332. }
  333. }
  334. static struct wpan_phy *lowpan_get_phy(const struct net_device *dev)
  335. {
  336. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  337. return ieee802154_mlme_ops(real_dev)->get_phy(real_dev);
  338. }
  339. static __le16 lowpan_get_pan_id(const struct net_device *dev)
  340. {
  341. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  342. return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
  343. }
  344. static __le16 lowpan_get_short_addr(const struct net_device *dev)
  345. {
  346. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  347. return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
  348. }
  349. static u8 lowpan_get_dsn(const struct net_device *dev)
  350. {
  351. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  352. return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
  353. }
  354. static struct header_ops lowpan_header_ops = {
  355. .create = lowpan_header_create,
  356. };
  357. static struct lock_class_key lowpan_tx_busylock;
  358. static struct lock_class_key lowpan_netdev_xmit_lock_key;
  359. static void lowpan_set_lockdep_class_one(struct net_device *dev,
  360. struct netdev_queue *txq,
  361. void *_unused)
  362. {
  363. lockdep_set_class(&txq->_xmit_lock,
  364. &lowpan_netdev_xmit_lock_key);
  365. }
  366. static int lowpan_dev_init(struct net_device *dev)
  367. {
  368. netdev_for_each_tx_queue(dev, lowpan_set_lockdep_class_one, NULL);
  369. dev->qdisc_tx_busylock = &lowpan_tx_busylock;
  370. return 0;
  371. }
  372. static const struct net_device_ops lowpan_netdev_ops = {
  373. .ndo_init = lowpan_dev_init,
  374. .ndo_start_xmit = lowpan_xmit,
  375. };
  376. static struct ieee802154_mlme_ops lowpan_mlme = {
  377. .get_pan_id = lowpan_get_pan_id,
  378. .get_phy = lowpan_get_phy,
  379. .get_short_addr = lowpan_get_short_addr,
  380. .get_dsn = lowpan_get_dsn,
  381. };
  382. static void lowpan_setup(struct net_device *dev)
  383. {
  384. dev->addr_len = IEEE802154_ADDR_LEN;
  385. memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
  386. dev->type = ARPHRD_IEEE802154;
  387. /* Frame Control + Sequence Number + Address fields + Security Header */
  388. dev->hard_header_len = 2 + 1 + 20 + 14;
  389. dev->needed_tailroom = 2; /* FCS */
  390. dev->mtu = IPV6_MIN_MTU;
  391. dev->tx_queue_len = 0;
  392. dev->flags = IFF_BROADCAST | IFF_MULTICAST;
  393. dev->watchdog_timeo = 0;
  394. dev->netdev_ops = &lowpan_netdev_ops;
  395. dev->header_ops = &lowpan_header_ops;
  396. dev->ml_priv = &lowpan_mlme;
  397. dev->destructor = free_netdev;
  398. }
  399. static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
  400. {
  401. if (tb[IFLA_ADDRESS]) {
  402. if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
  403. return -EINVAL;
  404. }
  405. return 0;
  406. }
  407. static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
  408. struct packet_type *pt, struct net_device *orig_dev)
  409. {
  410. struct ieee802154_hdr hdr;
  411. int ret;
  412. skb = skb_share_check(skb, GFP_ATOMIC);
  413. if (!skb)
  414. goto drop;
  415. if (!netif_running(dev))
  416. goto drop_skb;
  417. if (skb->pkt_type == PACKET_OTHERHOST)
  418. goto drop_skb;
  419. if (dev->type != ARPHRD_IEEE802154)
  420. goto drop_skb;
  421. if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
  422. goto drop_skb;
  423. /* check that it's our buffer */
  424. if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
  425. /* Pull off the 1-byte of 6lowpan header. */
  426. skb_pull(skb, 1);
  427. return lowpan_give_skb_to_devices(skb, NULL);
  428. } else {
  429. switch (skb->data[0] & 0xe0) {
  430. case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */
  431. ret = iphc_decompress(skb, &hdr);
  432. if (ret < 0)
  433. goto drop;
  434. return lowpan_give_skb_to_devices(skb, NULL);
  435. case LOWPAN_DISPATCH_FRAG1: /* first fragment header */
  436. ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
  437. if (ret == 1) {
  438. ret = iphc_decompress(skb, &hdr);
  439. if (ret < 0)
  440. goto drop;
  441. return lowpan_give_skb_to_devices(skb, NULL);
  442. } else if (ret == -1) {
  443. return NET_RX_DROP;
  444. } else {
  445. return NET_RX_SUCCESS;
  446. }
  447. case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */
  448. ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
  449. if (ret == 1) {
  450. ret = iphc_decompress(skb, &hdr);
  451. if (ret < 0)
  452. goto drop;
  453. return lowpan_give_skb_to_devices(skb, NULL);
  454. } else if (ret == -1) {
  455. return NET_RX_DROP;
  456. } else {
  457. return NET_RX_SUCCESS;
  458. }
  459. default:
  460. break;
  461. }
  462. }
  463. drop_skb:
  464. kfree_skb(skb);
  465. drop:
  466. return NET_RX_DROP;
  467. }
  468. static struct packet_type lowpan_packet_type = {
  469. .type = htons(ETH_P_IEEE802154),
  470. .func = lowpan_rcv,
  471. };
  472. static int lowpan_newlink(struct net *src_net, struct net_device *dev,
  473. struct nlattr *tb[], struct nlattr *data[])
  474. {
  475. struct net_device *real_dev;
  476. struct lowpan_dev_record *entry;
  477. int ret;
  478. ASSERT_RTNL();
  479. pr_debug("adding new link\n");
  480. if (!tb[IFLA_LINK])
  481. return -EINVAL;
  482. /* find and hold real wpan device */
  483. real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
  484. if (!real_dev)
  485. return -ENODEV;
  486. if (real_dev->type != ARPHRD_IEEE802154) {
  487. dev_put(real_dev);
  488. return -EINVAL;
  489. }
  490. lowpan_dev_info(dev)->real_dev = real_dev;
  491. mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
  492. entry = kzalloc(sizeof(*entry), GFP_KERNEL);
  493. if (!entry) {
  494. dev_put(real_dev);
  495. lowpan_dev_info(dev)->real_dev = NULL;
  496. return -ENOMEM;
  497. }
  498. entry->ldev = dev;
  499. /* Set the lowpan harware address to the wpan hardware address. */
  500. memcpy(dev->dev_addr, real_dev->dev_addr, IEEE802154_ADDR_LEN);
  501. mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
  502. INIT_LIST_HEAD(&entry->list);
  503. list_add_tail(&entry->list, &lowpan_devices);
  504. mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
  505. ret = register_netdevice(dev);
  506. if (ret >= 0) {
  507. if (!lowpan_open_count)
  508. dev_add_pack(&lowpan_packet_type);
  509. lowpan_open_count++;
  510. }
  511. return ret;
  512. }
  513. static void lowpan_dellink(struct net_device *dev, struct list_head *head)
  514. {
  515. struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
  516. struct net_device *real_dev = lowpan_dev->real_dev;
  517. struct lowpan_dev_record *entry, *tmp;
  518. ASSERT_RTNL();
  519. lowpan_open_count--;
  520. if (!lowpan_open_count)
  521. dev_remove_pack(&lowpan_packet_type);
  522. mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
  523. list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
  524. if (entry->ldev == dev) {
  525. list_del(&entry->list);
  526. kfree(entry);
  527. }
  528. }
  529. mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
  530. mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
  531. unregister_netdevice_queue(dev, head);
  532. dev_put(real_dev);
  533. }
  534. static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
  535. .kind = "lowpan",
  536. .priv_size = sizeof(struct lowpan_dev_info),
  537. .setup = lowpan_setup,
  538. .newlink = lowpan_newlink,
  539. .dellink = lowpan_dellink,
  540. .validate = lowpan_validate,
  541. };
  542. static inline int __init lowpan_netlink_init(void)
  543. {
  544. return rtnl_link_register(&lowpan_link_ops);
  545. }
  546. static inline void lowpan_netlink_fini(void)
  547. {
  548. rtnl_link_unregister(&lowpan_link_ops);
  549. }
  550. static int lowpan_device_event(struct notifier_block *unused,
  551. unsigned long event, void *ptr)
  552. {
  553. struct net_device *dev = netdev_notifier_info_to_dev(ptr);
  554. LIST_HEAD(del_list);
  555. struct lowpan_dev_record *entry, *tmp;
  556. if (dev->type != ARPHRD_IEEE802154)
  557. goto out;
  558. if (event == NETDEV_UNREGISTER) {
  559. list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
  560. if (lowpan_dev_info(entry->ldev)->real_dev == dev)
  561. lowpan_dellink(entry->ldev, &del_list);
  562. }
  563. unregister_netdevice_many(&del_list);
  564. }
  565. out:
  566. return NOTIFY_DONE;
  567. }
  568. static struct notifier_block lowpan_dev_notifier = {
  569. .notifier_call = lowpan_device_event,
  570. };
  571. static int __init lowpan_init_module(void)
  572. {
  573. int err = 0;
  574. err = lowpan_net_frag_init();
  575. if (err < 0)
  576. goto out;
  577. err = lowpan_netlink_init();
  578. if (err < 0)
  579. goto out_frag;
  580. err = register_netdevice_notifier(&lowpan_dev_notifier);
  581. if (err < 0)
  582. goto out_pack;
  583. return 0;
  584. out_pack:
  585. lowpan_netlink_fini();
  586. out_frag:
  587. lowpan_net_frag_exit();
  588. out:
  589. return err;
  590. }
  591. static void __exit lowpan_cleanup_module(void)
  592. {
  593. lowpan_netlink_fini();
  594. lowpan_net_frag_exit();
  595. unregister_netdevice_notifier(&lowpan_dev_notifier);
  596. }
  597. module_init(lowpan_init_module);
  598. module_exit(lowpan_cleanup_module);
  599. MODULE_LICENSE("GPL");
  600. MODULE_ALIAS_RTNL_LINK("lowpan");