Bluetooth: Add HCI Read Flow Control Mode function
[GitHub/mt8127/android_kernel_alcatel_ttab.git] / net / bluetooth / hci_core.c
index 56943add45cc44707167f4877e27fe5630b00c95..ef0423e62a22382cf21d90ca065581d6cb81d08c 100644 (file)
@@ -54,6 +54,8 @@
 
 #define AUTO_OFF_TIMEOUT 2000
 
+int enable_hs;
+
 static void hci_cmd_task(unsigned long arg);
 static void hci_rx_task(unsigned long arg);
 static void hci_tx_task(unsigned long arg);
@@ -319,8 +321,7 @@ static void hci_linkpol_req(struct hci_dev *hdev, unsigned long opt)
  * Device is held on return. */
 struct hci_dev *hci_dev_get(int index)
 {
-       struct hci_dev *hdev = NULL;
-       struct list_head *p;
+       struct hci_dev *hdev = NULL, *d;
 
        BT_DBG("%d", index);
 
@@ -328,8 +329,7 @@ struct hci_dev *hci_dev_get(int index)
                return NULL;
 
        read_lock(&hci_dev_list_lock);
-       list_for_each(p, &hci_dev_list) {
-               struct hci_dev *d = list_entry(p, struct hci_dev, list);
+       list_for_each_entry(d, &hci_dev_list, list) {
                if (d->id == index) {
                        hdev = hci_dev_hold(d);
                        break;
@@ -523,8 +523,9 @@ int hci_dev_open(__u16 dev)
        if (test_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks))
                set_bit(HCI_RAW, &hdev->flags);
 
-       /* Treat all non BR/EDR controllers as raw devices for now */
-       if (hdev->dev_type != HCI_BREDR)
+       /* Treat all non BR/EDR controllers as raw devices if
+          enable_hs is not set */
+       if (hdev->dev_type != HCI_BREDR && !enable_hs)
                set_bit(HCI_RAW, &hdev->flags);
 
        if (hdev->open(hdev)) {
@@ -551,8 +552,11 @@ int hci_dev_open(__u16 dev)
                hci_dev_hold(hdev);
                set_bit(HCI_UP, &hdev->flags);
                hci_notify(hdev, HCI_DEV_UP);
-               if (!test_bit(HCI_SETUP, &hdev->flags))
-                       mgmt_powered(hdev->id, 1);
+               if (!test_bit(HCI_SETUP, &hdev->flags)) {
+                       hci_dev_lock_bh(hdev);
+                       mgmt_powered(hdev, 1);
+                       hci_dev_unlock_bh(hdev);
+               }
        } else {
                /* Init failed, cleanup */
                tasklet_kill(&hdev->rx_task);
@@ -597,6 +601,14 @@ static int hci_dev_do_close(struct hci_dev *hdev)
        tasklet_kill(&hdev->rx_task);
        tasklet_kill(&hdev->tx_task);
 
+       if (hdev->discov_timeout > 0) {
+               cancel_delayed_work(&hdev->discov_off);
+               hdev->discov_timeout = 0;
+       }
+
+       if (test_and_clear_bit(HCI_AUTO_OFF, &hdev->flags))
+               cancel_delayed_work(&hdev->power_off);
+
        hci_dev_lock_bh(hdev);
        inquiry_cache_flush(hdev);
        hci_conn_hash_flush(hdev);
@@ -613,7 +625,7 @@ static int hci_dev_do_close(struct hci_dev *hdev)
        if (!test_bit(HCI_RAW, &hdev->flags)) {
                set_bit(HCI_INIT, &hdev->flags);
                __hci_request(hdev, hci_reset_req, 0,
-                                       msecs_to_jiffies(250));
+                                       msecs_to_jiffies(HCI_INIT_TIMEOUT));
                clear_bit(HCI_INIT, &hdev->flags);
        }
 
@@ -636,7 +648,9 @@ static int hci_dev_do_close(struct hci_dev *hdev)
         * and no tasks are scheduled. */
        hdev->close(hdev);
 
-       mgmt_powered(hdev->id, 0);
+       hci_dev_lock_bh(hdev);
+       mgmt_powered(hdev, 0);
+       hci_dev_unlock_bh(hdev);
 
        /* Clear flags */
        hdev->flags = 0;
@@ -794,9 +808,9 @@ int hci_dev_cmd(unsigned int cmd, void __user *arg)
 
 int hci_get_dev_list(void __user *arg)
 {
+       struct hci_dev *hdev;
        struct hci_dev_list_req *dl;
        struct hci_dev_req *dr;
-       struct list_head *p;
        int n = 0, size, err;
        __u16 dev_num;
 
@@ -815,12 +829,9 @@ int hci_get_dev_list(void __user *arg)
        dr = dl->dev_req;
 
        read_lock_bh(&hci_dev_list_lock);
-       list_for_each(p, &hci_dev_list) {
-               struct hci_dev *hdev;
-
-               hdev = list_entry(p, struct hci_dev, list);
-
-               hci_del_off_timer(hdev);
+       list_for_each_entry(hdev, &hci_dev_list, list) {
+               if (test_and_clear_bit(HCI_AUTO_OFF, &hdev->flags))
+                       cancel_delayed_work(&hdev->power_off);
 
                if (!test_bit(HCI_MGMT, &hdev->flags))
                        set_bit(HCI_PAIRABLE, &hdev->flags);
@@ -855,7 +866,8 @@ int hci_get_dev_info(void __user *arg)
        if (!hdev)
                return -ENODEV;
 
-       hci_del_off_timer(hdev);
+       if (test_and_clear_bit(HCI_AUTO_OFF, &hdev->flags))
+               cancel_delayed_work_sync(&hdev->power_off);
 
        if (!test_bit(HCI_MGMT, &hdev->flags))
                set_bit(HCI_PAIRABLE, &hdev->flags);
@@ -912,6 +924,7 @@ struct hci_dev *hci_alloc_dev(void)
        if (!hdev)
                return NULL;
 
+       hci_init_sysfs(hdev);
        skb_queue_head_init(&hdev->driver_init);
 
        return hdev;
@@ -938,39 +951,41 @@ static void hci_power_on(struct work_struct *work)
                return;
 
        if (test_bit(HCI_AUTO_OFF, &hdev->flags))
-               mod_timer(&hdev->off_timer,
-                               jiffies + msecs_to_jiffies(AUTO_OFF_TIMEOUT));
+               queue_delayed_work(hdev->workqueue, &hdev->power_off,
+                                       msecs_to_jiffies(AUTO_OFF_TIMEOUT));
 
        if (test_and_clear_bit(HCI_SETUP, &hdev->flags))
-               mgmt_index_added(hdev->id);
+               mgmt_index_added(hdev);
 }
 
 static void hci_power_off(struct work_struct *work)
 {
-       struct hci_dev *hdev = container_of(work, struct hci_dev, power_off);
+       struct hci_dev *hdev = container_of(work, struct hci_dev,
+                                                       power_off.work);
 
        BT_DBG("%s", hdev->name);
 
+       clear_bit(HCI_AUTO_OFF, &hdev->flags);
+
        hci_dev_close(hdev->id);
 }
 
-static void hci_auto_off(unsigned long data)
+static void hci_discov_off(struct work_struct *work)
 {
-       struct hci_dev *hdev = (struct hci_dev *) data;
+       struct hci_dev *hdev;
+       u8 scan = SCAN_PAGE;
+
+       hdev = container_of(work, struct hci_dev, discov_off.work);
 
        BT_DBG("%s", hdev->name);
 
-       clear_bit(HCI_AUTO_OFF, &hdev->flags);
+       hci_dev_lock_bh(hdev);
 
-       queue_work(hdev->workqueue, &hdev->power_off);
-}
+       hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, sizeof(scan), &scan);
 
-void hci_del_off_timer(struct hci_dev *hdev)
-{
-       BT_DBG("%s", hdev->name);
+       hdev->discov_timeout = 0;
 
-       clear_bit(HCI_AUTO_OFF, &hdev->flags);
-       del_timer(&hdev->off_timer);
+       hci_dev_unlock_bh(hdev);
 }
 
 int hci_uuids_clear(struct hci_dev *hdev)
@@ -1007,16 +1022,11 @@ int hci_link_keys_clear(struct hci_dev *hdev)
 
 struct link_key *hci_find_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr)
 {
-       struct list_head *p;
-
-       list_for_each(p, &hdev->link_keys) {
-               struct link_key *k;
-
-               k = list_entry(p, struct link_key, list);
+       struct link_key *k;
 
+       list_for_each_entry(k, &hdev->link_keys, list)
                if (bacmp(bdaddr, &k->bdaddr) == 0)
                        return k;
-       }
 
        return NULL;
 }
@@ -1138,7 +1148,7 @@ int hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, int new_key,
 
        persistent = hci_persistent_key(hdev, conn, type, old_key_type);
 
-       mgmt_new_key(hdev->id, key, persistent);
+       mgmt_new_link_key(hdev, key, persistent);
 
        if (!persistent) {
                list_del(&key->list);
@@ -1181,7 +1191,7 @@ int hci_add_ltk(struct hci_dev *hdev, int new_key, bdaddr_t *bdaddr,
        memcpy(id->rand, rand, sizeof(id->rand));
 
        if (new_key)
-               mgmt_new_key(hdev->id, key, old_key_type);
+               mgmt_new_link_key(hdev, key, old_key_type);
 
        return 0;
 }
@@ -1279,16 +1289,11 @@ int hci_add_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 *hash,
 struct bdaddr_list *hci_blacklist_lookup(struct hci_dev *hdev,
                                                bdaddr_t *bdaddr)
 {
-       struct list_head *p;
-
-       list_for_each(p, &hdev->blacklist) {
-               struct bdaddr_list *b;
-
-               b = list_entry(p, struct bdaddr_list, list);
+       struct bdaddr_list *b;
 
+       list_for_each_entry(b, &hdev->blacklist, list)
                if (bacmp(bdaddr, &b->bdaddr) == 0)
                        return b;
-       }
 
        return NULL;
 }
@@ -1312,59 +1317,39 @@ int hci_blacklist_clear(struct hci_dev *hdev)
 int hci_blacklist_add(struct hci_dev *hdev, bdaddr_t *bdaddr)
 {
        struct bdaddr_list *entry;
-       int err;
 
        if (bacmp(bdaddr, BDADDR_ANY) == 0)
                return -EBADF;
 
-       hci_dev_lock_bh(hdev);
-
-       if (hci_blacklist_lookup(hdev, bdaddr)) {
-               err = -EEXIST;
-               goto err;
-       }
+       if (hci_blacklist_lookup(hdev, bdaddr))
+               return -EEXIST;
 
        entry = kzalloc(sizeof(struct bdaddr_list), GFP_KERNEL);
-       if (!entry) {
-               err = -ENOMEM;
-               goto err;
-       }
+       if (!entry)
+               return -ENOMEM;
 
        bacpy(&entry->bdaddr, bdaddr);
 
        list_add(&entry->list, &hdev->blacklist);
 
-       err = 0;
-
-err:
-       hci_dev_unlock_bh(hdev);
-       return err;
+       return mgmt_device_blocked(hdev, bdaddr);
 }
 
 int hci_blacklist_del(struct hci_dev *hdev, bdaddr_t *bdaddr)
 {
        struct bdaddr_list *entry;
-       int err = 0;
 
-       hci_dev_lock_bh(hdev);
-
-       if (bacmp(bdaddr, BDADDR_ANY) == 0) {
-               hci_blacklist_clear(hdev);
-               goto done;
-       }
+       if (bacmp(bdaddr, BDADDR_ANY) == 0)
+               return hci_blacklist_clear(hdev);
 
        entry = hci_blacklist_lookup(hdev, bdaddr);
-       if (!entry) {
-               err = -ENOENT;
-               goto done;
-       }
+       if (!entry)
+               return -ENOENT;
 
        list_del(&entry->list);
        kfree(entry);
 
-done:
-       hci_dev_unlock_bh(hdev);
-       return err;
+       return mgmt_device_unblocked(hdev, bdaddr);
 }
 
 static void hci_clear_adv_cache(unsigned long arg)
@@ -1443,7 +1428,7 @@ int hci_add_adv_entry(struct hci_dev *hdev,
 int hci_register_dev(struct hci_dev *hdev)
 {
        struct list_head *head = &hci_dev_list, *p;
-       int i, id = 0;
+       int i, id, error;
 
        BT_DBG("%p name %s bus %d owner %p", hdev, hdev->name,
                                                hdev->bus, hdev->owner);
@@ -1451,6 +1436,11 @@ int hci_register_dev(struct hci_dev *hdev)
        if (!hdev->open || !hdev->close || !hdev->destruct)
                return -EINVAL;
 
+       /* Do not allow HCI_AMP devices to register at index 0,
+        * so the index can be used as the AMP controller ID.
+        */
+       id = (hdev->dev_type == HCI_BREDR) ? 0 : 1;
+
        write_lock_bh(&hci_dev_list_lock);
 
        /* Find first available device id */
@@ -1462,7 +1452,7 @@ int hci_register_dev(struct hci_dev *hdev)
 
        sprintf(hdev->name, "hci%d", id);
        hdev->id = id;
-       list_add(&hdev->list, head);
+       list_add_tail(&hdev->list, head);
 
        atomic_set(&hdev->refcnt, 1);
        spin_lock_init(&hdev->lock);
@@ -1497,6 +1487,8 @@ int hci_register_dev(struct hci_dev *hdev)
 
        hci_conn_hash_init(hdev);
 
+       INIT_LIST_HEAD(&hdev->mgmt_pending);
+
        INIT_LIST_HEAD(&hdev->blacklist);
 
        INIT_LIST_HEAD(&hdev->uuids);
@@ -1510,8 +1502,9 @@ int hci_register_dev(struct hci_dev *hdev)
                                                (unsigned long) hdev);
 
        INIT_WORK(&hdev->power_on, hci_power_on);
-       INIT_WORK(&hdev->power_off, hci_power_off);
-       setup_timer(&hdev->off_timer, hci_auto_off, (unsigned long) hdev);
+       INIT_DELAYED_WORK(&hdev->power_off, hci_power_off);
+
+       INIT_DELAYED_WORK(&hdev->discov_off, hci_discov_off);
 
        memset(&hdev->stat, 0, sizeof(struct hci_dev_stats));
 
@@ -1520,15 +1513,14 @@ int hci_register_dev(struct hci_dev *hdev)
        write_unlock_bh(&hci_dev_list_lock);
 
        hdev->workqueue = create_singlethread_workqueue(hdev->name);
-       if (!hdev->workqueue)
-               goto nomem;
-
-       hdev->tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC);
-       if (IS_ERR(hdev->tfm))
-               BT_INFO("Failed to load transform for ecb(aes): %ld",
-                                                       PTR_ERR(hdev->tfm));
+       if (!hdev->workqueue) {
+               error = -ENOMEM;
+               goto err;
+       }
 
-       hci_register_sysfs(hdev);
+       error = hci_add_sysfs(hdev);
+       if (error < 0)
+               goto err_wqueue;
 
        hdev->rfkill = rfkill_alloc(hdev->name, &hdev->dev,
                                RFKILL_TYPE_BLUETOOTH, &hci_rfkill_ops, hdev);
@@ -1547,17 +1539,19 @@ int hci_register_dev(struct hci_dev *hdev)
 
        return id;
 
-nomem:
+err_wqueue:
+       destroy_workqueue(hdev->workqueue);
+err:
        write_lock_bh(&hci_dev_list_lock);
        list_del(&hdev->list);
        write_unlock_bh(&hci_dev_list_lock);
 
-       return -ENOMEM;
+       return error;
 }
 EXPORT_SYMBOL(hci_register_dev);
 
 /* Unregister HCI device */
-int hci_unregister_dev(struct hci_dev *hdev)
+void hci_unregister_dev(struct hci_dev *hdev)
 {
        int i;
 
@@ -1573,11 +1567,15 @@ int hci_unregister_dev(struct hci_dev *hdev)
                kfree_skb(hdev->reassembly[i]);
 
        if (!test_bit(HCI_INIT, &hdev->flags) &&
-                                       !test_bit(HCI_SETUP, &hdev->flags))
-               mgmt_index_removed(hdev->id);
+                                       !test_bit(HCI_SETUP, &hdev->flags)) {
+               hci_dev_lock_bh(hdev);
+               mgmt_index_removed(hdev);
+               hci_dev_unlock_bh(hdev);
+       }
 
-       if (!IS_ERR(hdev->tfm))
-               crypto_free_blkcipher(hdev->tfm);
+       /* mgmt_index_removed should take care of emptying the
+        * pending list */
+       BUG_ON(!list_empty(&hdev->mgmt_pending));
 
        hci_notify(hdev, HCI_DEV_UNREG);
 
@@ -1586,9 +1584,8 @@ int hci_unregister_dev(struct hci_dev *hdev)
                rfkill_destroy(hdev->rfkill);
        }
 
-       hci_unregister_sysfs(hdev);
+       hci_del_sysfs(hdev);
 
-       hci_del_off_timer(hdev);
        del_timer(&hdev->adv_timer);
 
        destroy_workqueue(hdev->workqueue);
@@ -1602,8 +1599,6 @@ int hci_unregister_dev(struct hci_dev *hdev)
        hci_dev_unlock_bh(hdev);
 
        __hci_dev_put(hdev);
-
-       return 0;
 }
 EXPORT_SYMBOL(hci_unregister_dev);
 
@@ -1974,23 +1969,18 @@ static void hci_add_acl_hdr(struct sk_buff *skb, __u16 handle, __u16 flags)
        hdr->dlen   = cpu_to_le16(len);
 }
 
-void hci_send_acl(struct hci_conn *conn, struct sk_buff *skb, __u16 flags)
+static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue,
+                               struct sk_buff *skb, __u16 flags)
 {
        struct hci_dev *hdev = conn->hdev;
        struct sk_buff *list;
 
-       BT_DBG("%s conn %p flags 0x%x", hdev->name, conn, flags);
-
-       skb->dev = (void *) hdev;
-       bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT;
-       hci_add_acl_hdr(skb, conn->handle, flags);
-
        list = skb_shinfo(skb)->frag_list;
        if (!list) {
                /* Non fragmented */
                BT_DBG("%s nonfrag skb %p len %d", hdev->name, skb, skb->len);
 
-               skb_queue_tail(&conn->data_q, skb);
+               skb_queue_tail(queue, skb);
        } else {
                /* Fragmented */
                BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len);
@@ -1998,9 +1988,9 @@ void hci_send_acl(struct hci_conn *conn, struct sk_buff *skb, __u16 flags)
                skb_shinfo(skb)->frag_list = NULL;
 
                /* Queue all fragments atomically */
-               spin_lock_bh(&conn->data_q.lock);
+               spin_lock_bh(&queue->lock);
 
-               __skb_queue_tail(&conn->data_q, skb);
+               __skb_queue_tail(queue, skb);
 
                flags &= ~ACL_START;
                flags |= ACL_CONT;
@@ -2013,11 +2003,25 @@ void hci_send_acl(struct hci_conn *conn, struct sk_buff *skb, __u16 flags)
 
                        BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len);
 
-                       __skb_queue_tail(&conn->data_q, skb);
+                       __skb_queue_tail(queue, skb);
                } while (list);
 
-               spin_unlock_bh(&conn->data_q.lock);
+               spin_unlock_bh(&queue->lock);
        }
+}
+
+void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags)
+{
+       struct hci_conn *conn = chan->conn;
+       struct hci_dev *hdev = conn->hdev;
+
+       BT_DBG("%s chan %p flags 0x%x", hdev->name, chan, flags);
+
+       skb->dev = (void *) hdev;
+       bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT;
+       hci_add_acl_hdr(skb, conn->handle, flags);
+
+       hci_queue_acl(conn, &chan->data_q, skb, flags);
 
        tasklet_schedule(&hdev->tx_task);
 }
@@ -2052,16 +2056,12 @@ EXPORT_SYMBOL(hci_send_sco);
 static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int *quote)
 {
        struct hci_conn_hash *h = &hdev->conn_hash;
-       struct hci_conn *conn = NULL;
+       struct hci_conn *conn = NULL, *c;
        int num = 0, min = ~0;
-       struct list_head *p;
 
        /* We don't have to lock device here. Connections are always
         * added and removed with TX task disabled. */
-       list_for_each(p, &h->list) {
-               struct hci_conn *c;
-               c = list_entry(p, struct hci_conn, list);
-
+       list_for_each_entry(c, &h->list, list) {
                if (c->type != type || skb_queue_empty(&c->data_q))
                        continue;
 
@@ -2074,6 +2074,9 @@ static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int
                        min  = c->sent;
                        conn = c;
                }
+
+               if (hci_conn_num(hdev, type) == num)
+                       break;
        }
 
        if (conn) {
@@ -2107,14 +2110,12 @@ static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int
 static inline void hci_link_tx_to(struct hci_dev *hdev, __u8 type)
 {
        struct hci_conn_hash *h = &hdev->conn_hash;
-       struct list_head *p;
-       struct hci_conn  *c;
+       struct hci_conn *c;
 
        BT_ERR("%s link tx timeout", hdev->name);
 
        /* Kill stalled connections */
-       list_for_each(p, &h->list) {
-               c = list_entry(p, struct hci_conn, list);
+       list_for_each_entry(c, &h->list, list) {
                if (c->type == type && c->sent) {
                        BT_ERR("%s killing stalled connection %s",
                                hdev->name, batostr(&c->dst));
@@ -2123,14 +2124,143 @@ static inline void hci_link_tx_to(struct hci_dev *hdev, __u8 type)
        }
 }
 
-static inline void hci_sched_acl(struct hci_dev *hdev)
+static inline struct hci_chan *hci_chan_sent(struct hci_dev *hdev, __u8 type,
+                                               int *quote)
+{
+       struct hci_conn_hash *h = &hdev->conn_hash;
+       struct hci_chan *chan = NULL;
+       int num = 0, min = ~0, cur_prio = 0;
+       struct hci_conn *conn;
+       int cnt, q, conn_num = 0;
+
+       BT_DBG("%s", hdev->name);
+
+       list_for_each_entry(conn, &h->list, list) {
+               struct hci_chan_hash *ch;
+               struct hci_chan *tmp;
+
+               if (conn->type != type)
+                       continue;
+
+               if (conn->state != BT_CONNECTED && conn->state != BT_CONFIG)
+                       continue;
+
+               conn_num++;
+
+               ch = &conn->chan_hash;
+
+               list_for_each_entry(tmp, &ch->list, list) {
+                       struct sk_buff *skb;
+
+                       if (skb_queue_empty(&tmp->data_q))
+                               continue;
+
+                       skb = skb_peek(&tmp->data_q);
+                       if (skb->priority < cur_prio)
+                               continue;
+
+                       if (skb->priority > cur_prio) {
+                               num = 0;
+                               min = ~0;
+                               cur_prio = skb->priority;
+                       }
+
+                       num++;
+
+                       if (conn->sent < min) {
+                               min  = conn->sent;
+                               chan = tmp;
+                       }
+               }
+
+               if (hci_conn_num(hdev, type) == conn_num)
+                       break;
+       }
+
+       if (!chan)
+               return NULL;
+
+       switch (chan->conn->type) {
+       case ACL_LINK:
+               cnt = hdev->acl_cnt;
+               break;
+       case SCO_LINK:
+       case ESCO_LINK:
+               cnt = hdev->sco_cnt;
+               break;
+       case LE_LINK:
+               cnt = hdev->le_mtu ? hdev->le_cnt : hdev->acl_cnt;
+               break;
+       default:
+               cnt = 0;
+               BT_ERR("Unknown link type");
+       }
+
+       q = cnt / num;
+       *quote = q ? q : 1;
+       BT_DBG("chan %p quote %d", chan, *quote);
+       return chan;
+}
+
+static void hci_prio_recalculate(struct hci_dev *hdev, __u8 type)
 {
+       struct hci_conn_hash *h = &hdev->conn_hash;
        struct hci_conn *conn;
+       int num = 0;
+
+       BT_DBG("%s", hdev->name);
+
+       list_for_each_entry(conn, &h->list, list) {
+               struct hci_chan_hash *ch;
+               struct hci_chan *chan;
+
+               if (conn->type != type)
+                       continue;
+
+               if (conn->state != BT_CONNECTED && conn->state != BT_CONFIG)
+                       continue;
+
+               num++;
+
+               ch = &conn->chan_hash;
+               list_for_each_entry(chan, &ch->list, list) {
+                       struct sk_buff *skb;
+
+                       if (chan->sent) {
+                               chan->sent = 0;
+                               continue;
+                       }
+
+                       if (skb_queue_empty(&chan->data_q))
+                               continue;
+
+                       skb = skb_peek(&chan->data_q);
+                       if (skb->priority >= HCI_PRIO_MAX - 1)
+                               continue;
+
+                       skb->priority = HCI_PRIO_MAX - 1;
+
+                       BT_DBG("chan %p skb %p promoted to %d", chan, skb,
+                                                               skb->priority);
+               }
+
+               if (hci_conn_num(hdev, type) == num)
+                       break;
+       }
+}
+
+static inline void hci_sched_acl(struct hci_dev *hdev)
+{
+       struct hci_chan *chan;
        struct sk_buff *skb;
        int quote;
+       unsigned int cnt;
 
        BT_DBG("%s", hdev->name);
 
+       if (!hci_conn_num(hdev, ACL_LINK))
+               return;
+
        if (!test_bit(HCI_RAW, &hdev->flags)) {
                /* ACL tx timeout must be longer than maximum
                 * link supervision timeout (40.9 seconds) */
@@ -2138,19 +2268,35 @@ static inline void hci_sched_acl(struct hci_dev *hdev)
                        hci_link_tx_to(hdev, ACL_LINK);
        }
 
-       while (hdev->acl_cnt && (conn = hci_low_sent(hdev, ACL_LINK, &quote))) {
-               while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
-                       BT_DBG("skb %p len %d", skb, skb->len);
+       cnt = hdev->acl_cnt;
 
-                       hci_conn_enter_active_mode(conn, bt_cb(skb)->force_active);
+       while (hdev->acl_cnt &&
+                       (chan = hci_chan_sent(hdev, ACL_LINK, &quote))) {
+               u32 priority = (skb_peek(&chan->data_q))->priority;
+               while (quote-- && (skb = skb_peek(&chan->data_q))) {
+                       BT_DBG("chan %p skb %p len %d priority %u", chan, skb,
+                                       skb->len, skb->priority);
+
+                       /* Stop if priority has changed */
+                       if (skb->priority < priority)
+                               break;
+
+                       skb = skb_dequeue(&chan->data_q);
+
+                       hci_conn_enter_active_mode(chan->conn,
+                                               bt_cb(skb)->force_active);
 
                        hci_send_frame(skb);
                        hdev->acl_last_tx = jiffies;
 
                        hdev->acl_cnt--;
-                       conn->sent++;
+                       chan->sent++;
+                       chan->conn->sent++;
                }
        }
+
+       if (cnt != hdev->acl_cnt)
+               hci_prio_recalculate(hdev, ACL_LINK);
 }
 
 /* Schedule SCO */
@@ -2162,6 +2308,9 @@ static inline void hci_sched_sco(struct hci_dev *hdev)
 
        BT_DBG("%s", hdev->name);
 
+       if (!hci_conn_num(hdev, SCO_LINK))
+               return;
+
        while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, &quote))) {
                while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
                        BT_DBG("skb %p len %d", skb, skb->len);
@@ -2182,6 +2331,9 @@ static inline void hci_sched_esco(struct hci_dev *hdev)
 
        BT_DBG("%s", hdev->name);
 
+       if (!hci_conn_num(hdev, ESCO_LINK))
+               return;
+
        while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK, &quote))) {
                while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
                        BT_DBG("skb %p len %d", skb, skb->len);
@@ -2196,12 +2348,15 @@ static inline void hci_sched_esco(struct hci_dev *hdev)
 
 static inline void hci_sched_le(struct hci_dev *hdev)
 {
-       struct hci_conn *conn;
+       struct hci_chan *chan;
        struct sk_buff *skb;
-       int quote, cnt;
+       int quote, cnt, tmp;
 
        BT_DBG("%s", hdev->name);
 
+       if (!hci_conn_num(hdev, LE_LINK))
+               return;
+
        if (!test_bit(HCI_RAW, &hdev->flags)) {
                /* LE tx timeout must be longer than maximum
                 * link supervision timeout (40.9 seconds) */
@@ -2211,21 +2366,35 @@ static inline void hci_sched_le(struct hci_dev *hdev)
        }
 
        cnt = hdev->le_pkts ? hdev->le_cnt : hdev->acl_cnt;
-       while (cnt && (conn = hci_low_sent(hdev, LE_LINK, &quote))) {
-               while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
-                       BT_DBG("skb %p len %d", skb, skb->len);
+       tmp = cnt;
+       while (cnt && (chan = hci_chan_sent(hdev, LE_LINK, &quote))) {
+               u32 priority = (skb_peek(&chan->data_q))->priority;
+               while (quote-- && (skb = skb_peek(&chan->data_q))) {
+                       BT_DBG("chan %p skb %p len %d priority %u", chan, skb,
+                                       skb->len, skb->priority);
+
+                       /* Stop if priority has changed */
+                       if (skb->priority < priority)
+                               break;
+
+                       skb = skb_dequeue(&chan->data_q);
 
                        hci_send_frame(skb);
                        hdev->le_last_tx = jiffies;
 
                        cnt--;
-                       conn->sent++;
+                       chan->sent++;
+                       chan->conn->sent++;
                }
        }
+
        if (hdev->le_pkts)
                hdev->le_cnt = cnt;
        else
                hdev->acl_cnt = cnt;
+
+       if (cnt != tmp)
+               hci_prio_recalculate(hdev, LE_LINK);
 }
 
 static void hci_tx_task(unsigned long arg)
@@ -2418,3 +2587,34 @@ static void hci_cmd_task(unsigned long arg)
                }
        }
 }
+
+int hci_do_inquiry(struct hci_dev *hdev, u8 length)
+{
+       /* General inquiry access code (GIAC) */
+       u8 lap[3] = { 0x33, 0x8b, 0x9e };
+       struct hci_cp_inquiry cp;
+
+       BT_DBG("%s", hdev->name);
+
+       if (test_bit(HCI_INQUIRY, &hdev->flags))
+               return -EINPROGRESS;
+
+       memset(&cp, 0, sizeof(cp));
+       memcpy(&cp.lap, lap, sizeof(cp.lap));
+       cp.length  = length;
+
+       return hci_send_cmd(hdev, HCI_OP_INQUIRY, sizeof(cp), &cp);
+}
+
+int hci_cancel_inquiry(struct hci_dev *hdev)
+{
+       BT_DBG("%s", hdev->name);
+
+       if (!test_bit(HCI_INQUIRY, &hdev->flags))
+               return -EPERM;
+
+       return hci_send_cmd(hdev, HCI_OP_INQUIRY_CANCEL, 0, NULL);
+}
+
+module_param(enable_hs, bool, 0644);
+MODULE_PARM_DESC(enable_hs, "Enable High Speed");