6lowpan.c 28 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323
  1. /*
  2. Copyright (c) 2013-2014 Intel Corp.
  3. This program is free software; you can redistribute it and/or modify
  4. it under the terms of the GNU General Public License version 2 and
  5. only version 2 as published by the Free Software Foundation.
  6. This program is distributed in the hope that it will be useful,
  7. but WITHOUT ANY WARRANTY; without even the implied warranty of
  8. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  9. GNU General Public License for more details.
  10. */
  11. #include <linux/if_arp.h>
  12. #include <linux/netdevice.h>
  13. #include <linux/etherdevice.h>
  14. #include <linux/module.h>
  15. #include <linux/debugfs.h>
  16. #include <net/ipv6.h>
  17. #include <net/ip6_route.h>
  18. #include <net/addrconf.h>
  19. #include <net/pkt_sched.h>
  20. #include <net/bluetooth/bluetooth.h>
  21. #include <net/bluetooth/hci_core.h>
  22. #include <net/bluetooth/l2cap.h>
  23. #include <net/6lowpan.h> /* for the compression support */
  24. #define VERSION "0.1"
  25. static struct dentry *lowpan_enable_debugfs;
  26. static struct dentry *lowpan_control_debugfs;
  27. #define IFACE_NAME_TEMPLATE "bt%d"
  28. struct skb_cb {
  29. struct in6_addr addr;
  30. struct in6_addr gw;
  31. struct l2cap_chan *chan;
  32. };
  33. #define lowpan_cb(skb) ((struct skb_cb *)((skb)->cb))
  34. /* The devices list contains those devices that we are acting
  35. * as a proxy. The BT 6LoWPAN device is a virtual device that
  36. * connects to the Bluetooth LE device. The real connection to
  37. * BT device is done via l2cap layer. There exists one
  38. * virtual device / one BT 6LoWPAN network (=hciX device).
  39. * The list contains struct lowpan_dev elements.
  40. */
  41. static LIST_HEAD(bt_6lowpan_devices);
  42. static DEFINE_SPINLOCK(devices_lock);
  43. static bool enable_6lowpan;
  44. /* We are listening incoming connections via this channel
  45. */
  46. static struct l2cap_chan *listen_chan;
  47. static DEFINE_MUTEX(set_lock);
  48. struct lowpan_peer {
  49. struct list_head list;
  50. struct rcu_head rcu;
  51. struct l2cap_chan *chan;
  52. /* peer addresses in various formats */
  53. unsigned char lladdr[ETH_ALEN];
  54. struct in6_addr peer_addr;
  55. };
  56. struct lowpan_btle_dev {
  57. struct list_head list;
  58. struct hci_dev *hdev;
  59. struct net_device *netdev;
  60. struct list_head peers;
  61. atomic_t peer_count; /* number of items in peers list */
  62. struct work_struct delete_netdev;
  63. struct delayed_work notify_peers;
  64. };
  65. static inline struct lowpan_btle_dev *
  66. lowpan_btle_dev(const struct net_device *netdev)
  67. {
  68. return (struct lowpan_btle_dev *)lowpan_dev(netdev)->priv;
  69. }
  70. static inline void peer_add(struct lowpan_btle_dev *dev,
  71. struct lowpan_peer *peer)
  72. {
  73. list_add_rcu(&peer->list, &dev->peers);
  74. atomic_inc(&dev->peer_count);
  75. }
  76. static inline bool peer_del(struct lowpan_btle_dev *dev,
  77. struct lowpan_peer *peer)
  78. {
  79. list_del_rcu(&peer->list);
  80. kfree_rcu(peer, rcu);
  81. module_put(THIS_MODULE);
  82. if (atomic_dec_and_test(&dev->peer_count)) {
  83. BT_DBG("last peer");
  84. return true;
  85. }
  86. return false;
  87. }
  88. static inline struct lowpan_peer *peer_lookup_ba(struct lowpan_btle_dev *dev,
  89. bdaddr_t *ba, __u8 type)
  90. {
  91. struct lowpan_peer *peer;
  92. BT_DBG("peers %d addr %pMR type %d", atomic_read(&dev->peer_count),
  93. ba, type);
  94. rcu_read_lock();
  95. list_for_each_entry_rcu(peer, &dev->peers, list) {
  96. BT_DBG("dst addr %pMR dst type %d",
  97. &peer->chan->dst, peer->chan->dst_type);
  98. if (bacmp(&peer->chan->dst, ba))
  99. continue;
  100. if (type == peer->chan->dst_type) {
  101. rcu_read_unlock();
  102. return peer;
  103. }
  104. }
  105. rcu_read_unlock();
  106. return NULL;
  107. }
  108. static inline struct lowpan_peer *
  109. __peer_lookup_chan(struct lowpan_btle_dev *dev, struct l2cap_chan *chan)
  110. {
  111. struct lowpan_peer *peer;
  112. list_for_each_entry_rcu(peer, &dev->peers, list) {
  113. if (peer->chan == chan)
  114. return peer;
  115. }
  116. return NULL;
  117. }
  118. static inline struct lowpan_peer *
  119. __peer_lookup_conn(struct lowpan_btle_dev *dev, struct l2cap_conn *conn)
  120. {
  121. struct lowpan_peer *peer;
  122. list_for_each_entry_rcu(peer, &dev->peers, list) {
  123. if (peer->chan->conn == conn)
  124. return peer;
  125. }
  126. return NULL;
  127. }
  128. static inline struct lowpan_peer *peer_lookup_dst(struct lowpan_btle_dev *dev,
  129. struct in6_addr *daddr,
  130. struct sk_buff *skb)
  131. {
  132. struct lowpan_peer *peer;
  133. struct in6_addr *nexthop;
  134. struct rt6_info *rt = (struct rt6_info *)skb_dst(skb);
  135. int count = atomic_read(&dev->peer_count);
  136. BT_DBG("peers %d addr %pI6c rt %p", count, daddr, rt);
  137. /* If we have multiple 6lowpan peers, then check where we should
  138. * send the packet. If only one peer exists, then we can send the
  139. * packet right away.
  140. */
  141. if (count == 1) {
  142. rcu_read_lock();
  143. peer = list_first_or_null_rcu(&dev->peers, struct lowpan_peer,
  144. list);
  145. rcu_read_unlock();
  146. return peer;
  147. }
  148. if (!rt) {
  149. if (ipv6_addr_any(&lowpan_cb(skb)->gw)) {
  150. /* There is neither route nor gateway,
  151. * probably the destination is a direct peer.
  152. */
  153. nexthop = daddr;
  154. } else {
  155. /* There is a known gateway
  156. */
  157. nexthop = &lowpan_cb(skb)->gw;
  158. }
  159. } else {
  160. nexthop = rt6_nexthop(rt, daddr);
  161. /* We need to remember the address because it is needed
  162. * by bt_xmit() when sending the packet. In bt_xmit(), the
  163. * destination routing info is not set.
  164. */
  165. memcpy(&lowpan_cb(skb)->gw, nexthop, sizeof(struct in6_addr));
  166. }
  167. BT_DBG("gw %pI6c", nexthop);
  168. rcu_read_lock();
  169. list_for_each_entry_rcu(peer, &dev->peers, list) {
  170. BT_DBG("dst addr %pMR dst type %d ip %pI6c",
  171. &peer->chan->dst, peer->chan->dst_type,
  172. &peer->peer_addr);
  173. if (!ipv6_addr_cmp(&peer->peer_addr, nexthop)) {
  174. rcu_read_unlock();
  175. return peer;
  176. }
  177. }
  178. rcu_read_unlock();
  179. return NULL;
  180. }
  181. static struct lowpan_peer *lookup_peer(struct l2cap_conn *conn)
  182. {
  183. struct lowpan_btle_dev *entry;
  184. struct lowpan_peer *peer = NULL;
  185. rcu_read_lock();
  186. list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
  187. peer = __peer_lookup_conn(entry, conn);
  188. if (peer)
  189. break;
  190. }
  191. rcu_read_unlock();
  192. return peer;
  193. }
  194. static struct lowpan_btle_dev *lookup_dev(struct l2cap_conn *conn)
  195. {
  196. struct lowpan_btle_dev *entry;
  197. struct lowpan_btle_dev *dev = NULL;
  198. rcu_read_lock();
  199. list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
  200. if (conn->hcon->hdev == entry->hdev) {
  201. dev = entry;
  202. break;
  203. }
  204. }
  205. rcu_read_unlock();
  206. return dev;
  207. }
  208. static int give_skb_to_upper(struct sk_buff *skb, struct net_device *dev)
  209. {
  210. struct sk_buff *skb_cp;
  211. skb_cp = skb_copy(skb, GFP_ATOMIC);
  212. if (!skb_cp)
  213. return NET_RX_DROP;
  214. return netif_rx_ni(skb_cp);
  215. }
  216. static int iphc_decompress(struct sk_buff *skb, struct net_device *netdev,
  217. struct lowpan_peer *peer)
  218. {
  219. const u8 *saddr;
  220. saddr = peer->lladdr;
  221. return lowpan_header_decompress(skb, netdev, netdev->dev_addr, saddr);
  222. }
  223. static int recv_pkt(struct sk_buff *skb, struct net_device *dev,
  224. struct lowpan_peer *peer)
  225. {
  226. struct sk_buff *local_skb;
  227. int ret;
  228. if (!netif_running(dev))
  229. goto drop;
  230. if (dev->type != ARPHRD_6LOWPAN || !skb->len)
  231. goto drop;
  232. skb_reset_network_header(skb);
  233. skb = skb_share_check(skb, GFP_ATOMIC);
  234. if (!skb)
  235. goto drop;
  236. /* check that it's our buffer */
  237. if (lowpan_is_ipv6(*skb_network_header(skb))) {
  238. /* Pull off the 1-byte of 6lowpan header. */
  239. skb_pull(skb, 1);
  240. /* Copy the packet so that the IPv6 header is
  241. * properly aligned.
  242. */
  243. local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
  244. skb_tailroom(skb), GFP_ATOMIC);
  245. if (!local_skb)
  246. goto drop;
  247. local_skb->protocol = htons(ETH_P_IPV6);
  248. local_skb->pkt_type = PACKET_HOST;
  249. local_skb->dev = dev;
  250. skb_set_transport_header(local_skb, sizeof(struct ipv6hdr));
  251. if (give_skb_to_upper(local_skb, dev) != NET_RX_SUCCESS) {
  252. kfree_skb(local_skb);
  253. goto drop;
  254. }
  255. dev->stats.rx_bytes += skb->len;
  256. dev->stats.rx_packets++;
  257. consume_skb(local_skb);
  258. consume_skb(skb);
  259. } else if (lowpan_is_iphc(*skb_network_header(skb))) {
  260. local_skb = skb_clone(skb, GFP_ATOMIC);
  261. if (!local_skb)
  262. goto drop;
  263. local_skb->dev = dev;
  264. ret = iphc_decompress(local_skb, dev, peer);
  265. if (ret < 0) {
  266. BT_DBG("iphc_decompress failed: %d", ret);
  267. kfree_skb(local_skb);
  268. goto drop;
  269. }
  270. local_skb->protocol = htons(ETH_P_IPV6);
  271. local_skb->pkt_type = PACKET_HOST;
  272. if (give_skb_to_upper(local_skb, dev)
  273. != NET_RX_SUCCESS) {
  274. kfree_skb(local_skb);
  275. goto drop;
  276. }
  277. dev->stats.rx_bytes += skb->len;
  278. dev->stats.rx_packets++;
  279. consume_skb(local_skb);
  280. consume_skb(skb);
  281. } else {
  282. BT_DBG("unknown packet type");
  283. goto drop;
  284. }
  285. return NET_RX_SUCCESS;
  286. drop:
  287. dev->stats.rx_dropped++;
  288. return NET_RX_DROP;
  289. }
  290. /* Packet from BT LE device */
  291. static int chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
  292. {
  293. struct lowpan_btle_dev *dev;
  294. struct lowpan_peer *peer;
  295. int err;
  296. peer = lookup_peer(chan->conn);
  297. if (!peer)
  298. return -ENOENT;
  299. dev = lookup_dev(chan->conn);
  300. if (!dev || !dev->netdev)
  301. return -ENOENT;
  302. err = recv_pkt(skb, dev->netdev, peer);
  303. if (err) {
  304. BT_DBG("recv pkt %d", err);
  305. err = -EAGAIN;
  306. }
  307. return err;
  308. }
  309. static int setup_header(struct sk_buff *skb, struct net_device *netdev,
  310. bdaddr_t *peer_addr, u8 *peer_addr_type)
  311. {
  312. struct in6_addr ipv6_daddr;
  313. struct ipv6hdr *hdr;
  314. struct lowpan_btle_dev *dev;
  315. struct lowpan_peer *peer;
  316. u8 *daddr;
  317. int err, status = 0;
  318. hdr = ipv6_hdr(skb);
  319. dev = lowpan_btle_dev(netdev);
  320. memcpy(&ipv6_daddr, &hdr->daddr, sizeof(ipv6_daddr));
  321. if (ipv6_addr_is_multicast(&ipv6_daddr)) {
  322. lowpan_cb(skb)->chan = NULL;
  323. daddr = NULL;
  324. } else {
  325. BT_DBG("dest IP %pI6c", &ipv6_daddr);
  326. /* The packet might be sent to 6lowpan interface
  327. * because of routing (either via default route
  328. * or user set route) so get peer according to
  329. * the destination address.
  330. */
  331. peer = peer_lookup_dst(dev, &ipv6_daddr, skb);
  332. if (!peer) {
  333. BT_DBG("no such peer");
  334. return -ENOENT;
  335. }
  336. daddr = peer->lladdr;
  337. *peer_addr = peer->chan->dst;
  338. *peer_addr_type = peer->chan->dst_type;
  339. lowpan_cb(skb)->chan = peer->chan;
  340. status = 1;
  341. }
  342. lowpan_header_compress(skb, netdev, daddr, dev->netdev->dev_addr);
  343. err = dev_hard_header(skb, netdev, ETH_P_IPV6, NULL, NULL, 0);
  344. if (err < 0)
  345. return err;
  346. return status;
  347. }
  348. static int header_create(struct sk_buff *skb, struct net_device *netdev,
  349. unsigned short type, const void *_daddr,
  350. const void *_saddr, unsigned int len)
  351. {
  352. if (type != ETH_P_IPV6)
  353. return -EINVAL;
  354. return 0;
  355. }
  356. /* Packet to BT LE device */
  357. static int send_pkt(struct l2cap_chan *chan, struct sk_buff *skb,
  358. struct net_device *netdev)
  359. {
  360. struct msghdr msg;
  361. struct kvec iv;
  362. int err;
  363. /* Remember the skb so that we can send EAGAIN to the caller if
  364. * we run out of credits.
  365. */
  366. chan->data = skb;
  367. iv.iov_base = skb->data;
  368. iv.iov_len = skb->len;
  369. memset(&msg, 0, sizeof(msg));
  370. iov_iter_kvec(&msg.msg_iter, WRITE | ITER_KVEC, &iv, 1, skb->len);
  371. err = l2cap_chan_send(chan, &msg, skb->len);
  372. if (err > 0) {
  373. netdev->stats.tx_bytes += err;
  374. netdev->stats.tx_packets++;
  375. return 0;
  376. }
  377. if (err < 0)
  378. netdev->stats.tx_errors++;
  379. return err;
  380. }
  381. static int send_mcast_pkt(struct sk_buff *skb, struct net_device *netdev)
  382. {
  383. struct sk_buff *local_skb;
  384. struct lowpan_btle_dev *entry;
  385. int err = 0;
  386. rcu_read_lock();
  387. list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
  388. struct lowpan_peer *pentry;
  389. struct lowpan_btle_dev *dev;
  390. if (entry->netdev != netdev)
  391. continue;
  392. dev = lowpan_btle_dev(entry->netdev);
  393. list_for_each_entry_rcu(pentry, &dev->peers, list) {
  394. int ret;
  395. local_skb = skb_clone(skb, GFP_ATOMIC);
  396. BT_DBG("xmit %s to %pMR type %d IP %pI6c chan %p",
  397. netdev->name,
  398. &pentry->chan->dst, pentry->chan->dst_type,
  399. &pentry->peer_addr, pentry->chan);
  400. ret = send_pkt(pentry->chan, local_skb, netdev);
  401. if (ret < 0)
  402. err = ret;
  403. kfree_skb(local_skb);
  404. }
  405. }
  406. rcu_read_unlock();
  407. return err;
  408. }
  409. static netdev_tx_t bt_xmit(struct sk_buff *skb, struct net_device *netdev)
  410. {
  411. int err = 0;
  412. bdaddr_t addr;
  413. u8 addr_type;
  414. /* We must take a copy of the skb before we modify/replace the ipv6
  415. * header as the header could be used elsewhere
  416. */
  417. skb = skb_unshare(skb, GFP_ATOMIC);
  418. if (!skb)
  419. return NET_XMIT_DROP;
  420. /* Return values from setup_header()
  421. * <0 - error, packet is dropped
  422. * 0 - this is a multicast packet
  423. * 1 - this is unicast packet
  424. */
  425. err = setup_header(skb, netdev, &addr, &addr_type);
  426. if (err < 0) {
  427. kfree_skb(skb);
  428. return NET_XMIT_DROP;
  429. }
  430. if (err) {
  431. if (lowpan_cb(skb)->chan) {
  432. BT_DBG("xmit %s to %pMR type %d IP %pI6c chan %p",
  433. netdev->name, &addr, addr_type,
  434. &lowpan_cb(skb)->addr, lowpan_cb(skb)->chan);
  435. err = send_pkt(lowpan_cb(skb)->chan, skb, netdev);
  436. } else {
  437. err = -ENOENT;
  438. }
  439. } else {
  440. /* We need to send the packet to every device behind this
  441. * interface.
  442. */
  443. err = send_mcast_pkt(skb, netdev);
  444. }
  445. dev_kfree_skb(skb);
  446. if (err)
  447. BT_DBG("ERROR: xmit failed (%d)", err);
  448. return err < 0 ? NET_XMIT_DROP : err;
  449. }
  450. static int bt_dev_init(struct net_device *dev)
  451. {
  452. netdev_lockdep_set_classes(dev);
  453. return 0;
  454. }
  455. static const struct net_device_ops netdev_ops = {
  456. .ndo_init = bt_dev_init,
  457. .ndo_start_xmit = bt_xmit,
  458. };
  459. static struct header_ops header_ops = {
  460. .create = header_create,
  461. };
  462. static void netdev_setup(struct net_device *dev)
  463. {
  464. dev->hard_header_len = 0;
  465. dev->needed_tailroom = 0;
  466. dev->flags = IFF_RUNNING | IFF_MULTICAST;
  467. dev->watchdog_timeo = 0;
  468. dev->tx_queue_len = DEFAULT_TX_QUEUE_LEN;
  469. dev->netdev_ops = &netdev_ops;
  470. dev->header_ops = &header_ops;
  471. dev->needs_free_netdev = true;
  472. }
  473. static struct device_type bt_type = {
  474. .name = "bluetooth",
  475. };
  476. static void ifup(struct net_device *netdev)
  477. {
  478. int err;
  479. rtnl_lock();
  480. err = dev_open(netdev);
  481. if (err < 0)
  482. BT_INFO("iface %s cannot be opened (%d)", netdev->name, err);
  483. rtnl_unlock();
  484. }
  485. static void ifdown(struct net_device *netdev)
  486. {
  487. rtnl_lock();
  488. dev_close(netdev);
  489. rtnl_unlock();
  490. }
  491. static void do_notify_peers(struct work_struct *work)
  492. {
  493. struct lowpan_btle_dev *dev = container_of(work, struct lowpan_btle_dev,
  494. notify_peers.work);
  495. netdev_notify_peers(dev->netdev); /* send neighbour adv at startup */
  496. }
  497. static bool is_bt_6lowpan(struct hci_conn *hcon)
  498. {
  499. if (hcon->type != LE_LINK)
  500. return false;
  501. if (!enable_6lowpan)
  502. return false;
  503. return true;
  504. }
  505. static struct l2cap_chan *chan_create(void)
  506. {
  507. struct l2cap_chan *chan;
  508. chan = l2cap_chan_create();
  509. if (!chan)
  510. return NULL;
  511. l2cap_chan_set_defaults(chan);
  512. chan->chan_type = L2CAP_CHAN_CONN_ORIENTED;
  513. chan->mode = L2CAP_MODE_LE_FLOWCTL;
  514. chan->imtu = 1280;
  515. return chan;
  516. }
  517. static struct l2cap_chan *add_peer_chan(struct l2cap_chan *chan,
  518. struct lowpan_btle_dev *dev,
  519. bool new_netdev)
  520. {
  521. struct lowpan_peer *peer;
  522. peer = kzalloc(sizeof(*peer), GFP_ATOMIC);
  523. if (!peer)
  524. return NULL;
  525. peer->chan = chan;
  526. memset(&peer->peer_addr, 0, sizeof(struct in6_addr));
  527. baswap((void *)peer->lladdr, &chan->dst);
  528. lowpan_iphc_uncompress_eui48_lladdr(&peer->peer_addr, peer->lladdr);
  529. spin_lock(&devices_lock);
  530. INIT_LIST_HEAD(&peer->list);
  531. peer_add(dev, peer);
  532. spin_unlock(&devices_lock);
  533. /* Notifying peers about us needs to be done without locks held */
  534. if (new_netdev)
  535. INIT_DELAYED_WORK(&dev->notify_peers, do_notify_peers);
  536. schedule_delayed_work(&dev->notify_peers, msecs_to_jiffies(100));
  537. return peer->chan;
  538. }
  539. static int setup_netdev(struct l2cap_chan *chan, struct lowpan_btle_dev **dev)
  540. {
  541. struct net_device *netdev;
  542. int err = 0;
  543. netdev = alloc_netdev(LOWPAN_PRIV_SIZE(sizeof(struct lowpan_btle_dev)),
  544. IFACE_NAME_TEMPLATE, NET_NAME_UNKNOWN,
  545. netdev_setup);
  546. if (!netdev)
  547. return -ENOMEM;
  548. netdev->addr_assign_type = NET_ADDR_PERM;
  549. baswap((void *)netdev->dev_addr, &chan->src);
  550. netdev->netdev_ops = &netdev_ops;
  551. SET_NETDEV_DEV(netdev, &chan->conn->hcon->hdev->dev);
  552. SET_NETDEV_DEVTYPE(netdev, &bt_type);
  553. *dev = lowpan_btle_dev(netdev);
  554. (*dev)->netdev = netdev;
  555. (*dev)->hdev = chan->conn->hcon->hdev;
  556. INIT_LIST_HEAD(&(*dev)->peers);
  557. spin_lock(&devices_lock);
  558. INIT_LIST_HEAD(&(*dev)->list);
  559. list_add_rcu(&(*dev)->list, &bt_6lowpan_devices);
  560. spin_unlock(&devices_lock);
  561. err = lowpan_register_netdev(netdev, LOWPAN_LLTYPE_BTLE);
  562. if (err < 0) {
  563. BT_INFO("register_netdev failed %d", err);
  564. spin_lock(&devices_lock);
  565. list_del_rcu(&(*dev)->list);
  566. spin_unlock(&devices_lock);
  567. free_netdev(netdev);
  568. goto out;
  569. }
  570. BT_DBG("ifindex %d peer bdaddr %pMR type %d my addr %pMR type %d",
  571. netdev->ifindex, &chan->dst, chan->dst_type,
  572. &chan->src, chan->src_type);
  573. set_bit(__LINK_STATE_PRESENT, &netdev->state);
  574. return 0;
  575. out:
  576. return err;
  577. }
  578. static inline void chan_ready_cb(struct l2cap_chan *chan)
  579. {
  580. struct lowpan_btle_dev *dev;
  581. bool new_netdev = false;
  582. dev = lookup_dev(chan->conn);
  583. BT_DBG("chan %p conn %p dev %p", chan, chan->conn, dev);
  584. if (!dev) {
  585. if (setup_netdev(chan, &dev) < 0) {
  586. l2cap_chan_del(chan, -ENOENT);
  587. return;
  588. }
  589. new_netdev = true;
  590. }
  591. if (!try_module_get(THIS_MODULE))
  592. return;
  593. add_peer_chan(chan, dev, new_netdev);
  594. ifup(dev->netdev);
  595. }
  596. static inline struct l2cap_chan *chan_new_conn_cb(struct l2cap_chan *pchan)
  597. {
  598. struct l2cap_chan *chan;
  599. chan = chan_create();
  600. if (!chan)
  601. return NULL;
  602. chan->ops = pchan->ops;
  603. BT_DBG("chan %p pchan %p", chan, pchan);
  604. return chan;
  605. }
  606. static void delete_netdev(struct work_struct *work)
  607. {
  608. struct lowpan_btle_dev *entry = container_of(work,
  609. struct lowpan_btle_dev,
  610. delete_netdev);
  611. lowpan_unregister_netdev(entry->netdev);
  612. /* The entry pointer is deleted by the netdev destructor. */
  613. }
  614. static void chan_close_cb(struct l2cap_chan *chan)
  615. {
  616. struct lowpan_btle_dev *entry;
  617. struct lowpan_btle_dev *dev = NULL;
  618. struct lowpan_peer *peer;
  619. int err = -ENOENT;
  620. bool last = false, remove = true;
  621. BT_DBG("chan %p conn %p", chan, chan->conn);
  622. if (chan->conn && chan->conn->hcon) {
  623. if (!is_bt_6lowpan(chan->conn->hcon))
  624. return;
  625. /* If conn is set, then the netdev is also there and we should
  626. * not remove it.
  627. */
  628. remove = false;
  629. }
  630. spin_lock(&devices_lock);
  631. list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
  632. dev = lowpan_btle_dev(entry->netdev);
  633. peer = __peer_lookup_chan(dev, chan);
  634. if (peer) {
  635. last = peer_del(dev, peer);
  636. err = 0;
  637. BT_DBG("dev %p removing %speer %p", dev,
  638. last ? "last " : "1 ", peer);
  639. BT_DBG("chan %p orig refcnt %d", chan,
  640. kref_read(&chan->kref));
  641. l2cap_chan_put(chan);
  642. break;
  643. }
  644. }
  645. if (!err && last && dev && !atomic_read(&dev->peer_count)) {
  646. spin_unlock(&devices_lock);
  647. cancel_delayed_work_sync(&dev->notify_peers);
  648. ifdown(dev->netdev);
  649. if (remove) {
  650. INIT_WORK(&entry->delete_netdev, delete_netdev);
  651. schedule_work(&entry->delete_netdev);
  652. }
  653. } else {
  654. spin_unlock(&devices_lock);
  655. }
  656. return;
  657. }
  658. static void chan_state_change_cb(struct l2cap_chan *chan, int state, int err)
  659. {
  660. BT_DBG("chan %p conn %p state %s err %d", chan, chan->conn,
  661. state_to_string(state), err);
  662. }
  663. static struct sk_buff *chan_alloc_skb_cb(struct l2cap_chan *chan,
  664. unsigned long hdr_len,
  665. unsigned long len, int nb)
  666. {
  667. /* Note that we must allocate using GFP_ATOMIC here as
  668. * this function is called originally from netdev hard xmit
  669. * function in atomic context.
  670. */
  671. return bt_skb_alloc(hdr_len + len, GFP_ATOMIC);
  672. }
  673. static void chan_suspend_cb(struct l2cap_chan *chan)
  674. {
  675. struct lowpan_btle_dev *dev;
  676. BT_DBG("chan %p suspend", chan);
  677. dev = lookup_dev(chan->conn);
  678. if (!dev || !dev->netdev)
  679. return;
  680. netif_stop_queue(dev->netdev);
  681. }
  682. static void chan_resume_cb(struct l2cap_chan *chan)
  683. {
  684. struct lowpan_btle_dev *dev;
  685. BT_DBG("chan %p resume", chan);
  686. dev = lookup_dev(chan->conn);
  687. if (!dev || !dev->netdev)
  688. return;
  689. netif_wake_queue(dev->netdev);
  690. }
  691. static long chan_get_sndtimeo_cb(struct l2cap_chan *chan)
  692. {
  693. return L2CAP_CONN_TIMEOUT;
  694. }
  695. static const struct l2cap_ops bt_6lowpan_chan_ops = {
  696. .name = "L2CAP 6LoWPAN channel",
  697. .new_connection = chan_new_conn_cb,
  698. .recv = chan_recv_cb,
  699. .close = chan_close_cb,
  700. .state_change = chan_state_change_cb,
  701. .ready = chan_ready_cb,
  702. .resume = chan_resume_cb,
  703. .suspend = chan_suspend_cb,
  704. .get_sndtimeo = chan_get_sndtimeo_cb,
  705. .alloc_skb = chan_alloc_skb_cb,
  706. .teardown = l2cap_chan_no_teardown,
  707. .defer = l2cap_chan_no_defer,
  708. .set_shutdown = l2cap_chan_no_set_shutdown,
  709. };
  710. static inline __u8 bdaddr_type(__u8 type)
  711. {
  712. if (type == ADDR_LE_DEV_PUBLIC)
  713. return BDADDR_LE_PUBLIC;
  714. else
  715. return BDADDR_LE_RANDOM;
  716. }
  717. static int bt_6lowpan_connect(bdaddr_t *addr, u8 dst_type)
  718. {
  719. struct l2cap_chan *chan;
  720. int err;
  721. chan = chan_create();
  722. if (!chan)
  723. return -EINVAL;
  724. chan->ops = &bt_6lowpan_chan_ops;
  725. err = l2cap_chan_connect(chan, cpu_to_le16(L2CAP_PSM_IPSP), 0,
  726. addr, dst_type);
  727. BT_DBG("chan %p err %d", chan, err);
  728. if (err < 0)
  729. l2cap_chan_put(chan);
  730. return err;
  731. }
  732. static int bt_6lowpan_disconnect(struct l2cap_conn *conn, u8 dst_type)
  733. {
  734. struct lowpan_peer *peer;
  735. BT_DBG("conn %p dst type %d", conn, dst_type);
  736. peer = lookup_peer(conn);
  737. if (!peer)
  738. return -ENOENT;
  739. BT_DBG("peer %p chan %p", peer, peer->chan);
  740. l2cap_chan_close(peer->chan, ENOENT);
  741. return 0;
  742. }
  743. static struct l2cap_chan *bt_6lowpan_listen(void)
  744. {
  745. bdaddr_t *addr = BDADDR_ANY;
  746. struct l2cap_chan *chan;
  747. int err;
  748. if (!enable_6lowpan)
  749. return NULL;
  750. chan = chan_create();
  751. if (!chan)
  752. return NULL;
  753. chan->ops = &bt_6lowpan_chan_ops;
  754. chan->state = BT_LISTEN;
  755. chan->src_type = BDADDR_LE_PUBLIC;
  756. atomic_set(&chan->nesting, L2CAP_NESTING_PARENT);
  757. BT_DBG("chan %p src type %d", chan, chan->src_type);
  758. err = l2cap_add_psm(chan, addr, cpu_to_le16(L2CAP_PSM_IPSP));
  759. if (err) {
  760. l2cap_chan_put(chan);
  761. BT_ERR("psm cannot be added err %d", err);
  762. return NULL;
  763. }
  764. return chan;
  765. }
  766. static int get_l2cap_conn(char *buf, bdaddr_t *addr, u8 *addr_type,
  767. struct l2cap_conn **conn)
  768. {
  769. struct hci_conn *hcon;
  770. struct hci_dev *hdev;
  771. int n;
  772. n = sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx %hhu",
  773. &addr->b[5], &addr->b[4], &addr->b[3],
  774. &addr->b[2], &addr->b[1], &addr->b[0],
  775. addr_type);
  776. if (n < 7)
  777. return -EINVAL;
  778. /* The LE_PUBLIC address type is ignored because of BDADDR_ANY */
  779. hdev = hci_get_route(addr, BDADDR_ANY, BDADDR_LE_PUBLIC);
  780. if (!hdev)
  781. return -ENOENT;
  782. hci_dev_lock(hdev);
  783. hcon = hci_conn_hash_lookup_le(hdev, addr, *addr_type);
  784. hci_dev_unlock(hdev);
  785. if (!hcon)
  786. return -ENOENT;
  787. *conn = (struct l2cap_conn *)hcon->l2cap_data;
  788. BT_DBG("conn %p dst %pMR type %d", *conn, &hcon->dst, hcon->dst_type);
  789. return 0;
  790. }
  791. static void disconnect_all_peers(void)
  792. {
  793. struct lowpan_btle_dev *entry;
  794. struct lowpan_peer *peer, *tmp_peer, *new_peer;
  795. struct list_head peers;
  796. INIT_LIST_HEAD(&peers);
  797. /* We make a separate list of peers as the close_cb() will
  798. * modify the device peers list so it is better not to mess
  799. * with the same list at the same time.
  800. */
  801. rcu_read_lock();
  802. list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
  803. list_for_each_entry_rcu(peer, &entry->peers, list) {
  804. new_peer = kmalloc(sizeof(*new_peer), GFP_ATOMIC);
  805. if (!new_peer)
  806. break;
  807. new_peer->chan = peer->chan;
  808. INIT_LIST_HEAD(&new_peer->list);
  809. list_add(&new_peer->list, &peers);
  810. }
  811. }
  812. rcu_read_unlock();
  813. spin_lock(&devices_lock);
  814. list_for_each_entry_safe(peer, tmp_peer, &peers, list) {
  815. l2cap_chan_close(peer->chan, ENOENT);
  816. list_del_rcu(&peer->list);
  817. kfree_rcu(peer, rcu);
  818. }
  819. spin_unlock(&devices_lock);
  820. }
  821. struct set_enable {
  822. struct work_struct work;
  823. bool flag;
  824. };
  825. static void do_enable_set(struct work_struct *work)
  826. {
  827. struct set_enable *set_enable = container_of(work,
  828. struct set_enable, work);
  829. if (!set_enable->flag || enable_6lowpan != set_enable->flag)
  830. /* Disconnect existing connections if 6lowpan is
  831. * disabled
  832. */
  833. disconnect_all_peers();
  834. enable_6lowpan = set_enable->flag;
  835. mutex_lock(&set_lock);
  836. if (listen_chan) {
  837. l2cap_chan_close(listen_chan, 0);
  838. l2cap_chan_put(listen_chan);
  839. }
  840. listen_chan = bt_6lowpan_listen();
  841. mutex_unlock(&set_lock);
  842. kfree(set_enable);
  843. }
  844. static int lowpan_enable_set(void *data, u64 val)
  845. {
  846. struct set_enable *set_enable;
  847. set_enable = kzalloc(sizeof(*set_enable), GFP_KERNEL);
  848. if (!set_enable)
  849. return -ENOMEM;
  850. set_enable->flag = !!val;
  851. INIT_WORK(&set_enable->work, do_enable_set);
  852. schedule_work(&set_enable->work);
  853. return 0;
  854. }
  855. static int lowpan_enable_get(void *data, u64 *val)
  856. {
  857. *val = enable_6lowpan;
  858. return 0;
  859. }
  860. DEFINE_SIMPLE_ATTRIBUTE(lowpan_enable_fops, lowpan_enable_get,
  861. lowpan_enable_set, "%llu\n");
  862. static ssize_t lowpan_control_write(struct file *fp,
  863. const char __user *user_buffer,
  864. size_t count,
  865. loff_t *position)
  866. {
  867. char buf[32];
  868. size_t buf_size = min(count, sizeof(buf) - 1);
  869. int ret;
  870. bdaddr_t addr;
  871. u8 addr_type;
  872. struct l2cap_conn *conn = NULL;
  873. if (copy_from_user(buf, user_buffer, buf_size))
  874. return -EFAULT;
  875. buf[buf_size] = '\0';
  876. if (memcmp(buf, "connect ", 8) == 0) {
  877. ret = get_l2cap_conn(&buf[8], &addr, &addr_type, &conn);
  878. if (ret == -EINVAL)
  879. return ret;
  880. mutex_lock(&set_lock);
  881. if (listen_chan) {
  882. l2cap_chan_close(listen_chan, 0);
  883. l2cap_chan_put(listen_chan);
  884. listen_chan = NULL;
  885. }
  886. mutex_unlock(&set_lock);
  887. if (conn) {
  888. struct lowpan_peer *peer;
  889. if (!is_bt_6lowpan(conn->hcon))
  890. return -EINVAL;
  891. peer = lookup_peer(conn);
  892. if (peer) {
  893. BT_DBG("6LoWPAN connection already exists");
  894. return -EALREADY;
  895. }
  896. BT_DBG("conn %p dst %pMR type %d user %d", conn,
  897. &conn->hcon->dst, conn->hcon->dst_type,
  898. addr_type);
  899. }
  900. ret = bt_6lowpan_connect(&addr, addr_type);
  901. if (ret < 0)
  902. return ret;
  903. return count;
  904. }
  905. if (memcmp(buf, "disconnect ", 11) == 0) {
  906. ret = get_l2cap_conn(&buf[11], &addr, &addr_type, &conn);
  907. if (ret < 0)
  908. return ret;
  909. ret = bt_6lowpan_disconnect(conn, addr_type);
  910. if (ret < 0)
  911. return ret;
  912. return count;
  913. }
  914. return count;
  915. }
  916. static int lowpan_control_show(struct seq_file *f, void *ptr)
  917. {
  918. struct lowpan_btle_dev *entry;
  919. struct lowpan_peer *peer;
  920. spin_lock(&devices_lock);
  921. list_for_each_entry(entry, &bt_6lowpan_devices, list) {
  922. list_for_each_entry(peer, &entry->peers, list)
  923. seq_printf(f, "%pMR (type %u)\n",
  924. &peer->chan->dst, peer->chan->dst_type);
  925. }
  926. spin_unlock(&devices_lock);
  927. return 0;
  928. }
  929. static int lowpan_control_open(struct inode *inode, struct file *file)
  930. {
  931. return single_open(file, lowpan_control_show, inode->i_private);
  932. }
  933. static const struct file_operations lowpan_control_fops = {
  934. .open = lowpan_control_open,
  935. .read = seq_read,
  936. .write = lowpan_control_write,
  937. .llseek = seq_lseek,
  938. .release = single_release,
  939. };
  940. static void disconnect_devices(void)
  941. {
  942. struct lowpan_btle_dev *entry, *tmp, *new_dev;
  943. struct list_head devices;
  944. INIT_LIST_HEAD(&devices);
  945. /* We make a separate list of devices because the unregister_netdev()
  946. * will call device_event() which will also want to modify the same
  947. * devices list.
  948. */
  949. rcu_read_lock();
  950. list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
  951. new_dev = kmalloc(sizeof(*new_dev), GFP_ATOMIC);
  952. if (!new_dev)
  953. break;
  954. new_dev->netdev = entry->netdev;
  955. INIT_LIST_HEAD(&new_dev->list);
  956. list_add_rcu(&new_dev->list, &devices);
  957. }
  958. rcu_read_unlock();
  959. list_for_each_entry_safe(entry, tmp, &devices, list) {
  960. ifdown(entry->netdev);
  961. BT_DBG("Unregistering netdev %s %p",
  962. entry->netdev->name, entry->netdev);
  963. lowpan_unregister_netdev(entry->netdev);
  964. kfree(entry);
  965. }
  966. }
  967. static int device_event(struct notifier_block *unused,
  968. unsigned long event, void *ptr)
  969. {
  970. struct net_device *netdev = netdev_notifier_info_to_dev(ptr);
  971. struct lowpan_btle_dev *entry;
  972. if (netdev->type != ARPHRD_6LOWPAN)
  973. return NOTIFY_DONE;
  974. switch (event) {
  975. case NETDEV_UNREGISTER:
  976. spin_lock(&devices_lock);
  977. list_for_each_entry(entry, &bt_6lowpan_devices, list) {
  978. if (entry->netdev == netdev) {
  979. BT_DBG("Unregistered netdev %s %p",
  980. netdev->name, netdev);
  981. list_del(&entry->list);
  982. break;
  983. }
  984. }
  985. spin_unlock(&devices_lock);
  986. break;
  987. }
  988. return NOTIFY_DONE;
  989. }
  990. static struct notifier_block bt_6lowpan_dev_notifier = {
  991. .notifier_call = device_event,
  992. };
  993. static int __init bt_6lowpan_init(void)
  994. {
  995. lowpan_enable_debugfs = debugfs_create_file("6lowpan_enable", 0644,
  996. bt_debugfs, NULL,
  997. &lowpan_enable_fops);
  998. lowpan_control_debugfs = debugfs_create_file("6lowpan_control", 0644,
  999. bt_debugfs, NULL,
  1000. &lowpan_control_fops);
  1001. return register_netdevice_notifier(&bt_6lowpan_dev_notifier);
  1002. }
  1003. static void __exit bt_6lowpan_exit(void)
  1004. {
  1005. debugfs_remove(lowpan_enable_debugfs);
  1006. debugfs_remove(lowpan_control_debugfs);
  1007. if (listen_chan) {
  1008. l2cap_chan_close(listen_chan, 0);
  1009. l2cap_chan_put(listen_chan);
  1010. }
  1011. disconnect_devices();
  1012. unregister_netdevice_notifier(&bt_6lowpan_dev_notifier);
  1013. }
  1014. module_init(bt_6lowpan_init);
  1015. module_exit(bt_6lowpan_exit);
  1016. MODULE_AUTHOR("Jukka Rissanen <jukka.rissanen@linux.intel.com>");
  1017. MODULE_DESCRIPTION("Bluetooth 6LoWPAN");
  1018. MODULE_VERSION(VERSION);
  1019. MODULE_LICENSE("GPL");