Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003    Copyright (c) 2013-2014 Intel Corp.
0004 
0005 */
0006 
0007 #include <linux/if_arp.h>
0008 #include <linux/netdevice.h>
0009 #include <linux/etherdevice.h>
0010 #include <linux/module.h>
0011 #include <linux/debugfs.h>
0012 
0013 #include <net/ipv6.h>
0014 #include <net/ip6_route.h>
0015 #include <net/addrconf.h>
0016 #include <net/pkt_sched.h>
0017 
0018 #include <net/bluetooth/bluetooth.h>
0019 #include <net/bluetooth/hci_core.h>
0020 #include <net/bluetooth/l2cap.h>
0021 
0022 #include <net/6lowpan.h> /* for the compression support */
0023 
0024 #define VERSION "0.1"
0025 
0026 static struct dentry *lowpan_enable_debugfs;
0027 static struct dentry *lowpan_control_debugfs;
0028 
0029 #define IFACE_NAME_TEMPLATE "bt%d"
0030 
0031 struct skb_cb {
0032     struct in6_addr addr;
0033     struct in6_addr gw;
0034     struct l2cap_chan *chan;
0035 };
0036 #define lowpan_cb(skb) ((struct skb_cb *)((skb)->cb))
0037 
0038 /* The devices list contains those devices that we are acting
0039  * as a proxy. The BT 6LoWPAN device is a virtual device that
0040  * connects to the Bluetooth LE device. The real connection to
0041  * BT device is done via l2cap layer. There exists one
0042  * virtual device / one BT 6LoWPAN network (=hciX device).
0043  * The list contains struct lowpan_dev elements.
0044  */
0045 static LIST_HEAD(bt_6lowpan_devices);
0046 static DEFINE_SPINLOCK(devices_lock);
0047 
0048 static bool enable_6lowpan;
0049 
0050 /* We are listening incoming connections via this channel
0051  */
0052 static struct l2cap_chan *listen_chan;
0053 static DEFINE_MUTEX(set_lock);
0054 
0055 struct lowpan_peer {
0056     struct list_head list;
0057     struct rcu_head rcu;
0058     struct l2cap_chan *chan;
0059 
0060     /* peer addresses in various formats */
0061     unsigned char lladdr[ETH_ALEN];
0062     struct in6_addr peer_addr;
0063 };
0064 
0065 struct lowpan_btle_dev {
0066     struct list_head list;
0067 
0068     struct hci_dev *hdev;
0069     struct net_device *netdev;
0070     struct list_head peers;
0071     atomic_t peer_count; /* number of items in peers list */
0072 
0073     struct work_struct delete_netdev;
0074     struct delayed_work notify_peers;
0075 };
0076 
0077 static inline struct lowpan_btle_dev *
0078 lowpan_btle_dev(const struct net_device *netdev)
0079 {
0080     return (struct lowpan_btle_dev *)lowpan_dev(netdev)->priv;
0081 }
0082 
0083 static inline void peer_add(struct lowpan_btle_dev *dev,
0084                 struct lowpan_peer *peer)
0085 {
0086     list_add_rcu(&peer->list, &dev->peers);
0087     atomic_inc(&dev->peer_count);
0088 }
0089 
0090 static inline bool peer_del(struct lowpan_btle_dev *dev,
0091                 struct lowpan_peer *peer)
0092 {
0093     list_del_rcu(&peer->list);
0094     kfree_rcu(peer, rcu);
0095 
0096     module_put(THIS_MODULE);
0097 
0098     if (atomic_dec_and_test(&dev->peer_count)) {
0099         BT_DBG("last peer");
0100         return true;
0101     }
0102 
0103     return false;
0104 }
0105 
0106 static inline struct lowpan_peer *
0107 __peer_lookup_chan(struct lowpan_btle_dev *dev, struct l2cap_chan *chan)
0108 {
0109     struct lowpan_peer *peer;
0110 
0111     list_for_each_entry_rcu(peer, &dev->peers, list) {
0112         if (peer->chan == chan)
0113             return peer;
0114     }
0115 
0116     return NULL;
0117 }
0118 
0119 static inline struct lowpan_peer *
0120 __peer_lookup_conn(struct lowpan_btle_dev *dev, struct l2cap_conn *conn)
0121 {
0122     struct lowpan_peer *peer;
0123 
0124     list_for_each_entry_rcu(peer, &dev->peers, list) {
0125         if (peer->chan->conn == conn)
0126             return peer;
0127     }
0128 
0129     return NULL;
0130 }
0131 
0132 static inline struct lowpan_peer *peer_lookup_dst(struct lowpan_btle_dev *dev,
0133                           struct in6_addr *daddr,
0134                           struct sk_buff *skb)
0135 {
0136     struct rt6_info *rt = (struct rt6_info *)skb_dst(skb);
0137     int count = atomic_read(&dev->peer_count);
0138     const struct in6_addr *nexthop;
0139     struct lowpan_peer *peer;
0140     struct neighbour *neigh;
0141 
0142     BT_DBG("peers %d addr %pI6c rt %p", count, daddr, rt);
0143 
0144     if (!rt) {
0145         if (ipv6_addr_any(&lowpan_cb(skb)->gw)) {
0146             /* There is neither route nor gateway,
0147              * probably the destination is a direct peer.
0148              */
0149             nexthop = daddr;
0150         } else {
0151             /* There is a known gateway
0152              */
0153             nexthop = &lowpan_cb(skb)->gw;
0154         }
0155     } else {
0156         nexthop = rt6_nexthop(rt, daddr);
0157 
0158         /* We need to remember the address because it is needed
0159          * by bt_xmit() when sending the packet. In bt_xmit(), the
0160          * destination routing info is not set.
0161          */
0162         memcpy(&lowpan_cb(skb)->gw, nexthop, sizeof(struct in6_addr));
0163     }
0164 
0165     BT_DBG("gw %pI6c", nexthop);
0166 
0167     rcu_read_lock();
0168 
0169     list_for_each_entry_rcu(peer, &dev->peers, list) {
0170         BT_DBG("dst addr %pMR dst type %u ip %pI6c",
0171                &peer->chan->dst, peer->chan->dst_type,
0172                &peer->peer_addr);
0173 
0174         if (!ipv6_addr_cmp(&peer->peer_addr, nexthop)) {
0175             rcu_read_unlock();
0176             return peer;
0177         }
0178     }
0179 
0180     /* use the neighbour cache for matching addresses assigned by SLAAC */
0181     neigh = __ipv6_neigh_lookup(dev->netdev, nexthop);
0182     if (neigh) {
0183         list_for_each_entry_rcu(peer, &dev->peers, list) {
0184             if (!memcmp(neigh->ha, peer->lladdr, ETH_ALEN)) {
0185                 neigh_release(neigh);
0186                 rcu_read_unlock();
0187                 return peer;
0188             }
0189         }
0190         neigh_release(neigh);
0191     }
0192 
0193     rcu_read_unlock();
0194 
0195     return NULL;
0196 }
0197 
0198 static struct lowpan_peer *lookup_peer(struct l2cap_conn *conn)
0199 {
0200     struct lowpan_btle_dev *entry;
0201     struct lowpan_peer *peer = NULL;
0202 
0203     rcu_read_lock();
0204 
0205     list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
0206         peer = __peer_lookup_conn(entry, conn);
0207         if (peer)
0208             break;
0209     }
0210 
0211     rcu_read_unlock();
0212 
0213     return peer;
0214 }
0215 
0216 static struct lowpan_btle_dev *lookup_dev(struct l2cap_conn *conn)
0217 {
0218     struct lowpan_btle_dev *entry;
0219     struct lowpan_btle_dev *dev = NULL;
0220 
0221     rcu_read_lock();
0222 
0223     list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
0224         if (conn->hcon->hdev == entry->hdev) {
0225             dev = entry;
0226             break;
0227         }
0228     }
0229 
0230     rcu_read_unlock();
0231 
0232     return dev;
0233 }
0234 
0235 static int give_skb_to_upper(struct sk_buff *skb, struct net_device *dev)
0236 {
0237     struct sk_buff *skb_cp;
0238 
0239     skb_cp = skb_copy(skb, GFP_ATOMIC);
0240     if (!skb_cp)
0241         return NET_RX_DROP;
0242 
0243     return netif_rx(skb_cp);
0244 }
0245 
0246 static int iphc_decompress(struct sk_buff *skb, struct net_device *netdev,
0247                struct lowpan_peer *peer)
0248 {
0249     const u8 *saddr;
0250 
0251     saddr = peer->lladdr;
0252 
0253     return lowpan_header_decompress(skb, netdev, netdev->dev_addr, saddr);
0254 }
0255 
0256 static int recv_pkt(struct sk_buff *skb, struct net_device *dev,
0257             struct lowpan_peer *peer)
0258 {
0259     struct sk_buff *local_skb;
0260     int ret;
0261 
0262     if (!netif_running(dev))
0263         goto drop;
0264 
0265     if (dev->type != ARPHRD_6LOWPAN || !skb->len)
0266         goto drop;
0267 
0268     skb_reset_network_header(skb);
0269 
0270     skb = skb_share_check(skb, GFP_ATOMIC);
0271     if (!skb)
0272         goto drop;
0273 
0274     /* check that it's our buffer */
0275     if (lowpan_is_ipv6(*skb_network_header(skb))) {
0276         /* Pull off the 1-byte of 6lowpan header. */
0277         skb_pull(skb, 1);
0278 
0279         /* Copy the packet so that the IPv6 header is
0280          * properly aligned.
0281          */
0282         local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
0283                         skb_tailroom(skb), GFP_ATOMIC);
0284         if (!local_skb)
0285             goto drop;
0286 
0287         local_skb->protocol = htons(ETH_P_IPV6);
0288         local_skb->pkt_type = PACKET_HOST;
0289         local_skb->dev = dev;
0290 
0291         skb_set_transport_header(local_skb, sizeof(struct ipv6hdr));
0292 
0293         if (give_skb_to_upper(local_skb, dev) != NET_RX_SUCCESS) {
0294             kfree_skb(local_skb);
0295             goto drop;
0296         }
0297 
0298         dev->stats.rx_bytes += skb->len;
0299         dev->stats.rx_packets++;
0300 
0301         consume_skb(local_skb);
0302         consume_skb(skb);
0303     } else if (lowpan_is_iphc(*skb_network_header(skb))) {
0304         local_skb = skb_clone(skb, GFP_ATOMIC);
0305         if (!local_skb)
0306             goto drop;
0307 
0308         local_skb->dev = dev;
0309 
0310         ret = iphc_decompress(local_skb, dev, peer);
0311         if (ret < 0) {
0312             BT_DBG("iphc_decompress failed: %d", ret);
0313             kfree_skb(local_skb);
0314             goto drop;
0315         }
0316 
0317         local_skb->protocol = htons(ETH_P_IPV6);
0318         local_skb->pkt_type = PACKET_HOST;
0319 
0320         if (give_skb_to_upper(local_skb, dev)
0321                 != NET_RX_SUCCESS) {
0322             kfree_skb(local_skb);
0323             goto drop;
0324         }
0325 
0326         dev->stats.rx_bytes += skb->len;
0327         dev->stats.rx_packets++;
0328 
0329         consume_skb(local_skb);
0330         consume_skb(skb);
0331     } else {
0332         BT_DBG("unknown packet type");
0333         goto drop;
0334     }
0335 
0336     return NET_RX_SUCCESS;
0337 
0338 drop:
0339     dev->stats.rx_dropped++;
0340     return NET_RX_DROP;
0341 }
0342 
0343 /* Packet from BT LE device */
0344 static int chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
0345 {
0346     struct lowpan_btle_dev *dev;
0347     struct lowpan_peer *peer;
0348     int err;
0349 
0350     peer = lookup_peer(chan->conn);
0351     if (!peer)
0352         return -ENOENT;
0353 
0354     dev = lookup_dev(chan->conn);
0355     if (!dev || !dev->netdev)
0356         return -ENOENT;
0357 
0358     err = recv_pkt(skb, dev->netdev, peer);
0359     if (err) {
0360         BT_DBG("recv pkt %d", err);
0361         err = -EAGAIN;
0362     }
0363 
0364     return err;
0365 }
0366 
0367 static int setup_header(struct sk_buff *skb, struct net_device *netdev,
0368             bdaddr_t *peer_addr, u8 *peer_addr_type)
0369 {
0370     struct in6_addr ipv6_daddr;
0371     struct ipv6hdr *hdr;
0372     struct lowpan_btle_dev *dev;
0373     struct lowpan_peer *peer;
0374     u8 *daddr;
0375     int err, status = 0;
0376 
0377     hdr = ipv6_hdr(skb);
0378 
0379     dev = lowpan_btle_dev(netdev);
0380 
0381     memcpy(&ipv6_daddr, &hdr->daddr, sizeof(ipv6_daddr));
0382 
0383     if (ipv6_addr_is_multicast(&ipv6_daddr)) {
0384         lowpan_cb(skb)->chan = NULL;
0385         daddr = NULL;
0386     } else {
0387         BT_DBG("dest IP %pI6c", &ipv6_daddr);
0388 
0389         /* The packet might be sent to 6lowpan interface
0390          * because of routing (either via default route
0391          * or user set route) so get peer according to
0392          * the destination address.
0393          */
0394         peer = peer_lookup_dst(dev, &ipv6_daddr, skb);
0395         if (!peer) {
0396             BT_DBG("no such peer");
0397             return -ENOENT;
0398         }
0399 
0400         daddr = peer->lladdr;
0401         *peer_addr = peer->chan->dst;
0402         *peer_addr_type = peer->chan->dst_type;
0403         lowpan_cb(skb)->chan = peer->chan;
0404 
0405         status = 1;
0406     }
0407 
0408     lowpan_header_compress(skb, netdev, daddr, dev->netdev->dev_addr);
0409 
0410     err = dev_hard_header(skb, netdev, ETH_P_IPV6, NULL, NULL, 0);
0411     if (err < 0)
0412         return err;
0413 
0414     return status;
0415 }
0416 
0417 static int header_create(struct sk_buff *skb, struct net_device *netdev,
0418              unsigned short type, const void *_daddr,
0419              const void *_saddr, unsigned int len)
0420 {
0421     if (type != ETH_P_IPV6)
0422         return -EINVAL;
0423 
0424     return 0;
0425 }
0426 
0427 /* Packet to BT LE device */
0428 static int send_pkt(struct l2cap_chan *chan, struct sk_buff *skb,
0429             struct net_device *netdev)
0430 {
0431     struct msghdr msg;
0432     struct kvec iv;
0433     int err;
0434 
0435     /* Remember the skb so that we can send EAGAIN to the caller if
0436      * we run out of credits.
0437      */
0438     chan->data = skb;
0439 
0440     iv.iov_base = skb->data;
0441     iv.iov_len = skb->len;
0442 
0443     memset(&msg, 0, sizeof(msg));
0444     iov_iter_kvec(&msg.msg_iter, WRITE, &iv, 1, skb->len);
0445 
0446     err = l2cap_chan_send(chan, &msg, skb->len);
0447     if (err > 0) {
0448         netdev->stats.tx_bytes += err;
0449         netdev->stats.tx_packets++;
0450         return 0;
0451     }
0452 
0453     if (err < 0)
0454         netdev->stats.tx_errors++;
0455 
0456     return err;
0457 }
0458 
0459 static int send_mcast_pkt(struct sk_buff *skb, struct net_device *netdev)
0460 {
0461     struct sk_buff *local_skb;
0462     struct lowpan_btle_dev *entry;
0463     int err = 0;
0464 
0465     rcu_read_lock();
0466 
0467     list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
0468         struct lowpan_peer *pentry;
0469         struct lowpan_btle_dev *dev;
0470 
0471         if (entry->netdev != netdev)
0472             continue;
0473 
0474         dev = lowpan_btle_dev(entry->netdev);
0475 
0476         list_for_each_entry_rcu(pentry, &dev->peers, list) {
0477             int ret;
0478 
0479             local_skb = skb_clone(skb, GFP_ATOMIC);
0480 
0481             BT_DBG("xmit %s to %pMR type %u IP %pI6c chan %p",
0482                    netdev->name,
0483                    &pentry->chan->dst, pentry->chan->dst_type,
0484                    &pentry->peer_addr, pentry->chan);
0485             ret = send_pkt(pentry->chan, local_skb, netdev);
0486             if (ret < 0)
0487                 err = ret;
0488 
0489             kfree_skb(local_skb);
0490         }
0491     }
0492 
0493     rcu_read_unlock();
0494 
0495     return err;
0496 }
0497 
0498 static netdev_tx_t bt_xmit(struct sk_buff *skb, struct net_device *netdev)
0499 {
0500     int err = 0;
0501     bdaddr_t addr;
0502     u8 addr_type;
0503 
0504     /* We must take a copy of the skb before we modify/replace the ipv6
0505      * header as the header could be used elsewhere
0506      */
0507     skb = skb_unshare(skb, GFP_ATOMIC);
0508     if (!skb)
0509         return NET_XMIT_DROP;
0510 
0511     /* Return values from setup_header()
0512      *  <0 - error, packet is dropped
0513      *   0 - this is a multicast packet
0514      *   1 - this is unicast packet
0515      */
0516     err = setup_header(skb, netdev, &addr, &addr_type);
0517     if (err < 0) {
0518         kfree_skb(skb);
0519         return NET_XMIT_DROP;
0520     }
0521 
0522     if (err) {
0523         if (lowpan_cb(skb)->chan) {
0524             BT_DBG("xmit %s to %pMR type %u IP %pI6c chan %p",
0525                    netdev->name, &addr, addr_type,
0526                    &lowpan_cb(skb)->addr, lowpan_cb(skb)->chan);
0527             err = send_pkt(lowpan_cb(skb)->chan, skb, netdev);
0528         } else {
0529             err = -ENOENT;
0530         }
0531     } else {
0532         /* We need to send the packet to every device behind this
0533          * interface.
0534          */
0535         err = send_mcast_pkt(skb, netdev);
0536     }
0537 
0538     dev_kfree_skb(skb);
0539 
0540     if (err)
0541         BT_DBG("ERROR: xmit failed (%d)", err);
0542 
0543     return err < 0 ? NET_XMIT_DROP : err;
0544 }
0545 
0546 static int bt_dev_init(struct net_device *dev)
0547 {
0548     netdev_lockdep_set_classes(dev);
0549 
0550     return 0;
0551 }
0552 
0553 static const struct net_device_ops netdev_ops = {
0554     .ndo_init       = bt_dev_init,
0555     .ndo_start_xmit     = bt_xmit,
0556 };
0557 
0558 static const struct header_ops header_ops = {
0559     .create = header_create,
0560 };
0561 
0562 static void netdev_setup(struct net_device *dev)
0563 {
0564     dev->hard_header_len    = 0;
0565     dev->needed_tailroom    = 0;
0566     dev->flags      = IFF_RUNNING | IFF_MULTICAST;
0567     dev->watchdog_timeo = 0;
0568     dev->tx_queue_len   = DEFAULT_TX_QUEUE_LEN;
0569 
0570     dev->netdev_ops     = &netdev_ops;
0571     dev->header_ops     = &header_ops;
0572     dev->needs_free_netdev  = true;
0573 }
0574 
0575 static struct device_type bt_type = {
0576     .name   = "bluetooth",
0577 };
0578 
0579 static void ifup(struct net_device *netdev)
0580 {
0581     int err;
0582 
0583     rtnl_lock();
0584     err = dev_open(netdev, NULL);
0585     if (err < 0)
0586         BT_INFO("iface %s cannot be opened (%d)", netdev->name, err);
0587     rtnl_unlock();
0588 }
0589 
0590 static void ifdown(struct net_device *netdev)
0591 {
0592     rtnl_lock();
0593     dev_close(netdev);
0594     rtnl_unlock();
0595 }
0596 
0597 static void do_notify_peers(struct work_struct *work)
0598 {
0599     struct lowpan_btle_dev *dev = container_of(work, struct lowpan_btle_dev,
0600                            notify_peers.work);
0601 
0602     netdev_notify_peers(dev->netdev); /* send neighbour adv at startup */
0603 }
0604 
0605 static bool is_bt_6lowpan(struct hci_conn *hcon)
0606 {
0607     if (hcon->type != LE_LINK)
0608         return false;
0609 
0610     if (!enable_6lowpan)
0611         return false;
0612 
0613     return true;
0614 }
0615 
0616 static struct l2cap_chan *chan_create(void)
0617 {
0618     struct l2cap_chan *chan;
0619 
0620     chan = l2cap_chan_create();
0621     if (!chan)
0622         return NULL;
0623 
0624     l2cap_chan_set_defaults(chan);
0625 
0626     chan->chan_type = L2CAP_CHAN_CONN_ORIENTED;
0627     chan->mode = L2CAP_MODE_LE_FLOWCTL;
0628     chan->imtu = 1280;
0629 
0630     return chan;
0631 }
0632 
0633 static struct l2cap_chan *add_peer_chan(struct l2cap_chan *chan,
0634                     struct lowpan_btle_dev *dev,
0635                     bool new_netdev)
0636 {
0637     struct lowpan_peer *peer;
0638 
0639     peer = kzalloc(sizeof(*peer), GFP_ATOMIC);
0640     if (!peer)
0641         return NULL;
0642 
0643     peer->chan = chan;
0644 
0645     baswap((void *)peer->lladdr, &chan->dst);
0646 
0647     lowpan_iphc_uncompress_eui48_lladdr(&peer->peer_addr, peer->lladdr);
0648 
0649     spin_lock(&devices_lock);
0650     INIT_LIST_HEAD(&peer->list);
0651     peer_add(dev, peer);
0652     spin_unlock(&devices_lock);
0653 
0654     /* Notifying peers about us needs to be done without locks held */
0655     if (new_netdev)
0656         INIT_DELAYED_WORK(&dev->notify_peers, do_notify_peers);
0657     schedule_delayed_work(&dev->notify_peers, msecs_to_jiffies(100));
0658 
0659     return peer->chan;
0660 }
0661 
0662 static int setup_netdev(struct l2cap_chan *chan, struct lowpan_btle_dev **dev)
0663 {
0664     struct net_device *netdev;
0665     bdaddr_t addr;
0666     int err;
0667 
0668     netdev = alloc_netdev(LOWPAN_PRIV_SIZE(sizeof(struct lowpan_btle_dev)),
0669                   IFACE_NAME_TEMPLATE, NET_NAME_UNKNOWN,
0670                   netdev_setup);
0671     if (!netdev)
0672         return -ENOMEM;
0673 
0674     netdev->addr_assign_type = NET_ADDR_PERM;
0675     baswap(&addr, &chan->src);
0676     __dev_addr_set(netdev, &addr, sizeof(addr));
0677 
0678     netdev->netdev_ops = &netdev_ops;
0679     SET_NETDEV_DEV(netdev, &chan->conn->hcon->hdev->dev);
0680     SET_NETDEV_DEVTYPE(netdev, &bt_type);
0681 
0682     *dev = lowpan_btle_dev(netdev);
0683     (*dev)->netdev = netdev;
0684     (*dev)->hdev = chan->conn->hcon->hdev;
0685     INIT_LIST_HEAD(&(*dev)->peers);
0686 
0687     spin_lock(&devices_lock);
0688     INIT_LIST_HEAD(&(*dev)->list);
0689     list_add_rcu(&(*dev)->list, &bt_6lowpan_devices);
0690     spin_unlock(&devices_lock);
0691 
0692     err = lowpan_register_netdev(netdev, LOWPAN_LLTYPE_BTLE);
0693     if (err < 0) {
0694         BT_INFO("register_netdev failed %d", err);
0695         spin_lock(&devices_lock);
0696         list_del_rcu(&(*dev)->list);
0697         spin_unlock(&devices_lock);
0698         free_netdev(netdev);
0699         goto out;
0700     }
0701 
0702     BT_DBG("ifindex %d peer bdaddr %pMR type %d my addr %pMR type %d",
0703            netdev->ifindex, &chan->dst, chan->dst_type,
0704            &chan->src, chan->src_type);
0705     set_bit(__LINK_STATE_PRESENT, &netdev->state);
0706 
0707     return 0;
0708 
0709 out:
0710     return err;
0711 }
0712 
0713 static inline void chan_ready_cb(struct l2cap_chan *chan)
0714 {
0715     struct lowpan_btle_dev *dev;
0716     bool new_netdev = false;
0717 
0718     dev = lookup_dev(chan->conn);
0719 
0720     BT_DBG("chan %p conn %p dev %p", chan, chan->conn, dev);
0721 
0722     if (!dev) {
0723         if (setup_netdev(chan, &dev) < 0) {
0724             l2cap_chan_del(chan, -ENOENT);
0725             return;
0726         }
0727         new_netdev = true;
0728     }
0729 
0730     if (!try_module_get(THIS_MODULE))
0731         return;
0732 
0733     add_peer_chan(chan, dev, new_netdev);
0734     ifup(dev->netdev);
0735 }
0736 
0737 static inline struct l2cap_chan *chan_new_conn_cb(struct l2cap_chan *pchan)
0738 {
0739     struct l2cap_chan *chan;
0740 
0741     chan = chan_create();
0742     if (!chan)
0743         return NULL;
0744 
0745     chan->ops = pchan->ops;
0746 
0747     BT_DBG("chan %p pchan %p", chan, pchan);
0748 
0749     return chan;
0750 }
0751 
0752 static void delete_netdev(struct work_struct *work)
0753 {
0754     struct lowpan_btle_dev *entry = container_of(work,
0755                              struct lowpan_btle_dev,
0756                              delete_netdev);
0757 
0758     lowpan_unregister_netdev(entry->netdev);
0759 
0760     /* The entry pointer is deleted by the netdev destructor. */
0761 }
0762 
0763 static void chan_close_cb(struct l2cap_chan *chan)
0764 {
0765     struct lowpan_btle_dev *entry;
0766     struct lowpan_btle_dev *dev = NULL;
0767     struct lowpan_peer *peer;
0768     int err = -ENOENT;
0769     bool last = false, remove = true;
0770 
0771     BT_DBG("chan %p conn %p", chan, chan->conn);
0772 
0773     if (chan->conn && chan->conn->hcon) {
0774         if (!is_bt_6lowpan(chan->conn->hcon))
0775             return;
0776 
0777         /* If conn is set, then the netdev is also there and we should
0778          * not remove it.
0779          */
0780         remove = false;
0781     }
0782 
0783     spin_lock(&devices_lock);
0784 
0785     list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
0786         dev = lowpan_btle_dev(entry->netdev);
0787         peer = __peer_lookup_chan(dev, chan);
0788         if (peer) {
0789             last = peer_del(dev, peer);
0790             err = 0;
0791 
0792             BT_DBG("dev %p removing %speer %p", dev,
0793                    last ? "last " : "1 ", peer);
0794             BT_DBG("chan %p orig refcnt %u", chan,
0795                    kref_read(&chan->kref));
0796 
0797             l2cap_chan_put(chan);
0798             break;
0799         }
0800     }
0801 
0802     if (!err && last && dev && !atomic_read(&dev->peer_count)) {
0803         spin_unlock(&devices_lock);
0804 
0805         cancel_delayed_work_sync(&dev->notify_peers);
0806 
0807         ifdown(dev->netdev);
0808 
0809         if (remove) {
0810             INIT_WORK(&entry->delete_netdev, delete_netdev);
0811             schedule_work(&entry->delete_netdev);
0812         }
0813     } else {
0814         spin_unlock(&devices_lock);
0815     }
0816 }
0817 
0818 static void chan_state_change_cb(struct l2cap_chan *chan, int state, int err)
0819 {
0820     BT_DBG("chan %p conn %p state %s err %d", chan, chan->conn,
0821            state_to_string(state), err);
0822 }
0823 
0824 static struct sk_buff *chan_alloc_skb_cb(struct l2cap_chan *chan,
0825                      unsigned long hdr_len,
0826                      unsigned long len, int nb)
0827 {
0828     /* Note that we must allocate using GFP_ATOMIC here as
0829      * this function is called originally from netdev hard xmit
0830      * function in atomic context.
0831      */
0832     return bt_skb_alloc(hdr_len + len, GFP_ATOMIC);
0833 }
0834 
0835 static void chan_suspend_cb(struct l2cap_chan *chan)
0836 {
0837     struct lowpan_btle_dev *dev;
0838 
0839     BT_DBG("chan %p suspend", chan);
0840 
0841     dev = lookup_dev(chan->conn);
0842     if (!dev || !dev->netdev)
0843         return;
0844 
0845     netif_stop_queue(dev->netdev);
0846 }
0847 
0848 static void chan_resume_cb(struct l2cap_chan *chan)
0849 {
0850     struct lowpan_btle_dev *dev;
0851 
0852     BT_DBG("chan %p resume", chan);
0853 
0854     dev = lookup_dev(chan->conn);
0855     if (!dev || !dev->netdev)
0856         return;
0857 
0858     netif_wake_queue(dev->netdev);
0859 }
0860 
0861 static long chan_get_sndtimeo_cb(struct l2cap_chan *chan)
0862 {
0863     return L2CAP_CONN_TIMEOUT;
0864 }
0865 
0866 static const struct l2cap_ops bt_6lowpan_chan_ops = {
0867     .name           = "L2CAP 6LoWPAN channel",
0868     .new_connection     = chan_new_conn_cb,
0869     .recv           = chan_recv_cb,
0870     .close          = chan_close_cb,
0871     .state_change       = chan_state_change_cb,
0872     .ready          = chan_ready_cb,
0873     .resume         = chan_resume_cb,
0874     .suspend        = chan_suspend_cb,
0875     .get_sndtimeo       = chan_get_sndtimeo_cb,
0876     .alloc_skb      = chan_alloc_skb_cb,
0877 
0878     .teardown       = l2cap_chan_no_teardown,
0879     .defer          = l2cap_chan_no_defer,
0880     .set_shutdown       = l2cap_chan_no_set_shutdown,
0881 };
0882 
0883 static int bt_6lowpan_connect(bdaddr_t *addr, u8 dst_type)
0884 {
0885     struct l2cap_chan *chan;
0886     int err;
0887 
0888     chan = chan_create();
0889     if (!chan)
0890         return -EINVAL;
0891 
0892     chan->ops = &bt_6lowpan_chan_ops;
0893 
0894     err = l2cap_chan_connect(chan, cpu_to_le16(L2CAP_PSM_IPSP), 0,
0895                  addr, dst_type);
0896 
0897     BT_DBG("chan %p err %d", chan, err);
0898     if (err < 0)
0899         l2cap_chan_put(chan);
0900 
0901     return err;
0902 }
0903 
0904 static int bt_6lowpan_disconnect(struct l2cap_conn *conn, u8 dst_type)
0905 {
0906     struct lowpan_peer *peer;
0907 
0908     BT_DBG("conn %p dst type %u", conn, dst_type);
0909 
0910     peer = lookup_peer(conn);
0911     if (!peer)
0912         return -ENOENT;
0913 
0914     BT_DBG("peer %p chan %p", peer, peer->chan);
0915 
0916     l2cap_chan_close(peer->chan, ENOENT);
0917 
0918     return 0;
0919 }
0920 
0921 static struct l2cap_chan *bt_6lowpan_listen(void)
0922 {
0923     bdaddr_t *addr = BDADDR_ANY;
0924     struct l2cap_chan *chan;
0925     int err;
0926 
0927     if (!enable_6lowpan)
0928         return NULL;
0929 
0930     chan = chan_create();
0931     if (!chan)
0932         return NULL;
0933 
0934     chan->ops = &bt_6lowpan_chan_ops;
0935     chan->state = BT_LISTEN;
0936     chan->src_type = BDADDR_LE_PUBLIC;
0937 
0938     atomic_set(&chan->nesting, L2CAP_NESTING_PARENT);
0939 
0940     BT_DBG("chan %p src type %u", chan, chan->src_type);
0941 
0942     err = l2cap_add_psm(chan, addr, cpu_to_le16(L2CAP_PSM_IPSP));
0943     if (err) {
0944         l2cap_chan_put(chan);
0945         BT_ERR("psm cannot be added err %d", err);
0946         return NULL;
0947     }
0948 
0949     return chan;
0950 }
0951 
0952 static int get_l2cap_conn(char *buf, bdaddr_t *addr, u8 *addr_type,
0953               struct l2cap_conn **conn)
0954 {
0955     struct hci_conn *hcon;
0956     struct hci_dev *hdev;
0957     int n;
0958 
0959     n = sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx %hhu",
0960            &addr->b[5], &addr->b[4], &addr->b[3],
0961            &addr->b[2], &addr->b[1], &addr->b[0],
0962            addr_type);
0963 
0964     if (n < 7)
0965         return -EINVAL;
0966 
0967     /* The LE_PUBLIC address type is ignored because of BDADDR_ANY */
0968     hdev = hci_get_route(addr, BDADDR_ANY, BDADDR_LE_PUBLIC);
0969     if (!hdev)
0970         return -ENOENT;
0971 
0972     hci_dev_lock(hdev);
0973     hcon = hci_conn_hash_lookup_le(hdev, addr, *addr_type);
0974     hci_dev_unlock(hdev);
0975 
0976     if (!hcon)
0977         return -ENOENT;
0978 
0979     *conn = (struct l2cap_conn *)hcon->l2cap_data;
0980 
0981     BT_DBG("conn %p dst %pMR type %u", *conn, &hcon->dst, hcon->dst_type);
0982 
0983     return 0;
0984 }
0985 
0986 static void disconnect_all_peers(void)
0987 {
0988     struct lowpan_btle_dev *entry;
0989     struct lowpan_peer *peer, *tmp_peer, *new_peer;
0990     struct list_head peers;
0991 
0992     INIT_LIST_HEAD(&peers);
0993 
0994     /* We make a separate list of peers as the close_cb() will
0995      * modify the device peers list so it is better not to mess
0996      * with the same list at the same time.
0997      */
0998 
0999     rcu_read_lock();
1000 
1001     list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
1002         list_for_each_entry_rcu(peer, &entry->peers, list) {
1003             new_peer = kmalloc(sizeof(*new_peer), GFP_ATOMIC);
1004             if (!new_peer)
1005                 break;
1006 
1007             new_peer->chan = peer->chan;
1008             INIT_LIST_HEAD(&new_peer->list);
1009 
1010             list_add(&new_peer->list, &peers);
1011         }
1012     }
1013 
1014     rcu_read_unlock();
1015 
1016     spin_lock(&devices_lock);
1017     list_for_each_entry_safe(peer, tmp_peer, &peers, list) {
1018         l2cap_chan_close(peer->chan, ENOENT);
1019 
1020         list_del_rcu(&peer->list);
1021         kfree_rcu(peer, rcu);
1022     }
1023     spin_unlock(&devices_lock);
1024 }
1025 
1026 struct set_enable {
1027     struct work_struct work;
1028     bool flag;
1029 };
1030 
1031 static void do_enable_set(struct work_struct *work)
1032 {
1033     struct set_enable *set_enable = container_of(work,
1034                              struct set_enable, work);
1035 
1036     if (!set_enable->flag || enable_6lowpan != set_enable->flag)
1037         /* Disconnect existing connections if 6lowpan is
1038          * disabled
1039          */
1040         disconnect_all_peers();
1041 
1042     enable_6lowpan = set_enable->flag;
1043 
1044     mutex_lock(&set_lock);
1045     if (listen_chan) {
1046         l2cap_chan_close(listen_chan, 0);
1047         l2cap_chan_put(listen_chan);
1048     }
1049 
1050     listen_chan = bt_6lowpan_listen();
1051     mutex_unlock(&set_lock);
1052 
1053     kfree(set_enable);
1054 }
1055 
1056 static int lowpan_enable_set(void *data, u64 val)
1057 {
1058     struct set_enable *set_enable;
1059 
1060     set_enable = kzalloc(sizeof(*set_enable), GFP_KERNEL);
1061     if (!set_enable)
1062         return -ENOMEM;
1063 
1064     set_enable->flag = !!val;
1065     INIT_WORK(&set_enable->work, do_enable_set);
1066 
1067     schedule_work(&set_enable->work);
1068 
1069     return 0;
1070 }
1071 
1072 static int lowpan_enable_get(void *data, u64 *val)
1073 {
1074     *val = enable_6lowpan;
1075     return 0;
1076 }
1077 
1078 DEFINE_DEBUGFS_ATTRIBUTE(lowpan_enable_fops, lowpan_enable_get,
1079              lowpan_enable_set, "%llu\n");
1080 
1081 static ssize_t lowpan_control_write(struct file *fp,
1082                     const char __user *user_buffer,
1083                     size_t count,
1084                     loff_t *position)
1085 {
1086     char buf[32];
1087     size_t buf_size = min(count, sizeof(buf) - 1);
1088     int ret;
1089     bdaddr_t addr;
1090     u8 addr_type;
1091     struct l2cap_conn *conn = NULL;
1092 
1093     if (copy_from_user(buf, user_buffer, buf_size))
1094         return -EFAULT;
1095 
1096     buf[buf_size] = '\0';
1097 
1098     if (memcmp(buf, "connect ", 8) == 0) {
1099         ret = get_l2cap_conn(&buf[8], &addr, &addr_type, &conn);
1100         if (ret == -EINVAL)
1101             return ret;
1102 
1103         mutex_lock(&set_lock);
1104         if (listen_chan) {
1105             l2cap_chan_close(listen_chan, 0);
1106             l2cap_chan_put(listen_chan);
1107             listen_chan = NULL;
1108         }
1109         mutex_unlock(&set_lock);
1110 
1111         if (conn) {
1112             struct lowpan_peer *peer;
1113 
1114             if (!is_bt_6lowpan(conn->hcon))
1115                 return -EINVAL;
1116 
1117             peer = lookup_peer(conn);
1118             if (peer) {
1119                 BT_DBG("6LoWPAN connection already exists");
1120                 return -EALREADY;
1121             }
1122 
1123             BT_DBG("conn %p dst %pMR type %d user %u", conn,
1124                    &conn->hcon->dst, conn->hcon->dst_type,
1125                    addr_type);
1126         }
1127 
1128         ret = bt_6lowpan_connect(&addr, addr_type);
1129         if (ret < 0)
1130             return ret;
1131 
1132         return count;
1133     }
1134 
1135     if (memcmp(buf, "disconnect ", 11) == 0) {
1136         ret = get_l2cap_conn(&buf[11], &addr, &addr_type, &conn);
1137         if (ret < 0)
1138             return ret;
1139 
1140         ret = bt_6lowpan_disconnect(conn, addr_type);
1141         if (ret < 0)
1142             return ret;
1143 
1144         return count;
1145     }
1146 
1147     return count;
1148 }
1149 
1150 static int lowpan_control_show(struct seq_file *f, void *ptr)
1151 {
1152     struct lowpan_btle_dev *entry;
1153     struct lowpan_peer *peer;
1154 
1155     spin_lock(&devices_lock);
1156 
1157     list_for_each_entry(entry, &bt_6lowpan_devices, list) {
1158         list_for_each_entry(peer, &entry->peers, list)
1159             seq_printf(f, "%pMR (type %u)\n",
1160                    &peer->chan->dst, peer->chan->dst_type);
1161     }
1162 
1163     spin_unlock(&devices_lock);
1164 
1165     return 0;
1166 }
1167 
1168 static int lowpan_control_open(struct inode *inode, struct file *file)
1169 {
1170     return single_open(file, lowpan_control_show, inode->i_private);
1171 }
1172 
1173 static const struct file_operations lowpan_control_fops = {
1174     .open       = lowpan_control_open,
1175     .read       = seq_read,
1176     .write      = lowpan_control_write,
1177     .llseek     = seq_lseek,
1178     .release    = single_release,
1179 };
1180 
1181 static void disconnect_devices(void)
1182 {
1183     struct lowpan_btle_dev *entry, *tmp, *new_dev;
1184     struct list_head devices;
1185 
1186     INIT_LIST_HEAD(&devices);
1187 
1188     /* We make a separate list of devices because the unregister_netdev()
1189      * will call device_event() which will also want to modify the same
1190      * devices list.
1191      */
1192 
1193     rcu_read_lock();
1194 
1195     list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
1196         new_dev = kmalloc(sizeof(*new_dev), GFP_ATOMIC);
1197         if (!new_dev)
1198             break;
1199 
1200         new_dev->netdev = entry->netdev;
1201         INIT_LIST_HEAD(&new_dev->list);
1202 
1203         list_add_rcu(&new_dev->list, &devices);
1204     }
1205 
1206     rcu_read_unlock();
1207 
1208     list_for_each_entry_safe(entry, tmp, &devices, list) {
1209         ifdown(entry->netdev);
1210         BT_DBG("Unregistering netdev %s %p",
1211                entry->netdev->name, entry->netdev);
1212         lowpan_unregister_netdev(entry->netdev);
1213         kfree(entry);
1214     }
1215 }
1216 
1217 static int device_event(struct notifier_block *unused,
1218             unsigned long event, void *ptr)
1219 {
1220     struct net_device *netdev = netdev_notifier_info_to_dev(ptr);
1221     struct lowpan_btle_dev *entry;
1222 
1223     if (netdev->type != ARPHRD_6LOWPAN)
1224         return NOTIFY_DONE;
1225 
1226     switch (event) {
1227     case NETDEV_UNREGISTER:
1228         spin_lock(&devices_lock);
1229         list_for_each_entry(entry, &bt_6lowpan_devices, list) {
1230             if (entry->netdev == netdev) {
1231                 BT_DBG("Unregistered netdev %s %p",
1232                        netdev->name, netdev);
1233                 list_del(&entry->list);
1234                 break;
1235             }
1236         }
1237         spin_unlock(&devices_lock);
1238         break;
1239     }
1240 
1241     return NOTIFY_DONE;
1242 }
1243 
1244 static struct notifier_block bt_6lowpan_dev_notifier = {
1245     .notifier_call = device_event,
1246 };
1247 
1248 static int __init bt_6lowpan_init(void)
1249 {
1250     lowpan_enable_debugfs = debugfs_create_file_unsafe("6lowpan_enable",
1251                                0644, bt_debugfs,
1252                                NULL,
1253                                &lowpan_enable_fops);
1254     lowpan_control_debugfs = debugfs_create_file("6lowpan_control", 0644,
1255                              bt_debugfs, NULL,
1256                              &lowpan_control_fops);
1257 
1258     return register_netdevice_notifier(&bt_6lowpan_dev_notifier);
1259 }
1260 
1261 static void __exit bt_6lowpan_exit(void)
1262 {
1263     debugfs_remove(lowpan_enable_debugfs);
1264     debugfs_remove(lowpan_control_debugfs);
1265 
1266     if (listen_chan) {
1267         l2cap_chan_close(listen_chan, 0);
1268         l2cap_chan_put(listen_chan);
1269     }
1270 
1271     disconnect_devices();
1272 
1273     unregister_netdevice_notifier(&bt_6lowpan_dev_notifier);
1274 }
1275 
1276 module_init(bt_6lowpan_init);
1277 module_exit(bt_6lowpan_exit);
1278 
1279 MODULE_AUTHOR("Jukka Rissanen <jukka.rissanen@linux.intel.com>");
1280 MODULE_DESCRIPTION("Bluetooth 6LoWPAN");
1281 MODULE_VERSION(VERSION);
1282 MODULE_LICENSE("GPL");
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!