1 files changed, 325 insertions, 26 deletions

diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 0383635f91fb..9f5c5f244502 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -24,7 +24,6 @@
 
 /* Bluetooth HCI event handling. */
 
-#include <linux/export.h>
 #include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -203,6 +202,11 @@ static void hci_cc_reset(struct hci_dev *hdev, struct sk_buff *skb)
                              BIT(HCI_PERIODIC_INQ));
 
         hdev->discovery.state = DISCOVERY_STOPPED;
+        hdev->inq_tx_power = HCI_TX_POWER_INVALID;
+        hdev->adv_tx_power = HCI_TX_POWER_INVALID;
+
+        memset(hdev->adv_data, 0, sizeof(hdev->adv_data));
+        hdev->adv_data_len = 0;
 }
 
 static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb)
@@ -225,6 +229,9 @@ static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb)
 
         hci_dev_unlock(hdev);
 
+        if (!status && !test_bit(HCI_INIT, &hdev->flags))
+                hci_update_ad(hdev);
+
         hci_req_complete(hdev, HCI_OP_WRITE_LOCAL_NAME, status);
 }
 
@@ -440,7 +447,7 @@ static void hci_cc_host_buffer_size(struct hci_dev *hdev, struct sk_buff *skb)
 static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
-        void *sent;
+        struct hci_cp_write_ssp_mode *sent;
 
         BT_DBG("%s status 0x%2.2x", hdev->name, status);
 
@@ -448,10 +455,17 @@ static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb)
         if (!sent)
                 return;
 
+        if (!status) {
+                if (sent->mode)
+                        hdev->host_features[0] |= LMP_HOST_SSP;
+                else
+                        hdev->host_features[0] &= ~LMP_HOST_SSP;
+        }
+
         if (test_bit(HCI_MGMT, &hdev->dev_flags))
-                mgmt_ssp_enable_complete(hdev, *((u8 *) sent), status);
+                mgmt_ssp_enable_complete(hdev, sent->mode, status);
         else if (!status) {
-                if (*((u8 *) sent))
+                if (sent->mode)
                         set_bit(HCI_SSP_ENABLED, &hdev->dev_flags);
                 else
                         clear_bit(HCI_SSP_ENABLED, &hdev->dev_flags);
@@ -460,10 +474,10 @@ static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb)
 
 static u8 hci_get_inquiry_mode(struct hci_dev *hdev)
 {
-        if (hdev->features[6] & LMP_EXT_INQ)
+        if (lmp_ext_inq_capable(hdev))
                 return 2;
 
-        if (hdev->features[3] & LMP_RSSI_INQ)
+        if (lmp_inq_rssi_capable(hdev))
                 return 1;
 
         if (hdev->manufacturer == 11 && hdev->hci_rev == 0x00 &&
@@ -507,28 +521,30 @@ static void hci_setup_event_mask(struct hci_dev *hdev)
         if (hdev->hci_ver < BLUETOOTH_VER_1_2)
                 return;
 
-        events[4] |= 0x01; /* Flow Specification Complete */
-        events[4] |= 0x02; /* Inquiry Result with RSSI */
-        events[4] |= 0x04; /* Read Remote Extended Features Complete */
-        events[5] |= 0x08; /* Synchronous Connection Complete */
-        events[5] |= 0x10; /* Synchronous Connection Changed */
+        if (lmp_bredr_capable(hdev)) {
+                events[4] |= 0x01; /* Flow Specification Complete */
+                events[4] |= 0x02; /* Inquiry Result with RSSI */
+                events[4] |= 0x04; /* Read Remote Extended Features Complete */
+                events[5] |= 0x08; /* Synchronous Connection Complete */
+                events[5] |= 0x10; /* Synchronous Connection Changed */
+        }
 
-        if (hdev->features[3] & LMP_RSSI_INQ)
+        if (lmp_inq_rssi_capable(hdev))
                 events[4] |= 0x02; /* Inquiry Result with RSSI */
 
         if (lmp_sniffsubr_capable(hdev))
                 events[5] |= 0x20; /* Sniff Subrating */
 
-        if (hdev->features[5] & LMP_PAUSE_ENC)
+        if (lmp_pause_enc_capable(hdev))
                 events[5] |= 0x80; /* Encryption Key Refresh Complete */
 
-        if (hdev->features[6] & LMP_EXT_INQ)
+        if (lmp_ext_inq_capable(hdev))
                 events[5] |= 0x40; /* Extended Inquiry Result */
 
         if (lmp_no_flush_capable(hdev))
                 events[7] |= 0x01; /* Enhanced Flush Complete */
 
-        if (hdev->features[7] & LMP_LSTO)
+        if (lmp_lsto_capable(hdev))
                 events[6] |= 0x80; /* Link Supervision Timeout Changed */
 
         if (lmp_ssp_capable(hdev)) {
@@ -548,6 +564,53 @@ static void hci_setup_event_mask(struct hci_dev *hdev)
                 events[7] |= 0x20;      /* LE Meta-Event */
 
         hci_send_cmd(hdev, HCI_OP_SET_EVENT_MASK, sizeof(events), events);
+
+        if (lmp_le_capable(hdev)) {
+                memset(events, 0, sizeof(events));
+                events[0] = 0x1f;
+                hci_send_cmd(hdev, HCI_OP_LE_SET_EVENT_MASK,
+                             sizeof(events), events);
+        }
+}
+
+static void bredr_setup(struct hci_dev *hdev)
+{
+        struct hci_cp_delete_stored_link_key cp;
+        __le16 param;
+        __u8 flt_type;
+
+        /* Read Buffer Size (ACL mtu, max pkt, etc.) */
+        hci_send_cmd(hdev, HCI_OP_READ_BUFFER_SIZE, 0, NULL);
+
+        /* Read Class of Device */
+        hci_send_cmd(hdev, HCI_OP_READ_CLASS_OF_DEV, 0, NULL);
+
+        /* Read Local Name */
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_NAME, 0, NULL);
+
+        /* Read Voice Setting */
+        hci_send_cmd(hdev, HCI_OP_READ_VOICE_SETTING, 0, NULL);
+
+        /* Clear Event Filters */
+        flt_type = HCI_FLT_CLEAR_ALL;
+        hci_send_cmd(hdev, HCI_OP_SET_EVENT_FLT, 1, &flt_type);
+
+        /* Connection accept timeout ~20 secs */
+        param = __constant_cpu_to_le16(0x7d00);
+        hci_send_cmd(hdev, HCI_OP_WRITE_CA_TIMEOUT, 2, &param);
+
+        bacpy(&cp.bdaddr, BDADDR_ANY);
+        cp.delete_all = 1;
+        hci_send_cmd(hdev, HCI_OP_DELETE_STORED_LINK_KEY, sizeof(cp), &cp);
+}
+
+static void le_setup(struct hci_dev *hdev)
+{
+        /* Read LE Buffer Size */
+        hci_send_cmd(hdev, HCI_OP_LE_READ_BUFFER_SIZE, 0, NULL);
+
+        /* Read LE Advertising Channel TX Power */
+        hci_send_cmd(hdev, HCI_OP_LE_READ_ADV_TX_POWER, 0, NULL);
 }
 
 static void hci_setup(struct hci_dev *hdev)
@@ -555,6 +618,15 @@ static void hci_setup(struct hci_dev *hdev)
         if (hdev->dev_type != HCI_BREDR)
                 return;
 
+        /* Read BD Address */
+        hci_send_cmd(hdev, HCI_OP_READ_BD_ADDR, 0, NULL);
+
+        if (lmp_bredr_capable(hdev))
+                bredr_setup(hdev);
+
+        if (lmp_le_capable(hdev))
+                le_setup(hdev);
+
         hci_setup_event_mask(hdev);
 
         if (hdev->hci_ver > BLUETOOTH_VER_1_1)
@@ -575,13 +647,13 @@ static void hci_setup(struct hci_dev *hdev)
                 }
         }
 
-        if (hdev->features[3] & LMP_RSSI_INQ)
+        if (lmp_inq_rssi_capable(hdev))
                 hci_setup_inquiry_mode(hdev);
 
-        if (hdev->features[7] & LMP_INQ_TX_PWR)
+        if (lmp_inq_tx_pwr_capable(hdev))
                 hci_send_cmd(hdev, HCI_OP_READ_INQ_RSP_TX_POWER, 0, NULL);
 
-        if (hdev->features[7] & LMP_EXTFEATURES) {
+        if (lmp_ext_feat_capable(hdev)) {
                 struct hci_cp_read_local_ext_features cp;
 
                 cp.page = 0x01;
@@ -628,11 +700,11 @@ static void hci_setup_link_policy(struct hci_dev *hdev)
 
         if (lmp_rswitch_capable(hdev))
                 link_policy |= HCI_LP_RSWITCH;
-        if (hdev->features[0] & LMP_HOLD)
+        if (lmp_hold_capable(hdev))
                 link_policy |= HCI_LP_HOLD;
         if (lmp_sniff_capable(hdev))
                 link_policy |= HCI_LP_SNIFF;
-        if (hdev->features[1] & LMP_PARK)
+        if (lmp_park_capable(hdev))
                 link_policy |= HCI_LP_PARK;
 
         cp.policy = cpu_to_le16(link_policy);
@@ -722,10 +794,10 @@ static void hci_set_le_support(struct hci_dev *hdev)
 
         if (test_bit(HCI_LE_ENABLED, &hdev->dev_flags)) {
                 cp.le = 1;
-                cp.simul = !!(hdev->features[6] & LMP_SIMUL_LE_BR);
+                cp.simul = !!lmp_le_br_capable(hdev);
         }
 
-        if (cp.le != !!(hdev->host_features[0] & LMP_HOST_LE))
+        if (cp.le != !!lmp_host_le_capable(hdev))
                 hci_send_cmd(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, sizeof(cp),
                              &cp);
 }
@@ -1018,6 +1090,31 @@ static void hci_cc_le_read_buffer_size(struct hci_dev *hdev,
         hci_req_complete(hdev, HCI_OP_LE_READ_BUFFER_SIZE, rp->status);
 }
 
+static void hci_cc_le_read_adv_tx_power(struct hci_dev *hdev,
+                                        struct sk_buff *skb)
+{
+        struct hci_rp_le_read_adv_tx_power *rp = (void *) skb->data;
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+        if (!rp->status) {
+                hdev->adv_tx_power = rp->tx_power;
+                if (!test_bit(HCI_INIT, &hdev->flags))
+                        hci_update_ad(hdev);
+        }
+
+        hci_req_complete(hdev, HCI_OP_LE_READ_ADV_TX_POWER, rp->status);
+}
+
+static void hci_cc_le_set_event_mask(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        __u8 status = *((__u8 *) skb->data);
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+        hci_req_complete(hdev, HCI_OP_LE_SET_EVENT_MASK, status);
+}
+
 static void hci_cc_user_confirm_reply(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_rp_user_confirm_reply *rp = (void *) skb->data;
@@ -1093,6 +1190,33 @@ static void hci_cc_read_local_oob_data_reply(struct hci_dev *hdev,
         hci_dev_unlock(hdev);
 }
 
+static void hci_cc_le_set_adv_enable(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        __u8 *sent, status = *((__u8 *) skb->data);
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+        sent = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADV_ENABLE);
+        if (!sent)
+                return;
+
+        hci_dev_lock(hdev);
+
+        if (!status) {
+                if (*sent)
+                        set_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags);
+                else
+                        clear_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags);
+        }
+
+        hci_dev_unlock(hdev);
+
+        if (!test_bit(HCI_INIT, &hdev->flags))
+                hci_update_ad(hdev);
+
+        hci_req_complete(hdev, HCI_OP_LE_SET_ADV_ENABLE, status);
+}
+
 static void hci_cc_le_set_scan_param(struct hci_dev *hdev, struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
@@ -1207,6 +1331,11 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
                         hdev->host_features[0] |= LMP_HOST_LE;
                 else
                         hdev->host_features[0] &= ~LMP_HOST_LE;
+
+                if (sent->simul)
+                        hdev->host_features[0] |= LMP_HOST_LE_BREDR;
+                else
+                        hdev->host_features[0] &= ~LMP_HOST_LE_BREDR;
         }
 
         if (test_bit(HCI_MGMT, &hdev->dev_flags) &&
@@ -1718,14 +1847,23 @@ static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
 
         BT_DBG("%s status 0x%2.2x", hdev->name, status);
 
-        if (status)
-                return;
-
         cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_PHY_LINK);
         if (!cp)
                 return;
 
-        amp_write_remote_assoc(hdev, cp->phy_handle);
+        hci_dev_lock(hdev);
+
+        if (status) {
+                struct hci_conn *hcon;
+
+                hcon = hci_conn_hash_lookup_handle(hdev, cp->phy_handle);
+                if (hcon)
+                        hci_conn_del(hcon);
+        } else {
+                amp_write_remote_assoc(hdev, cp->phy_handle);
+        }
+
+        hci_dev_unlock(hdev);
 }
 
 static void hci_cs_accept_phylink(struct hci_dev *hdev, u8 status)
@@ -1744,6 +1882,11 @@ static void hci_cs_accept_phylink(struct hci_dev *hdev, u8 status)
         amp_write_remote_assoc(hdev, cp->phy_handle);
 }
 
+static void hci_cs_create_logical_link(struct hci_dev *hdev, u8 status)
+{
+        BT_DBG("%s status 0x%2.2x", hdev->name, status);
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
@@ -2441,6 +2584,14 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cc_le_read_buffer_size(hdev, skb);
                 break;
 
+        case HCI_OP_LE_READ_ADV_TX_POWER:
+                hci_cc_le_read_adv_tx_power(hdev, skb);
+                break;
+
+        case HCI_OP_LE_SET_EVENT_MASK:
+                hci_cc_le_set_event_mask(hdev, skb);
+                break;
+
         case HCI_OP_USER_CONFIRM_REPLY:
                 hci_cc_user_confirm_reply(hdev, skb);
                 break;
@@ -2461,6 +2612,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cc_le_set_scan_param(hdev, skb);
                 break;
 
+        case HCI_OP_LE_SET_ADV_ENABLE:
+                hci_cc_le_set_adv_enable(hdev, skb);
+                break;
+
         case HCI_OP_LE_SET_SCAN_ENABLE:
                 hci_cc_le_set_scan_enable(hdev, skb);
                 break;
@@ -2570,6 +2725,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cs_accept_phylink(hdev, ev->status);
                 break;
 
+        case HCI_OP_CREATE_LOGICAL_LINK:
+                hci_cs_create_logical_link(hdev, ev->status);
+                break;
+
         default:
                 BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
                 break;
@@ -3544,6 +3703,130 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
+static void hci_phy_link_complete_evt(struct hci_dev *hdev,
+                                      struct sk_buff *skb)
+{
+        struct hci_ev_phy_link_complete *ev = (void *) skb->data;
+        struct hci_conn *hcon, *bredr_hcon;
+
+        BT_DBG("%s handle 0x%2.2x status 0x%2.2x", hdev->name, ev->phy_handle,
+               ev->status);
+
+        hci_dev_lock(hdev);
+
+        hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle);
+        if (!hcon) {
+                hci_dev_unlock(hdev);
+                return;
+        }
+
+        if (ev->status) {
+                hci_conn_del(hcon);
+                hci_dev_unlock(hdev);
+                return;
+        }
+
+        bredr_hcon = hcon->amp_mgr->l2cap_conn->hcon;
+
+        hcon->state = BT_CONNECTED;
+        bacpy(&hcon->dst, &bredr_hcon->dst);
+
+        hci_conn_hold(hcon);
+        hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
+        hci_conn_put(hcon);
+
+        hci_conn_hold_device(hcon);
+        hci_conn_add_sysfs(hcon);
+
+        amp_physical_cfm(bredr_hcon, hcon);
+
+        hci_dev_unlock(hdev);
+}
+
+static void hci_loglink_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_ev_logical_link_complete *ev = (void *) skb->data;
+        struct hci_conn *hcon;
+        struct hci_chan *hchan;
+        struct amp_mgr *mgr;
+
+        BT_DBG("%s log_handle 0x%4.4x phy_handle 0x%2.2x status 0x%2.2x",
+               hdev->name, le16_to_cpu(ev->handle), ev->phy_handle,
+               ev->status);
+
+        hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle);
+        if (!hcon)
+                return;
+
+        /* Create AMP hchan */
+        hchan = hci_chan_create(hcon);
+        if (!hchan)
+                return;
+
+        hchan->handle = le16_to_cpu(ev->handle);
+
+        BT_DBG("hcon %p mgr %p hchan %p", hcon, hcon->amp_mgr, hchan);
+
+        mgr = hcon->amp_mgr;
+        if (mgr && mgr->bredr_chan) {
+                struct l2cap_chan *bredr_chan = mgr->bredr_chan;
+
+                l2cap_chan_lock(bredr_chan);
+
+                bredr_chan->conn->mtu = hdev->block_mtu;
+                l2cap_logical_cfm(bredr_chan, hchan, 0);
+                hci_conn_hold(hcon);
+
+                l2cap_chan_unlock(bredr_chan);
+        }
+}
+
+static void hci_disconn_loglink_complete_evt(struct hci_dev *hdev,
+                                             struct sk_buff *skb)
+{
+        struct hci_ev_disconn_logical_link_complete *ev = (void *) skb->data;
+        struct hci_chan *hchan;
+
+        BT_DBG("%s log handle 0x%4.4x status 0x%2.2x", hdev->name,
+               le16_to_cpu(ev->handle), ev->status);
+
+        if (ev->status)
+                return;
+
+        hci_dev_lock(hdev);
+
+        hchan = hci_chan_lookup_handle(hdev, le16_to_cpu(ev->handle));
+        if (!hchan)
+                goto unlock;
+
+        amp_destroy_logical_link(hchan, ev->reason);
+
+unlock:
+        hci_dev_unlock(hdev);
+}
+
+static void hci_disconn_phylink_complete_evt(struct hci_dev *hdev,
+                                             struct sk_buff *skb)
+{
+        struct hci_ev_disconn_phy_link_complete *ev = (void *) skb->data;
+        struct hci_conn *hcon;
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, ev->status);
+
+        if (ev->status)
+                return;
+
+        hci_dev_lock(hdev);
+
+        hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle);
+        if (hcon) {
+                hcon->state = BT_CLOSED;
+                hci_conn_del(hcon);
+        }
+
+        hci_dev_unlock(hdev);
+}
+
 static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_le_conn_complete *ev = (void *) skb->data;
@@ -3871,6 +4154,22 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_remote_oob_data_request_evt(hdev, skb);
                 break;
 
+        case HCI_EV_PHY_LINK_COMPLETE:
+                hci_phy_link_complete_evt(hdev, skb);
+                break;
+
+        case HCI_EV_LOGICAL_LINK_COMPLETE:
+                hci_loglink_complete_evt(hdev, skb);
+                break;
+
+        case HCI_EV_DISCONN_LOGICAL_LINK_COMPLETE:
+                hci_disconn_loglink_complete_evt(hdev, skb);
+                break;
+
+        case HCI_EV_DISCONN_PHY_LINK_COMPLETE:
+                hci_disconn_phylink_complete_evt(hdev, skb);
+                break;
+
         case HCI_EV_NUM_COMP_BLOCKS:
                 hci_num_comp_blocks_evt(hdev, skb);
                 break;