6lowpan_rtnl.c 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732
  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 <net/af_ieee802154.h>
  50. #include <net/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. /* private device info */
  57. struct lowpan_dev_info {
  58. struct net_device *real_dev; /* real WPAN device ptr */
  59. struct mutex dev_list_mtx; /* mutex for list ops */
  60. __be16 fragment_tag;
  61. };
  62. struct lowpan_dev_record {
  63. struct net_device *ldev;
  64. struct list_head list;
  65. };
  66. /* don't save pan id, it's intra pan */
  67. struct lowpan_addr {
  68. u8 mode;
  69. union {
  70. /* IPv6 needs big endian here */
  71. __be64 extended_addr;
  72. __be16 short_addr;
  73. } u;
  74. };
  75. struct lowpan_addr_info {
  76. struct lowpan_addr daddr;
  77. struct lowpan_addr saddr;
  78. };
  79. static inline struct
  80. lowpan_dev_info *lowpan_dev_info(const struct net_device *dev)
  81. {
  82. return netdev_priv(dev);
  83. }
  84. static inline struct
  85. lowpan_addr_info *lowpan_skb_priv(const struct sk_buff *skb)
  86. {
  87. WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct lowpan_addr_info));
  88. return (struct lowpan_addr_info *)(skb->data -
  89. sizeof(struct lowpan_addr_info));
  90. }
  91. static int lowpan_header_create(struct sk_buff *skb, struct net_device *dev,
  92. unsigned short type, const void *_daddr,
  93. const void *_saddr, unsigned int len)
  94. {
  95. const u8 *saddr = _saddr;
  96. const u8 *daddr = _daddr;
  97. struct lowpan_addr_info *info;
  98. /* TODO:
  99. * if this package isn't ipv6 one, where should it be routed?
  100. */
  101. if (type != ETH_P_IPV6)
  102. return 0;
  103. if (!saddr)
  104. saddr = dev->dev_addr;
  105. raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
  106. raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
  107. info = lowpan_skb_priv(skb);
  108. /* TODO: Currently we only support extended_addr */
  109. info->daddr.mode = IEEE802154_ADDR_LONG;
  110. memcpy(&info->daddr.u.extended_addr, daddr,
  111. sizeof(info->daddr.u.extended_addr));
  112. info->saddr.mode = IEEE802154_ADDR_LONG;
  113. memcpy(&info->saddr.u.extended_addr, saddr,
  114. sizeof(info->daddr.u.extended_addr));
  115. return 0;
  116. }
  117. static int lowpan_give_skb_to_devices(struct sk_buff *skb,
  118. struct net_device *dev)
  119. {
  120. struct lowpan_dev_record *entry;
  121. struct sk_buff *skb_cp;
  122. int stat = NET_RX_SUCCESS;
  123. rcu_read_lock();
  124. list_for_each_entry_rcu(entry, &lowpan_devices, list)
  125. if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
  126. skb_cp = skb_copy(skb, GFP_ATOMIC);
  127. if (!skb_cp) {
  128. stat = -ENOMEM;
  129. break;
  130. }
  131. skb_cp->dev = entry->ldev;
  132. stat = netif_rx(skb_cp);
  133. }
  134. rcu_read_unlock();
  135. return stat;
  136. }
  137. static int process_data(struct sk_buff *skb, const struct ieee802154_hdr *hdr)
  138. {
  139. u8 iphc0, iphc1;
  140. struct ieee802154_addr_sa sa, da;
  141. void *sap, *dap;
  142. raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
  143. /* at least two bytes will be used for the encoding */
  144. if (skb->len < 2)
  145. goto drop;
  146. if (lowpan_fetch_skb_u8(skb, &iphc0))
  147. goto drop;
  148. if (lowpan_fetch_skb_u8(skb, &iphc1))
  149. goto drop;
  150. ieee802154_addr_to_sa(&sa, &hdr->source);
  151. ieee802154_addr_to_sa(&da, &hdr->dest);
  152. if (sa.addr_type == IEEE802154_ADDR_SHORT)
  153. sap = &sa.short_addr;
  154. else
  155. sap = &sa.hwaddr;
  156. if (da.addr_type == IEEE802154_ADDR_SHORT)
  157. dap = &da.short_addr;
  158. else
  159. dap = &da.hwaddr;
  160. return lowpan_process_data(skb, skb->dev, sap, sa.addr_type,
  161. IEEE802154_ADDR_LEN, dap, da.addr_type,
  162. IEEE802154_ADDR_LEN, iphc0, iphc1,
  163. lowpan_give_skb_to_devices);
  164. drop:
  165. kfree_skb(skb);
  166. return -EINVAL;
  167. }
  168. static int lowpan_set_address(struct net_device *dev, void *p)
  169. {
  170. struct sockaddr *sa = p;
  171. if (netif_running(dev))
  172. return -EBUSY;
  173. /* TODO: validate addr */
  174. memcpy(dev->dev_addr, sa->sa_data, dev->addr_len);
  175. return 0;
  176. }
  177. static struct sk_buff*
  178. lowpan_alloc_frag(struct sk_buff *skb, int size,
  179. const struct ieee802154_hdr *master_hdr)
  180. {
  181. struct net_device *real_dev = lowpan_dev_info(skb->dev)->real_dev;
  182. struct sk_buff *frag;
  183. int rc;
  184. frag = alloc_skb(real_dev->hard_header_len +
  185. real_dev->needed_tailroom + size,
  186. GFP_ATOMIC);
  187. if (likely(frag)) {
  188. frag->dev = real_dev;
  189. frag->priority = skb->priority;
  190. skb_reserve(frag, real_dev->hard_header_len);
  191. skb_reset_network_header(frag);
  192. *mac_cb(frag) = *mac_cb(skb);
  193. rc = dev_hard_header(frag, real_dev, 0, &master_hdr->dest,
  194. &master_hdr->source, size);
  195. if (rc < 0) {
  196. kfree_skb(frag);
  197. return ERR_PTR(-rc);
  198. }
  199. } else {
  200. frag = ERR_PTR(-ENOMEM);
  201. }
  202. return frag;
  203. }
  204. static int
  205. lowpan_xmit_fragment(struct sk_buff *skb, const struct ieee802154_hdr *wpan_hdr,
  206. u8 *frag_hdr, int frag_hdrlen,
  207. int offset, int len)
  208. {
  209. struct sk_buff *frag;
  210. raw_dump_inline(__func__, " fragment header", frag_hdr, frag_hdrlen);
  211. frag = lowpan_alloc_frag(skb, frag_hdrlen + len, wpan_hdr);
  212. if (IS_ERR(frag))
  213. return -PTR_ERR(frag);
  214. memcpy(skb_put(frag, frag_hdrlen), frag_hdr, frag_hdrlen);
  215. memcpy(skb_put(frag, len), skb_network_header(skb) + offset, len);
  216. raw_dump_table(__func__, " fragment dump", frag->data, frag->len);
  217. return dev_queue_xmit(frag);
  218. }
  219. static int
  220. lowpan_xmit_fragmented(struct sk_buff *skb, struct net_device *dev,
  221. const struct ieee802154_hdr *wpan_hdr)
  222. {
  223. u16 dgram_size, dgram_offset;
  224. __be16 frag_tag;
  225. u8 frag_hdr[5];
  226. int frag_cap, frag_len, payload_cap, rc;
  227. int skb_unprocessed, skb_offset;
  228. dgram_size = lowpan_uncompress_size(skb, &dgram_offset) -
  229. skb->mac_len;
  230. frag_tag = lowpan_dev_info(dev)->fragment_tag++;
  231. frag_hdr[0] = LOWPAN_DISPATCH_FRAG1 | ((dgram_size >> 8) & 0x07);
  232. frag_hdr[1] = dgram_size & 0xff;
  233. memcpy(frag_hdr + 2, &frag_tag, sizeof(frag_tag));
  234. payload_cap = ieee802154_max_payload(wpan_hdr);
  235. frag_len = round_down(payload_cap - LOWPAN_FRAG1_HEAD_SIZE -
  236. skb_network_header_len(skb), 8);
  237. skb_offset = skb_network_header_len(skb);
  238. skb_unprocessed = skb->len - skb->mac_len - skb_offset;
  239. rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
  240. LOWPAN_FRAG1_HEAD_SIZE, 0,
  241. frag_len + skb_network_header_len(skb));
  242. if (rc) {
  243. pr_debug("%s unable to send FRAG1 packet (tag: %d)",
  244. __func__, frag_tag);
  245. goto err;
  246. }
  247. frag_hdr[0] &= ~LOWPAN_DISPATCH_FRAG1;
  248. frag_hdr[0] |= LOWPAN_DISPATCH_FRAGN;
  249. frag_cap = round_down(payload_cap - LOWPAN_FRAGN_HEAD_SIZE, 8);
  250. do {
  251. dgram_offset += frag_len;
  252. skb_offset += frag_len;
  253. skb_unprocessed -= frag_len;
  254. frag_len = min(frag_cap, skb_unprocessed);
  255. frag_hdr[4] = dgram_offset >> 3;
  256. rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
  257. LOWPAN_FRAGN_HEAD_SIZE, skb_offset,
  258. frag_len);
  259. if (rc) {
  260. pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
  261. __func__, frag_tag, skb_offset);
  262. goto err;
  263. }
  264. } while (skb_unprocessed > frag_cap);
  265. consume_skb(skb);
  266. return NET_XMIT_SUCCESS;
  267. err:
  268. kfree_skb(skb);
  269. return rc;
  270. }
  271. static int lowpan_header(struct sk_buff *skb, struct net_device *dev)
  272. {
  273. struct ieee802154_addr sa, da;
  274. struct ieee802154_mac_cb *cb = mac_cb_init(skb);
  275. struct lowpan_addr_info info;
  276. void *daddr, *saddr;
  277. memcpy(&info, lowpan_skb_priv(skb), sizeof(info));
  278. /* TODO: Currently we only support extended_addr */
  279. daddr = &info.daddr.u.extended_addr;
  280. saddr = &info.saddr.u.extended_addr;
  281. lowpan_header_compress(skb, dev, ETH_P_IPV6, daddr, saddr, skb->len);
  282. cb->type = IEEE802154_FC_TYPE_DATA;
  283. /* prepare wpan address data */
  284. sa.mode = IEEE802154_ADDR_LONG;
  285. sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
  286. sa.extended_addr = ieee802154_devaddr_from_raw(saddr);
  287. /* intra-PAN communications */
  288. da.pan_id = sa.pan_id;
  289. /* if the destination address is the broadcast address, use the
  290. * corresponding short address
  291. */
  292. if (lowpan_is_addr_broadcast((const u8 *)daddr)) {
  293. da.mode = IEEE802154_ADDR_SHORT;
  294. da.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
  295. cb->ackreq = false;
  296. } else {
  297. da.mode = IEEE802154_ADDR_LONG;
  298. da.extended_addr = ieee802154_devaddr_from_raw(daddr);
  299. cb->ackreq = true;
  300. }
  301. return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
  302. ETH_P_IPV6, (void *)&da, (void *)&sa, 0);
  303. }
  304. static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
  305. {
  306. struct ieee802154_hdr wpan_hdr;
  307. int max_single, ret;
  308. pr_debug("package xmit\n");
  309. /* We must take a copy of the skb before we modify/replace the ipv6
  310. * header as the header could be used elsewhere
  311. */
  312. skb = skb_unshare(skb, GFP_ATOMIC);
  313. if (!skb)
  314. return NET_XMIT_DROP;
  315. ret = lowpan_header(skb, dev);
  316. if (ret < 0) {
  317. kfree_skb(skb);
  318. return NET_XMIT_DROP;
  319. }
  320. if (ieee802154_hdr_peek(skb, &wpan_hdr) < 0) {
  321. kfree_skb(skb);
  322. return NET_XMIT_DROP;
  323. }
  324. max_single = ieee802154_max_payload(&wpan_hdr);
  325. if (skb_tail_pointer(skb) - skb_network_header(skb) <= max_single) {
  326. skb->dev = lowpan_dev_info(dev)->real_dev;
  327. return dev_queue_xmit(skb);
  328. } else {
  329. netdev_tx_t rc;
  330. pr_debug("frame is too big, fragmentation is needed\n");
  331. rc = lowpan_xmit_fragmented(skb, dev, &wpan_hdr);
  332. return rc < 0 ? NET_XMIT_DROP : rc;
  333. }
  334. }
  335. static struct wpan_phy *lowpan_get_phy(const struct net_device *dev)
  336. {
  337. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  338. return ieee802154_mlme_ops(real_dev)->get_phy(real_dev);
  339. }
  340. static __le16 lowpan_get_pan_id(const struct net_device *dev)
  341. {
  342. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  343. return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
  344. }
  345. static __le16 lowpan_get_short_addr(const struct net_device *dev)
  346. {
  347. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  348. return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
  349. }
  350. static u8 lowpan_get_dsn(const struct net_device *dev)
  351. {
  352. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  353. return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
  354. }
  355. static struct header_ops lowpan_header_ops = {
  356. .create = lowpan_header_create,
  357. };
  358. static struct lock_class_key lowpan_tx_busylock;
  359. static struct lock_class_key lowpan_netdev_xmit_lock_key;
  360. static void lowpan_set_lockdep_class_one(struct net_device *dev,
  361. struct netdev_queue *txq,
  362. void *_unused)
  363. {
  364. lockdep_set_class(&txq->_xmit_lock,
  365. &lowpan_netdev_xmit_lock_key);
  366. }
  367. static int lowpan_dev_init(struct net_device *dev)
  368. {
  369. netdev_for_each_tx_queue(dev, lowpan_set_lockdep_class_one, NULL);
  370. dev->qdisc_tx_busylock = &lowpan_tx_busylock;
  371. return 0;
  372. }
  373. static const struct net_device_ops lowpan_netdev_ops = {
  374. .ndo_init = lowpan_dev_init,
  375. .ndo_start_xmit = lowpan_xmit,
  376. .ndo_set_mac_address = lowpan_set_address,
  377. };
  378. static struct ieee802154_mlme_ops lowpan_mlme = {
  379. .get_pan_id = lowpan_get_pan_id,
  380. .get_phy = lowpan_get_phy,
  381. .get_short_addr = lowpan_get_short_addr,
  382. .get_dsn = lowpan_get_dsn,
  383. };
  384. static void lowpan_setup(struct net_device *dev)
  385. {
  386. dev->addr_len = IEEE802154_ADDR_LEN;
  387. memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
  388. dev->type = ARPHRD_IEEE802154;
  389. /* Frame Control + Sequence Number + Address fields + Security Header */
  390. dev->hard_header_len = 2 + 1 + 20 + 14;
  391. dev->needed_tailroom = 2; /* FCS */
  392. dev->mtu = IPV6_MIN_MTU;
  393. dev->tx_queue_len = 0;
  394. dev->flags = IFF_BROADCAST | IFF_MULTICAST;
  395. dev->watchdog_timeo = 0;
  396. dev->netdev_ops = &lowpan_netdev_ops;
  397. dev->header_ops = &lowpan_header_ops;
  398. dev->ml_priv = &lowpan_mlme;
  399. dev->destructor = free_netdev;
  400. }
  401. static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
  402. {
  403. if (tb[IFLA_ADDRESS]) {
  404. if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
  405. return -EINVAL;
  406. }
  407. return 0;
  408. }
  409. static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
  410. struct packet_type *pt, struct net_device *orig_dev)
  411. {
  412. struct ieee802154_hdr hdr;
  413. int ret;
  414. skb = skb_share_check(skb, GFP_ATOMIC);
  415. if (!skb)
  416. goto drop;
  417. if (!netif_running(dev))
  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. skb->protocol = htons(ETH_P_IPV6);
  426. skb->pkt_type = PACKET_HOST;
  427. /* Pull off the 1-byte of 6lowpan header. */
  428. skb_pull(skb, 1);
  429. ret = lowpan_give_skb_to_devices(skb, NULL);
  430. if (ret == NET_RX_DROP)
  431. goto drop;
  432. } else {
  433. switch (skb->data[0] & 0xe0) {
  434. case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */
  435. ret = process_data(skb, &hdr);
  436. if (ret == NET_RX_DROP)
  437. goto drop;
  438. break;
  439. case LOWPAN_DISPATCH_FRAG1: /* first fragment header */
  440. ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
  441. if (ret == 1) {
  442. ret = process_data(skb, &hdr);
  443. if (ret == NET_RX_DROP)
  444. goto drop;
  445. }
  446. break;
  447. case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */
  448. ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
  449. if (ret == 1) {
  450. ret = process_data(skb, &hdr);
  451. if (ret == NET_RX_DROP)
  452. goto drop;
  453. }
  454. break;
  455. default:
  456. break;
  457. }
  458. }
  459. return NET_RX_SUCCESS;
  460. drop_skb:
  461. kfree_skb(skb);
  462. drop:
  463. return NET_RX_DROP;
  464. }
  465. static int lowpan_newlink(struct net *src_net, struct net_device *dev,
  466. struct nlattr *tb[], struct nlattr *data[])
  467. {
  468. struct net_device *real_dev;
  469. struct lowpan_dev_record *entry;
  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. register_netdevice(dev);
  497. return 0;
  498. }
  499. static void lowpan_dellink(struct net_device *dev, struct list_head *head)
  500. {
  501. struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
  502. struct net_device *real_dev = lowpan_dev->real_dev;
  503. struct lowpan_dev_record *entry, *tmp;
  504. ASSERT_RTNL();
  505. mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
  506. list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
  507. if (entry->ldev == dev) {
  508. list_del(&entry->list);
  509. kfree(entry);
  510. }
  511. }
  512. mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
  513. mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
  514. unregister_netdevice_queue(dev, head);
  515. dev_put(real_dev);
  516. }
  517. static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
  518. .kind = "lowpan",
  519. .priv_size = sizeof(struct lowpan_dev_info),
  520. .setup = lowpan_setup,
  521. .newlink = lowpan_newlink,
  522. .dellink = lowpan_dellink,
  523. .validate = lowpan_validate,
  524. };
  525. static inline int __init lowpan_netlink_init(void)
  526. {
  527. return rtnl_link_register(&lowpan_link_ops);
  528. }
  529. static inline void lowpan_netlink_fini(void)
  530. {
  531. rtnl_link_unregister(&lowpan_link_ops);
  532. }
  533. static int lowpan_device_event(struct notifier_block *unused,
  534. unsigned long event, void *ptr)
  535. {
  536. struct net_device *dev = netdev_notifier_info_to_dev(ptr);
  537. LIST_HEAD(del_list);
  538. struct lowpan_dev_record *entry, *tmp;
  539. if (dev->type != ARPHRD_IEEE802154)
  540. goto out;
  541. if (event == NETDEV_UNREGISTER) {
  542. list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
  543. if (lowpan_dev_info(entry->ldev)->real_dev == dev)
  544. lowpan_dellink(entry->ldev, &del_list);
  545. }
  546. unregister_netdevice_many(&del_list);
  547. }
  548. out:
  549. return NOTIFY_DONE;
  550. }
  551. static struct notifier_block lowpan_dev_notifier = {
  552. .notifier_call = lowpan_device_event,
  553. };
  554. static struct packet_type lowpan_packet_type = {
  555. .type = htons(ETH_P_IEEE802154),
  556. .func = lowpan_rcv,
  557. };
  558. static int __init lowpan_init_module(void)
  559. {
  560. int err = 0;
  561. err = lowpan_net_frag_init();
  562. if (err < 0)
  563. goto out;
  564. err = lowpan_netlink_init();
  565. if (err < 0)
  566. goto out_frag;
  567. dev_add_pack(&lowpan_packet_type);
  568. err = register_netdevice_notifier(&lowpan_dev_notifier);
  569. if (err < 0)
  570. goto out_pack;
  571. return 0;
  572. out_pack:
  573. dev_remove_pack(&lowpan_packet_type);
  574. lowpan_netlink_fini();
  575. out_frag:
  576. lowpan_net_frag_exit();
  577. out:
  578. return err;
  579. }
  580. static void __exit lowpan_cleanup_module(void)
  581. {
  582. lowpan_netlink_fini();
  583. dev_remove_pack(&lowpan_packet_type);
  584. lowpan_net_frag_exit();
  585. unregister_netdevice_notifier(&lowpan_dev_notifier);
  586. }
  587. module_init(lowpan_init_module);
  588. module_exit(lowpan_cleanup_module);
  589. MODULE_LICENSE("GPL");
  590. MODULE_ALIAS_RTNL_LINK("lowpan");